diff --git a/Makefile b/Makefile index cea681b70b..6a1e7351d0 100644 --- a/Makefile +++ b/Makefile @@ -210,7 +210,7 @@ MOD_INC = src/modules/interface MOD_SRC = src/modules/src bindings_python build/cffirmware.py: bindings/setup.py $(MOD_SRC)/*.c - swig -python -I$(MOD_INC) -Isrc/hal/interface -Isrc/utils/interface -I$(MOD_INC)/controller -Isrc/platform/interface -I$(MOD_INC)/outlierfilter -I$(MOD_INC)/kalman_core -o build/cffirmware_wrap.c bindings/cffirmware.i + swig -python -I$(MOD_INC) -Isrc/hal/interface -Isrc/utils/interface -Isrc/utils/interface/lighthouse -I$(MOD_INC)/controller -Isrc/platform/interface -I$(MOD_INC)/outlierfilter -I$(MOD_INC)/kalman_core -o build/cffirmware_wrap.c bindings/cffirmware.i $(PYTHON) bindings/setup.py build_ext --inplace cp cffirmware_setup.py build/setup.py diff --git a/bindings/cffirmware.i b/bindings/cffirmware.i index 289706ba69..aa6e706782 100755 --- a/bindings/cffirmware.i +++ b/bindings/cffirmware.i @@ -24,6 +24,14 @@ #include "outlierFilterTdoa.h" #include "kalman_core.h" #include "mm_tdoa.h" +#include "mm_sweep_angles.h" +#include "outlierFilterLighthouse.h" +#include "mm_yaw_error.h" +#include "lighthouse_types.h" +#include "lighthouse_geometry.h" +#include "ootx_decoder.h" +#include "crc32.h" +#include "lighthouse_calibration.h" %} %include "math3d.h" @@ -40,9 +48,71 @@ %include "outlierFilterTdoa.h" %include "kalman_core.h" %include "mm_tdoa.h" +%include "mm_sweep_angles.h" +%include "outlierFilterLighthouse.h" +%include "mm_yaw_error.h" +%include "lighthouse_types.h" +%include "lighthouse_geometry.h" +%include "ootx_decoder.h" +%include "crc32.h" +%include "lighthouse_calibration.h" + %inline %{ + +float get_state(kalmanCoreData_t *data, int i) +{ + return data->S[i]; +} + +float get_mat_index(kalmanCoreData_t *data, int i, int j) +{ + return data->R[i][j]; +} + +float set_calibration_model(sweepAngleMeasurement_t *sweep, lighthouseCalibrationSweep_t *calib_in) +{ + sweep->calibrationMeasurementModel = lighthouseCalibrationMeasurementModelLh2; + sweep->calib = calib_in; + +} + + +void print_sweep_angle(sweepAngleMeasurement_t *sweep) +{ + //print all variables of struct + printf("uint32_t timestamp: %d\n", sweep->timestamp); + printf("sensorPos: %f, %f, %f\n", sweep->sensorPos.x, sweep->sensorPos.y, sweep->sensorPos.z); + printf('rotorPos: %f, %f, %f\n', sweep->rotorPos.x, sweep->rotorPos.y, sweep->rotorPos.z); + printf('rotorRot: %f, %f, %f\n', sweep->rotorRot.i11, sweep->rotorRot.i12, sweep->rotorRot.i13); + printf('rotorRot: %f, %f, %f\n', sweep->rotorRot.i21, sweep->rotorRot.i22, sweep->rotorRot.i23); + printf('rotorRot: %f, %f, %f\n', sweep->rotorRot.i31, sweep->rotorRot.i32, sweep->rotorRot.i33); + printf('rotorRotInv: %f, %f, %f\n', sweep->rotorRotInv.i11, sweep->rotorRotInv.i12, sweep->rotorRotInv.i13); + printf('rotorRotInv: %f, %f, %f\n', sweep->rotorRotInv.i21, sweep->rotorRotInv.i22, sweep->rotorRotInv.i23); + printf('rotorRotInv: %f, %f, %f\n', sweep->rotorRotInv.i31, sweep->rotorRotInv.i32, sweep->rotorRotInv.i33); + printf("sensorID: %d\n", sweep->sensorId); + printf("baseStationID: %d\n", sweep->baseStationId); + printf("sweepID: %d\n", sweep->sweepId); + printf("t: %f\n", sweep->t); + printf("measuredSweepAngle: %f\n", sweep->measuredSweepAngle); + printf("stdDev: %f\n", sweep->stdDev); + printf("calib: %f, %f, %f, %f, %f\n", sweep->calib->phase, sweep->calib->tilt, sweep->calib->curve, sweep->calib->gibmag, sweep->calib->gibphase); + printf("calibrationMeasurementModel: %f\n", sweep->calibrationMeasurementModel); +} + + + + +void set_sweep(lighthouseCalibration_t *calib, lighthouseCalibrationSweep_t sweep, int i) +{ + calib->sweep[i] = sweep; +} +void print_sweeps(lighthouseCalibration_t calib) +{ + printf("Sweep 0: phase: %f, tilt: %f, curve: %f, gibmag: %f, gibphase: %f\n", calib.sweep[0].phase, calib.sweep[0].tilt, calib.sweep[0].curve, calib.sweep[0].gibmag, calib.sweep[0].gibphase); + printf("Sweep 1: phase: %f, tilt: %f, curve: %f, gibmag: %f, gibphase: %f\n", calib.sweep[1].phase, calib.sweep[1].tilt, calib.sweep[1].curve, calib.sweep[1].gibmag, calib.sweep[1].gibphase); +} struct poly4d* piecewise_get(struct piecewise_traj *pp, int i) { return &pp->pieces[i]; diff --git a/bindings/setup.py b/bindings/setup.py index a543e31c62..7a5890bcea 100644 --- a/bindings/setup.py +++ b/bindings/setup.py @@ -50,6 +50,13 @@ "src/modules/src/kalman_core/kalman_core.c", "src/modules/src/kalman_core/mm_tdoa.c", "src/modules/src/outlierfilter/outlierFilterTdoa.c", + "src/modules/src/kalman_core/mm_sweep_angles.c", + "src/modules/src/outlierfilter/outlierFilterLighthouse.c", + "src/modules/src/kalman_core/mm_yaw_error.c", + "src/utils/src/lighthouse/lighthouse_calibration.c", + "src/utils/src/lighthouse/ootx_decoder.c", + "src/utils/src/lighthouse/lighthouse_geometry.c", + "src/utils/src/crc32.c", ] cffirmware = Extension( diff --git a/bindings/util/estimator_kalman_emulator.py b/bindings/util/estimator_kalman_emulator.py index 21ba5cf767..a157485bba 100644 --- a/bindings/util/estimator_kalman_emulator.py +++ b/bindings/util/estimator_kalman_emulator.py @@ -1,10 +1,7 @@ +from __future__ import annotations import math import cffirmware - - - - class EstimatorKalmanEmulator: """ This class emulates the behavior of estimator_kalman.c and is used as a helper to enable testing of the kalman @@ -18,14 +15,18 @@ class EstimatorKalmanEmulator: how they are connected. """ - def __init__(self, anchor_positions) -> None: + def __init__(self, anchor_positions=None, basestation_poses=None, basestation_calibration=None) -> None: self.anchor_positions = anchor_positions + self.basestation_poses = basestation_poses + self.basestation_calibration = basestation_calibration self.accSubSampler = cffirmware.Axis3fSubSampler_t() self.gyroSubSampler = cffirmware.Axis3fSubSampler_t() self.coreData = cffirmware.kalmanCoreData_t() - self.outlierFilterState = cffirmware.OutlierFilterTdoaState_t() + self.outlierFilterStateTdoa = cffirmware.OutlierFilterTdoaState_t() + self.outlierFilterStateLH = cffirmware.OutlierFilterLhState_t() self.TDOA_ENGINE_MEASUREMENT_NOISE_STD = 0.30 + self.LH_ENGINE_MEASUREMENT_NOISE_STD = 0.001 self.PREDICT_RATE = 100 self.PREDICT_STEP_MS = 1000 / self.PREDICT_RATE @@ -86,7 +87,8 @@ def _lazy_init(self, sample_time_ms): self.coreParams = cffirmware.kalmanCoreParams_t() cffirmware.kalmanCoreDefaultParams(self.coreParams) - cffirmware.outlierFilterTdoaReset(self.outlierFilterState) + cffirmware.outlierFilterTdoaReset(self.outlierFilterStateTdoa) + cffirmware.outlierFilterLighthouseReset(self.outlierFilterStateLH, self.now_ms) cffirmware.kalmanCoreInit(self.coreData, self.coreParams, self.now_ms) self._is_initialized = True @@ -103,6 +105,17 @@ def _update_queued_measurements(self, now_ms: int, sensor_samples): return def _update_with_sample(self, sample, now_ms): + position = [0.0, 0.0, 0.0] + position[0] = cffirmware.get_state(self.coreData, 0) + position[1] = cffirmware.get_state(self.coreData, 1) + position[2] = cffirmware.get_state(self.coreData, 2) + + rotation_matrix = [[0.0, 0.0, 0.0],[0.0, 0.0, 0.0],[0.0, 0.0, 0.0]] + + for i in range(0, 3): + for j in range(0, 3): + rotation_matrix[i][j] = cffirmware.get_mat_index(self.coreData, i,j) + if sample[0] == 'estTDOA': tdoa_data = sample[1] tdoa = cffirmware.tdoaMeasurement_t() @@ -114,7 +127,81 @@ def _update_with_sample(self, sample, now_ms): tdoa.distanceDiff = float(tdoa_data['distanceDiff']) tdoa.stdDev = self.TDOA_ENGINE_MEASUREMENT_NOISE_STD - cffirmware.kalmanCoreUpdateWithTdoa(self.coreData, tdoa, now_ms, self.outlierFilterState) + cffirmware.kalmanCoreUpdateWithTdoa(self.coreData, tdoa, now_ms, self.outlierFilterStateTdoa) + + if sample[0] == 'estYawError': + yaw_error_data = sample[1] + yaw_error = cffirmware.yawErrorMeasurement_t() + yaw_error.yawError = float(yaw_error_data['yawError']) + yaw_error.stdDev = 0.01 + + cffirmware.kalmanCoreUpdateWithYawError(self.coreData, yaw_error) + + + if sample[0] == 'estSweepAngle': + sweep_data = sample[1] + sweep = cffirmware.sweepAngleMeasurement_t() + + sweep.sensorId = int(sweep_data['sensorId']) + sweep.baseStationId = int(sweep_data['baseStationId']) + sweep.sweepId = int(sweep_data['sweepId']) + sweep.t = float(sweep_data['t']) + sweep.measuredSweepAngle = float(sweep_data['sweepAngle']) + sweep.stdDev = self.LH_ENGINE_MEASUREMENT_NOISE_STD + cffirmware.set_calibration_model(sweep, self.basestation_calibration[sweep.baseStationId][sweep.sweepId]) + + sensor_pos_w = 0.015/2.0 + sensor_pos_l = 0.030/2.0 + sensor_position = {} + sensor_position[0] = [-sensor_pos_w, sensor_pos_l, 0.0] + sensor_position[1] = [-sensor_pos_w, -sensor_pos_l, 0.0] + sensor_position[2] = [sensor_pos_w, sensor_pos_l, 0.0] + sensor_position[3] = [sensor_pos_w, -sensor_pos_l, 0.0] + + sensorPos = cffirmware.vec3_s() + sensorPos.x = sensor_position[int(sweep.sensorId)][0] + sensorPos.y = sensor_position[int(sweep.sensorId)][1] + sensorPos.z = sensor_position[int(sweep.sensorId)][2] + + rotorPos = cffirmware.vec3_s() + rotorPos.x = self.basestation_poses[sweep.baseStationId]['origin'].x + rotorPos.y = self.basestation_poses[sweep.baseStationId]['origin'].y + rotorPos.z = self.basestation_poses[sweep.baseStationId]['origin'].z + + rotorRot = cffirmware.mat3_s() + rotorRot.i11 = self.basestation_poses[sweep.baseStationId]['mat'].i11 + rotorRot.i12 = self.basestation_poses[sweep.baseStationId]['mat'].i12 + rotorRot.i13 = self.basestation_poses[sweep.baseStationId]['mat'].i13 + rotorRot.i21 = self.basestation_poses[sweep.baseStationId]['mat'].i21 + rotorRot.i22 = self.basestation_poses[sweep.baseStationId]['mat'].i22 + rotorRot.i23 = self.basestation_poses[sweep.baseStationId]['mat'].i23 + rotorRot.i31 = self.basestation_poses[sweep.baseStationId]['mat'].i31 + rotorRot.i32 = self.basestation_poses[sweep.baseStationId]['mat'].i32 + rotorRot.i33 = self.basestation_poses[sweep.baseStationId]['mat'].i33 + + # transpose of the rotation matrix + rotorRotInv = cffirmware.mat3_s() + rotorRotInv.i11 = self.basestation_poses[sweep.baseStationId]['mat'].i11 + rotorRotInv.i12 = self.basestation_poses[sweep.baseStationId]['mat'].i21 + rotorRotInv.i13 = self.basestation_poses[sweep.baseStationId]['mat'].i31 + rotorRotInv.i21 = self.basestation_poses[sweep.baseStationId]['mat'].i12 + rotorRotInv.i22 = self.basestation_poses[sweep.baseStationId]['mat'].i22 + rotorRotInv.i23 = self.basestation_poses[sweep.baseStationId]['mat'].i32 + rotorRotInv.i31 = self.basestation_poses[sweep.baseStationId]['mat'].i13 + rotorRotInv.i32 = self.basestation_poses[sweep.baseStationId]['mat'].i23 + rotorRotInv.i33 = self.basestation_poses[sweep.baseStationId]['mat'].i33 + + + + sweep.sensorPos = sensorPos + sweep.rotorPos = rotorPos + sweep.rotorRot = rotorRot + sweep.rotorRotInv = rotorRotInv + + cffirmware.kalmanCoreUpdateWithSweepAngles(self.coreData, sweep, now_ms, self.outlierFilterStateLH) + + + if sample[0] == 'estAcceleration': acc_data = sample[1] diff --git a/bindings/util/lighthouse_utils.py b/bindings/util/lighthouse_utils.py new file mode 100644 index 0000000000..2cfb201035 --- /dev/null +++ b/bindings/util/lighthouse_utils.py @@ -0,0 +1,68 @@ +from __future__ import annotations +import cffirmware +import yaml +import ctypes + + +def read_lh_basestation_pose_calibration(file_name: str) -> dict[int, cffirmware.vec3_s]: + """ + Read basestation calibration and position data from a file exported from the client. + + Args: + file_name (str): The name of the file + + Returns: + dict[int, cffirmware.vec3_s]: A dictionary from anchor id to a 3D-vector + """ + result = {} + + with open(file_name, 'r') as file: + data = yaml.safe_load(file) + data_calib = data['calibs'] + results_calib = {} + for id, vals in data_calib.items(): + + + lhCalibration = {} + + for i in range(0, 2): + data_calib_sweep = vals['sweeps'][i] + lhSweep = cffirmware.lighthouseCalibrationSweep_t() + lhSweep.phase = data_calib_sweep['phase'] + lhSweep.tilt = data_calib_sweep['tilt'] + lhSweep.curve = data_calib_sweep['curve'] + lhSweep.gibmag = data_calib_sweep['gibmag'] + lhSweep.gibphase = data_calib_sweep['gibphase'] + lhSweep.ogeemag = data_calib_sweep['ogeemag'] + lhSweep.ogeephase = data_calib_sweep['ogeephase'] + lhCalibration[i] = lhSweep + + results_calib[id] = lhCalibration + + + data_geo = data['geos'] + results_geo = {} + for id, vals in data_geo.items(): + basestation_geo = {}#cffirmware.baseStationGeometry_t() + origin = cffirmware.vec3_s() + origin.x = vals['origin'][0] + origin.y = vals['origin'][1] + origin.z = vals['origin'][2] + mat = cffirmware.mat3_s() + mat.i11 = vals['rotation'][0][0] + mat.i12 = vals['rotation'][0][1] + mat.z13 = vals['rotation'][0][2] + mat.i21 = vals['rotation'][1][0] + mat.i22 = vals['rotation'][1][1] + mat.i23 = vals['rotation'][1][2] + mat.i31 = vals['rotation'][2][0] + mat.i32 = vals['rotation'][2][1] + mat.i33 = vals['rotation'][2][2] + + #cffirmware.set_origin_mat(basestation_geo, origin, mat1, mat2, mat3) + + + results_geo[id] = {'origin': origin, 'mat': mat} + + + return results_calib, results_geo \ No newline at end of file diff --git a/bindings/util/loco_utils.py b/bindings/util/loco_utils.py index 283348ebea..e66666fb59 100644 --- a/bindings/util/loco_utils.py +++ b/bindings/util/loco_utils.py @@ -1,3 +1,4 @@ +from __future__ import annotations import cffirmware import yaml diff --git a/src/modules/interface/stabilizer_types.h b/src/modules/interface/stabilizer_types.h index d49f852b6a..be744982c2 100644 --- a/src/modules/interface/stabilizer_types.h +++ b/src/modules/interface/stabilizer_types.h @@ -61,6 +61,14 @@ struct vec3_s { float z; }; +struct mat3_s { + uint32_t timestamp; // Timestamp when the data was computed + + float i11, i12, i13; + float i21, i22, i23; + float i31, i32, i33; +}; + typedef struct vec3_s vector_t; typedef struct vec3_s point_t; typedef struct vec3_s velocity_t; @@ -323,10 +331,10 @@ typedef struct { /** Sweep angle measurement */ typedef struct { uint32_t timestamp; - const vec3d* sensorPos; // Sensor position in the CF reference frame - const vec3d* rotorPos; // Pos of rotor origin in global reference frame - const mat3d* rotorRot; // Rotor rotation matrix - const mat3d* rotorRotInv; // Inverted rotor rotation matrix + struct vec3_s sensorPos; // Sensor position in the CF reference frame + struct vec3_s rotorPos; // Pos of rotor origin in global reference frame + struct mat3_s rotorRot; // Rotor rotation matrix + struct mat3_s rotorRotInv; // Inverted rotor rotation matrix uint8_t sensorId; uint8_t baseStationId; uint8_t sweepId; diff --git a/src/modules/src/kalman_core/mm_sweep_angles.c b/src/modules/src/kalman_core/mm_sweep_angles.c index 39e66891d3..1800a71966 100644 --- a/src/modules/src/kalman_core/mm_sweep_angles.c +++ b/src/modules/src/kalman_core/mm_sweep_angles.c @@ -31,7 +31,8 @@ void kalmanCoreUpdateWithSweepAngles(kalmanCoreData_t *this, sweepAngleMeasureme // using the CF roatation matrix vec3d s; arm_matrix_instance_f32 Rcf_ = {3, 3, (float32_t *)this->R}; - arm_matrix_instance_f32 scf_ = {3, 1, (float32_t *)*sweepInfo->sensorPos}; + vec3d scf = {sweepInfo->sensorPos.x, sweepInfo->sensorPos.y, sweepInfo->sensorPos.z}; + arm_matrix_instance_f32 scf_ = {3, 1, scf}; arm_matrix_instance_f32 s_ = {3, 1, s}; mat_mult(&Rcf_, &scf_, &s_); @@ -39,14 +40,17 @@ void kalmanCoreUpdateWithSweepAngles(kalmanCoreData_t *this, sweepAngleMeasureme vec3d pcf = {this->S[KC_STATE_X] + s[0], this->S[KC_STATE_Y] + s[1], this->S[KC_STATE_Z] + s[2]}; // Calculate the difference between the rotor and the sensor on the CF (global reference frame) - const vec3d* pr = sweepInfo->rotorPos; + const vec3d* pr = {&sweepInfo->rotorPos.x, &sweepInfo->rotorPos.y, &sweepInfo->rotorPos.z}; vec3d stmp = {pcf[0] - (*pr)[0], pcf[1] - (*pr)[1], pcf[2] - (*pr)[2]}; arm_matrix_instance_f32 stmp_ = {3, 1, stmp}; // Rotate the difference in position to the rotor reference frame, // using the rotor inverse rotation matrix + mat3d Rr_inv = {{sweepInfo->rotorRotInv.i11, sweepInfo->rotorRotInv.i12, sweepInfo->rotorRotInv.i13}, + {sweepInfo->rotorRotInv.i21, sweepInfo->rotorRotInv.i22, sweepInfo->rotorRotInv.i23}, + {sweepInfo->rotorRotInv.i31, sweepInfo->rotorRotInv.i32, sweepInfo->rotorRotInv.i33}}; vec3d sr; - arm_matrix_instance_f32 Rr_inv_ = {3, 3, (float32_t *)(*sweepInfo->rotorRotInv)}; + arm_matrix_instance_f32 Rr_inv_ = {3, 3, (float32_t *)(*Rr_inv)}; arm_matrix_instance_f32 sr_ = {3, 1, sr}; mat_mult(&Rr_inv_, &stmp_, &sr_); @@ -75,9 +79,12 @@ void kalmanCoreUpdateWithSweepAngles(kalmanCoreData_t *this, sweepAngleMeasureme // gr is in the rotor reference frame, rotate back to the global // reference frame using the rotor rotation matrix + mat3d Rr = {{sweepInfo->rotorRot.i11, sweepInfo->rotorRot.i12, sweepInfo->rotorRot.i13}, + {sweepInfo->rotorRot.i21, sweepInfo->rotorRot.i22, sweepInfo->rotorRot.i23}, + {sweepInfo->rotorRot.i31, sweepInfo->rotorRot.i32, sweepInfo->rotorRot.i33}}; vec3d g; arm_matrix_instance_f32 gr_ = {3, 1, gr}; - arm_matrix_instance_f32 Rr_ = {3, 3, (float32_t *)(*sweepInfo->rotorRot)}; + arm_matrix_instance_f32 Rr_ = {3, 3, (float32_t *)(*Rr)}; arm_matrix_instance_f32 g_ = {3, 1, g}; mat_mult(&Rr_, &gr_, &g_); diff --git a/src/utils/interface/lighthouse/ootx_decoder.h b/src/utils/interface/lighthouse/ootx_decoder.h index 6797fd348f..e59f8ca811 100644 --- a/src/utils/interface/lighthouse/ootx_decoder.h +++ b/src/utils/interface/lighthouse/ootx_decoder.h @@ -34,6 +34,8 @@ #define OOTX_MAX_FRAME_LENGTH 43 +typedef float __fp16; + // Content from https://github.com/nairol/LighthouseRedox/blob/master/docs/Base%20Station.md#base-station-info-block struct ootxDataFrame_s { uint16_t protocolVersion:6; diff --git a/test_python/fixtures/kalman_core/Bindings13 b/test_python/fixtures/kalman_core/Bindings13 new file mode 100644 index 0000000000..7ed1ed2e74 Binary files /dev/null and b/test_python/fixtures/kalman_core/Bindings13 differ diff --git a/test_python/fixtures/kalman_core/basestation_positions_calibration.yaml b/test_python/fixtures/kalman_core/basestation_positions_calibration.yaml new file mode 100644 index 0000000000..e266f477ee --- /dev/null +++ b/test_python/fixtures/kalman_core/basestation_positions_calibration.yaml @@ -0,0 +1,103 @@ +calibs: + 0: + sweeps: + - curve: -0.09857177734375 + gibmag: 0.00728607177734375 + gibphase: 0.234375 + ogeemag: -0.47314453125 + ogeephase: 0.7705078125 + phase: 0.0 + tilt: -0.052093505859375 + - curve: 0.358642578125 + gibmag: 0.0051422119140625 + gibphase: 1.0185546875 + ogeemag: -0.492919921875 + ogeephase: 1.5771484375 + phase: -0.00475311279296875 + tilt: 0.04638671875 + uid: 931912938 + 1: + sweeps: + - curve: 0.1448974609375 + gibmag: -0.005397796630859375 + gibphase: 1.2890625 + ogeemag: 0.0330810546875 + ogeephase: 2.00390625 + phase: 0.0 + tilt: -0.0489501953125 + - curve: 0.374267578125 + gibmag: -0.005641937255859375 + gibphase: 1.7275390625 + ogeemag: 0.11785888671875 + ogeephase: 2.5859375 + phase: -0.00467681884765625 + tilt: 0.047515869140625 + uid: 885173630 + 2: + sweeps: + - curve: 0.296142578125 + gibmag: -0.00672149658203125 + gibphase: 1.7578125 + ogeemag: 0.2105712890625 + ogeephase: 1.744140625 + phase: 0.0 + tilt: -0.04925537109375 + - curve: 0.186279296875 + gibmag: -0.007114410400390625 + gibphase: 1.9921875 + ogeemag: 0.2052001953125 + ogeephase: 2.1171875 + phase: -0.0091094970703125 + tilt: 0.044036865234375 + uid: 202149861 + 3: + sweeps: + - curve: 0.01904296875 + gibmag: 0.006561279296875 + gibphase: 0.89306640625 + ogeemag: -0.607421875 + ogeephase: 1.2509765625 + phase: 0.0 + tilt: -0.05108642578125 + - curve: 0.075927734375 + gibmag: 0.0052642822265625 + gibphase: 1.9384765625 + ogeemag: -0.544921875 + ogeephase: 1.697265625 + phase: -0.000988006591796875 + tilt: 0.0438232421875 + uid: 2360210604 +geos: + 1: + origin: + - 1.9650166034698486 + - 1.2099225521087646 + - 2.809037685394287 + rotation: + - - -0.6719139218330383 + - 0.2409101277589798 + - -0.7003527283668518 + - - -0.4112818241119385 + - -0.907783567905426 + - 0.08231813460588455 + - - -0.6159374117851257 + - 0.3433530628681183 + - 0.7090343832969666 + 2: + origin: + - -2.2806060314178467 + - -1.4312992095947266 + - 1.8253259658813477 + rotation: + - - 0.6308567523956299 + - -0.7237647175788879 + - 0.27961465716362 + - - 0.5605970621109009 + - 0.6743234992027283 + - 0.4806440770626068 + - - -0.5364239811897278 + - -0.14646640419960022 + - 0.8311419486999512 +systemType: 2 +type: lighthouse_system_configuration +version: '1' diff --git a/test_python/fixtures/kalman_core/geometry.yaml b/test_python/fixtures/kalman_core/geometry.yaml new file mode 100644 index 0000000000..6e10fea45d --- /dev/null +++ b/test_python/fixtures/kalman_core/geometry.yaml @@ -0,0 +1,133 @@ +calibs: + 0: + sweeps: + - curve: 0.052215576171875 + gibmag: -0.00391387939453125 + gibphase: 2.087890625 + ogeemag: -0.049285888671875 + ogeephase: 0.43310546875 + phase: 0.0 + tilt: -0.04705810546875 + - curve: 0.12237548828125 + gibmag: -0.00388336181640625 + gibphase: 2.09765625 + ogeemag: -0.03485107421875 + ogeephase: 0.6318359375 + phase: -0.005336761474609375 + tilt: 0.048065185546875 + uid: 3900185618 + 1: + sweeps: + - curve: -0.2489013671875 + gibmag: 0.004840850830078125 + gibphase: 2.30859375 + ogeemag: -0.501953125 + ogeephase: 1.845703125 + phase: 0.0 + tilt: -0.050048828125 + - curve: 0.34033203125 + gibmag: -0.005626678466796875 + gibphase: 0.2646484375 + ogeemag: -0.44775390625 + ogeephase: 2.625 + phase: -0.00021028518676757812 + tilt: 0.04754638671875 + uid: 1107188747 + 2: + sweeps: + - curve: 0.0865478515625 + gibmag: -0.00563812255859375 + gibphase: 2.638671875 + ogeemag: -0.2196044921875 + ogeephase: 0.427734375 + phase: 0.0 + tilt: -0.05029296875 + - curve: 0.250732421875 + gibmag: -0.0036869049072265625 + gibphase: 3.029296875 + ogeemag: -0.333251953125 + ogeephase: 0.86572265625 + phase: -0.0029697418212890625 + tilt: 0.047210693359375 + uid: 1047554996 + 3: + sweeps: + - curve: 0.01904296875 + gibmag: 0.006561279296875 + gibphase: 0.89306640625 + ogeemag: -0.607421875 + ogeephase: 1.2509765625 + phase: 0.0 + tilt: -0.05108642578125 + - curve: 0.075927734375 + gibmag: 0.0052642822265625 + gibphase: 1.9384765625 + ogeemag: -0.544921875 + ogeephase: 1.697265625 + phase: -0.000988006591796875 + tilt: 0.0438232421875 + uid: 2360210604 +geos: + 0: + origin: + - -3.9462287425994873 + - 0.34424617886543274 + - 3.09690260887146 + rotation: + - - 0.6573086380958557 + - -0.02583269774913788 + - 0.753178596496582 + - - 0.053390055894851685 + - 0.9984973669052124 + - -0.012347487732768059 + - - -0.7517279386520386 + - 0.04832835868000984 + - 0.6577001810073853 + 1: + origin: + - -0.5446804165840149 + - -3.217000722885132 + - 3.1017086505889893 + rotation: + - - 0.03625624254345894 + - -0.9991750121116638 + - 0.018295537680387497 + - - 0.5475273132324219 + - 0.03517625108361244 + - 0.8360481262207031 + - - -0.836001992225647 + - -0.020294655114412308 + - 0.548350989818573 + 2: + origin: + - 3.0141966342926025 + - 0.17101018130779266 + - 3.046125650405884 + rotation: + - - -0.7065027952194214 + - 0.08601535856723785 + - -0.7024636268615723 + - - -0.08820294588804245 + - -0.9955493211746216 + - -0.03319304808974266 + - - -0.7021923065185547 + - 0.038508377969264984 + - 0.710945188999176 + 3: + origin: + - -0.43198952078819275 + - 3.766599655151367 + - 3.070936441421509 + rotation: + - - -0.004606463946402073 + - 0.9998911619186401 + - -0.014015298336744308 + - - -0.716507077217102 + - -0.01307705882936716 + - -0.6974572539329529 + - - -0.6975646018981934 + - 0.006829248741269112 + - 0.716489315032959 +systemType: 2 +type: lighthouse_system_configuration +version: '1' diff --git a/test_python/test_kalman_core.py b/test_python/test_kalman_core.py index 1fea4b2db7..963c0fa5d5 100644 --- a/test_python/test_kalman_core.py +++ b/test_python/test_kalman_core.py @@ -4,13 +4,14 @@ from bindings.util.estimator_kalman_emulator import EstimatorKalmanEmulator from bindings.util.sd_card_file_runner import SdCardFileRunner from bindings.util.loco_utils import read_loco_anchor_positions +from bindings.util.lighthouse_utils import read_lh_basestation_pose_calibration def test_kalman_core_with_tdoa3(): # Fixture fixture_base = 'test_python/fixtures/kalman_core' anchor_positions = read_loco_anchor_positions(fixture_base + '/anchor_positions.yaml') runner = SdCardFileRunner(fixture_base + '/log05') - emulator = EstimatorKalmanEmulator(anchor_positions) + emulator = EstimatorKalmanEmulator(anchor_positions=anchor_positions) # Test actual = runner.run_estimator_loop(emulator) @@ -19,3 +20,24 @@ def test_kalman_core_with_tdoa3(): # Verify that the final position is close-ish to (0, 0, 0) actual_final_pos = np.array(actual[-1][1]) assert np.linalg.norm(actual_final_pos - [0.0, 0.0, 0.0]) < 0.4 + + +def test_kalman_core_with_sweep_angles(): + + # Fixture + fixture_base = 'test_python/fixtures/kalman_core' + bs_calib, bs_geo = read_lh_basestation_pose_calibration(fixture_base + '/geometry.yaml') + runner = SdCardFileRunner(fixture_base + '/Bindings13') + emulator = EstimatorKalmanEmulator(basestation_calibration=bs_calib, basestation_poses=bs_geo) + + # Test + actual = runner.run_estimator_loop(emulator) + + # Assert + # Verify that the final position is close-ish to (0, 0, 0) + + #for it in range(1,len(np.array(actual[:])),10): + # print(actual[it][1]) + #actual_final_pos = np.array(actual[-1][1]) + #print(actual_final_pos) + assert np.linalg.norm(actual_final_pos - [0.8, -1.2, 0.5]) < 0.4 \ No newline at end of file