From d602cf0fd1361b5692d85887906ce0cc3694b9e0 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 25 Oct 2020 11:00:11 -0400 Subject: [PATCH] WIP: save cal if disarmed, C -> C++, cleanup --- .../px4_work_queue/WorkQueueManager.hpp | 2 +- src/lib/sensor_calibration/Magnetometer.cpp | 4 +- src/lib/sensor_calibration/Magnetometer.hpp | 1 + src/modules/mag_calibrator/3dmath.h | 93 --------- src/modules/mag_calibrator/CMakeLists.txt | 5 +- src/modules/mag_calibrator/MagCalibrator.cpp | 61 +++--- src/modules/mag_calibrator/MagCalibrator.hpp | 9 +- .../mag_calibrator/{TRICAL.c => TRICAL.cpp} | 64 +------ .../mag_calibrator/{filter.c => filter.cpp} | 181 +++++++++--------- src/modules/mag_calibrator/filter.h | 19 +- .../mag_calibrator/test/test_3dmath.cpp | 1 - 11 files changed, 143 insertions(+), 297 deletions(-) delete mode 100644 src/modules/mag_calibrator/3dmath.h rename src/modules/mag_calibrator/{TRICAL.c => TRICAL.cpp} (76%) rename src/modules/mag_calibrator/{filter.c => filter.cpp} (72%) diff --git a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp index c08d44fb33..ad78839374 100644 --- a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp +++ b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp @@ -83,7 +83,7 @@ static constexpr wq_config_t UART7{"wq:UART7", 1400, -24}; static constexpr wq_config_t UART8{"wq:UART8", 1400, -25}; static constexpr wq_config_t UART_UNKNOWN{"wq:UART_UNKNOWN", 1400, -26}; -static constexpr wq_config_t lp_default{"wq:lp_default", 10700, -50}; +static constexpr wq_config_t lp_default{"wq:lp_default", 2700, -50}; static constexpr wq_config_t test1{"wq:test1", 2000, 0}; static constexpr wq_config_t test2{"wq:test2", 2000, 0}; diff --git a/src/lib/sensor_calibration/Magnetometer.cpp b/src/lib/sensor_calibration/Magnetometer.cpp index fe0814476c..e22b413f01 100644 --- a/src/lib/sensor_calibration/Magnetometer.cpp +++ b/src/lib/sensor_calibration/Magnetometer.cpp @@ -269,8 +269,8 @@ void Magnetometer::PrintStatus() (double)_scale(0, 0), (double)_scale(1, 1), (double)_scale(2, 2)); } -#if defined(DEBUG_BUILD) - _scale.print() +#if defined(DEBUG_BUILD) || true + _scale.print(); #endif // DEBUG_BUILD } diff --git a/src/lib/sensor_calibration/Magnetometer.hpp b/src/lib/sensor_calibration/Magnetometer.hpp index 13ddc83eff..eeb0e16a62 100644 --- a/src/lib/sensor_calibration/Magnetometer.hpp +++ b/src/lib/sensor_calibration/Magnetometer.hpp @@ -69,6 +69,7 @@ public: void set_rotation(Rotation rotation); uint8_t calibration_count() const { return _calibration_count; } + int8_t calibration_index() const { return _calibration_index; } uint32_t device_id() const { return _device_id; } bool enabled() const { return (_priority > 0); } bool external() const { return _external; } diff --git a/src/modules/mag_calibrator/3dmath.h b/src/modules/mag_calibrator/3dmath.h deleted file mode 100644 index acebb7634e..0000000000 --- a/src/modules/mag_calibrator/3dmath.h +++ /dev/null @@ -1,93 +0,0 @@ -/* -Copyright (C) 2013 Ben Dyer - -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in -all copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -SOFTWARE. -*/ - -#ifndef _3DMATH_H_ -#define _3DMATH_H_ - -#ifdef __cplusplus -extern "C" { -#endif - -#define X 0 -#define Y 1 -#define Z 2 -#define W 3 - -#ifndef absval -#define absval(x) ((x) < 0 ? -x : x) -#endif - -#ifndef min -#define min(a,b) (((a) < (b)) ? (a) : (b)) -#endif - -#ifndef max -#define max(a,b) (((a) > (b)) ? (a) : (b)) -#endif - -#ifndef M_PI -#define M_PI ((real_t)3.14159265358979323846) -#define M_PI_2 (M_PI * 0.5) -#define M_PI_4 (M_PI * 0.25) -#endif - -#define sqrt_inv(x) (1.0f / (float)sqrt((x))) -#define divide(a, b) ((a) / (b)) -#define recip(a) (1.0f / (a)) -#define _nassert(x) - -static void matrix_cholesky_decomp_scale_f(unsigned int dim, float L[], - const float A[], const float mul) -{ - assert(L && A && dim); - _nassert((size_t)L % 8 == 0); - _nassert((size_t)A % 8 == 0); - - /* - 9x9: - 900 mult - 72 div - 9 sqrt - */ - - unsigned int i, j, kn, in, jn; - - for (i = 0, in = 0; i < dim; i++, in += dim) { - L[i + 0] = (i == 0) ? sqrtf(A[i + in] * mul) : recip(L[0]) * (A[i] * mul); - - for (j = 1, jn = dim; j <= i; j++, jn += dim) { - float s = 0; - - for (kn = 0; kn < j * dim; kn += dim) { - s += L[i + kn] * L[j + kn]; - } - - L[i + jn] = (i == j) ? sqrtf(A[i + in] * mul - s) : recip(L[j + jn]) * (A[i + jn] * mul - s); - } - } -} - -#ifdef __cplusplus -} -#endif - -#endif diff --git a/src/modules/mag_calibrator/CMakeLists.txt b/src/modules/mag_calibrator/CMakeLists.txt index 02b8684583..144fdff38e 100644 --- a/src/modules/mag_calibrator/CMakeLists.txt +++ b/src/modules/mag_calibrator/CMakeLists.txt @@ -43,10 +43,9 @@ px4_add_module( MagCalibrator.hpp # TRICAL - 3dmath.h - filter.c + filter.cpp filter.h - TRICAL.c + TRICAL.cpp TRICAL.h DEPENDS ecl_geo_lookup diff --git a/src/modules/mag_calibrator/MagCalibrator.cpp b/src/modules/mag_calibrator/MagCalibrator.cpp index 402b0329ca..0e5ac75518 100644 --- a/src/modules/mag_calibrator/MagCalibrator.cpp +++ b/src/modules/mag_calibrator/MagCalibrator.cpp @@ -46,14 +46,13 @@ using math::radians; MagCalibrator::MagCalibrator() : ModuleParams(nullptr), - ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default), - _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default) { for (int mag_instance = 0; mag_instance < MAX_SENSOR_COUNT; mag_instance++) { TRICAL_init(&_trical_instance[mag_instance]); TRICAL_norm_set(&_trical_instance[mag_instance], 0.4f); - TRICAL_noise_set(&_trical_instance[mag_instance], 1e-4f); + TRICAL_noise_set(&_trical_instance[mag_instance], 0.01f); } } @@ -89,6 +88,14 @@ void MagCalibrator::Run() //parameters_updated(); } + if (_vehicle_status_sub.updated()) { + vehicle_status_s vehicle_status; + + if (_vehicle_status_sub.copy(&vehicle_status)) { + _armed = vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED; + } + } + if (_vehicle_gps_position_sub.updated()) { vehicle_gps_position_s gps; @@ -201,7 +208,24 @@ void MagCalibrator::Run() _estimator_mag_cal_pub[mag_instance].publish(mag_cal); // update calibration - //_calibration.set_offsets(offsets); + if ((mag_cal.error < 0.20f) && (mag_cal.error < _calibration_error[mag_instance])) { + _calibration[mag_instance].set_device_id(mag.device_id, mag.is_external); + _calibration[mag_instance].set_offset(offsets); + _calibration[mag_instance].set_scale(Vector3f{mag_cal.diagonal_scale}); + _calibration[mag_instance].set_offdiagonal(Vector3f{mag_cal.offdiagonal_scale}); + + if (!_armed && hrt_elapsed_time(&_last_calibration_save[mag_instance]) > 10_s) { + if (_calibration[mag_instance].calibration_index() < 0) { + _calibration[mag_instance].set_calibration_index(mag_instance); + } + + _calibration[mag_instance].ParametersSave(); + param_notify_changes(); + + _last_calibration_save[mag_instance] = hrt_absolute_time(); + _calibration_error[mag_instance] = mag_cal.error; + } + } } } } @@ -215,34 +239,7 @@ int MagCalibrator::print_status() for (int mag_instance = 0; mag_instance < MAX_SENSOR_COUNT; mag_instance++) { if (_initialized[mag_instance]) { - - float bias_estimate[3]; - float scale_estimate[9]; - float bias_estimate_variance[3]; - float scale_estimate_variance[9]; - - TRICAL_estimate_get_ext(&_trical_instance[mag_instance], bias_estimate, scale_estimate, bias_estimate_variance, - scale_estimate_variance); - - printf("bias_estimate: %d\n", mag_instance); - - for (int i = 0; i < 3; i++) { - printf("%05.4f ", (double)bias_estimate[i]); - } - - printf("\n\n"); - - printf("scale_estimate: %d\n", mag_instance); - - for (int c = 0; c < 3; c++) { - for (int r = 0; r < 3; r++) { - printf("%05.4f ", (double)scale_estimate[r + 3 * c]); - } - - printf("\n"); - } - - printf("\n"); + _calibration[mag_instance].PrintStatus(); } } diff --git a/src/modules/mag_calibrator/MagCalibrator.hpp b/src/modules/mag_calibrator/MagCalibrator.hpp index 5745db2983..9a3bb17f68 100644 --- a/src/modules/mag_calibrator/MagCalibrator.hpp +++ b/src/modules/mag_calibrator/MagCalibrator.hpp @@ -53,6 +53,7 @@ #include #include #include +#include class MagCalibrator : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { @@ -100,8 +101,14 @@ private: uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; uORB::SubscriptionMultiArray _sensor_mag_subs{ORB_ID::sensor_mag}; - perf_counter_t _loop_perf; /**< loop duration performance counter */ + hrt_abstime _last_calibration_save[MAX_SENSOR_COUNT] {}; + float _calibration_error[MAX_SENSOR_COUNT] {INFINITY, INFINITY, INFINITY, INFINITY}; + + bool _armed{false}; + + perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; }; diff --git a/src/modules/mag_calibrator/TRICAL.c b/src/modules/mag_calibrator/TRICAL.cpp similarity index 76% rename from src/modules/mag_calibrator/TRICAL.c rename to src/modules/mag_calibrator/TRICAL.cpp index 46d0cb86ec..1a6059cc16 100644 --- a/src/modules/mag_calibrator/TRICAL.c +++ b/src/modules/mag_calibrator/TRICAL.cpp @@ -38,8 +38,6 @@ reset that instance to its default state. */ void TRICAL_init(TRICAL_instance_t *instance) { - assert(instance); - memset(instance, 0, sizeof(TRICAL_instance_t)); instance->field_norm = 1.0f; @@ -49,10 +47,7 @@ void TRICAL_init(TRICAL_instance_t *instance) Set the state covariance diagonal to a small value, so that we can run the Cholesky decomposition without blowing up */ - unsigned int i; - - for (i = 0; i < TRICAL_STATE_DIM * TRICAL_STATE_DIM; - i += (TRICAL_STATE_DIM + 1)) { + for (unsigned i = 0; i < TRICAL_STATE_DIM * TRICAL_STATE_DIM; i += (TRICAL_STATE_DIM + 1)) { instance->state_covariance[i] = 1e-2f; } } @@ -63,8 +58,6 @@ Resets the state and state covariance of `instance`. */ void TRICAL_reset(TRICAL_instance_t *instance) { - assert(instance); - memset(instance->state, 0, sizeof(instance->state)); memset(instance->state_covariance, 0, sizeof(instance->state_covariance)); @@ -72,10 +65,7 @@ void TRICAL_reset(TRICAL_instance_t *instance) Set the state covariance diagonal to a small value, so that we can run the Cholesky decomposition without blowing up */ - unsigned int i; - - for (i = 0; i < TRICAL_STATE_DIM * TRICAL_STATE_DIM; - i += (TRICAL_STATE_DIM + 1)) { + for (unsigned i = 0; i < TRICAL_STATE_DIM * TRICAL_STATE_DIM; i += (TRICAL_STATE_DIM + 1)) { instance->state_covariance[i] = 1e-2f; } } @@ -86,9 +76,6 @@ Sets the expected field norm (magnitude) of `instance` to `norm`. */ void TRICAL_norm_set(TRICAL_instance_t *instance, float norm) { - assert(instance); - assert(norm > FLT_EPSILON); - instance->field_norm = norm; } @@ -98,8 +85,6 @@ Returns the expected field norm (magnitude) of `instance`. */ float TRICAL_norm_get(TRICAL_instance_t *instance) { - assert(instance); - return instance->field_norm; } @@ -109,9 +94,6 @@ Sets the standard deviation in measurement supplied to `instance` to `noise`. */ void TRICAL_noise_set(TRICAL_instance_t *instance, float noise) { - assert(instance); - assert(noise > FLT_EPSILON); - instance->measurement_noise = noise; } @@ -121,8 +103,6 @@ Returns the standard deviation in measurements supplied to `instance`. */ float TRICAL_noise_get(TRICAL_instance_t *instance) { - assert(instance); - return instance->measurement_noise; } @@ -133,8 +113,6 @@ TRICAL_estimate_update. */ unsigned int TRICAL_measurement_count_get(TRICAL_instance_t *instance) { - assert(instance); - return instance->measurement_count; } @@ -144,13 +122,8 @@ Updates the calibration estimate of `instance` based on the new data in `measurement`, and the current field direction estimate `reference_field`. Call this function with each reading you receive from your sensor. */ -void TRICAL_estimate_update(TRICAL_instance_t *instance, - float measurement[3], float reference_field[3]) +void TRICAL_estimate_update(TRICAL_instance_t *instance, float measurement[3], float reference_field[3]) { - assert(instance); - assert(measurement); - assert(reference_field); - _trical_filter_iterate(instance, measurement, reference_field); instance->measurement_count++; } @@ -161,14 +134,8 @@ Copies the calibration bias and scale esimates of `instance` to `bias_estimate` and `scale_estimate` respectively. A new calibration estimate will be available after every call to TRICAL_estimate_update. */ -void TRICAL_estimate_get(TRICAL_instance_t *restrict instance, - float bias_estimate[3], float scale_estimate[9]) +void TRICAL_estimate_get(TRICAL_instance_t *instance, float bias_estimate[3], float scale_estimate[9]) { - assert(instance); - assert(bias_estimate); - assert(scale_estimate); - assert(bias_estimate != scale_estimate); - /* Copy bias estimate from state[0:3] to the destination vector */ memcpy(bias_estimate, instance->state, 3 * sizeof(float)); @@ -181,21 +148,10 @@ TRICAL_estimate_get_ext Same as TRICAL_estimate_get, but additionally copies the bias and scale estimate variances to `bias_estimate_variance` and `scale_estimate_variance`. */ -void TRICAL_estimate_get_ext(TRICAL_instance_t *restrict instance, - float bias_estimate[3], float scale_estimate[9], - float bias_estimate_variance[3], float scale_estimate_variance[9]) +void TRICAL_estimate_get_ext(TRICAL_instance_t *instance, float bias_estimate[3], float scale_estimate[9], float bias_estimate_variance[3], float scale_estimate_variance[9]) { TRICAL_estimate_get(instance, bias_estimate, scale_estimate); - /* A bit of paranoia to avoid potential undefined behaviour */ - assert(bias_estimate_variance); - assert(scale_estimate_variance); - assert(bias_estimate != bias_estimate_variance); - assert(bias_estimate != scale_estimate_variance); - assert(scale_estimate != bias_estimate_variance); - assert(scale_estimate != scale_estimate_variance); - assert(bias_estimate_variance != scale_estimate_variance); - /* Copy bias estimate covariance from the state covariance diagonal */ bias_estimate_variance[0] = instance->state_covariance[0 * 12 + 0]; bias_estimate_variance[1] = instance->state_covariance[1 * 12 + 1]; @@ -224,14 +180,8 @@ copies the result to `calibrated_measurement`. The `measurement` and DO NOT pass the calibrated measurement into TRICAL_estimate_update, as it needs the raw measurement values to work. */ -void TRICAL_measurement_calibrate(TRICAL_instance_t *restrict instance, - float measurement[3], float calibrated_measurement[3]) +void TRICAL_measurement_calibrate(TRICAL_instance_t * instance, float measurement[3], float calibrated_measurement[3]) { - assert(instance); - assert(measurement); - assert(calibrated_measurement); - /* Pass off to the internal function */ - _trical_measurement_calibrate(instance->state, measurement, - calibrated_measurement); + _trical_measurement_calibrate(instance->state, measurement, calibrated_measurement); } diff --git a/src/modules/mag_calibrator/filter.c b/src/modules/mag_calibrator/filter.cpp similarity index 72% rename from src/modules/mag_calibrator/filter.c rename to src/modules/mag_calibrator/filter.cpp index 3807b617f0..c903c45bfa 100644 --- a/src/modules/mag_calibrator/filter.c +++ b/src/modules/mag_calibrator/filter.cpp @@ -27,7 +27,11 @@ SOFTWARE. #include "TRICAL.h" #include "filter.h" -#include "3dmath.h" + +#define X 0 +#define Y 1 +#define Z 2 +#define W 3 #ifdef DEBUG #include @@ -109,8 +113,7 @@ above, when we can just apply the current calibration estimate to the measurement and take its magnitude. Look into this further if the below approach doesn't work. */ -float _trical_measurement_reduce(float state[TRICAL_STATE_DIM], float - measurement[3], float field[3]) +float _trical_measurement_reduce(float state[TRICAL_STATE_DIM], float measurement[3], float field[3]) { float temp[3]; _trical_measurement_calibrate(state, measurement, temp); @@ -131,101 +134,122 @@ where B' is the calibrated measurement, I_{3x3} is the 3x3 identity matrix, D is the scale calibration matrix, B is the raw measurement, and b is the bias vector. */ -void _trical_measurement_calibrate(float state[TRICAL_STATE_DIM], - float measurement[3], float calibrated_measurement[3]) +void _trical_measurement_calibrate(float state[TRICAL_STATE_DIM], float measurement[3], float calibrated_measurement[3]) { - assert(state && measurement && calibrated_measurement); - float v[3]; - float *restrict s = state; - float *restrict c = calibrated_measurement; + float *s = state; v[0] = measurement[0] - s[0]; v[1] = measurement[1] - s[1]; v[2] = measurement[2] - s[2]; /* 3x3 matrix multiply */ + float *c = calibrated_measurement; c[0] = v[0] * (s[3] + 1.0f) + v[1] * s[4] + v[2] * s[5]; c[1] = v[0] * s[6] + v[1] * (s[7] + 1.0f) + v[2] * s[8]; c[2] = v[0] * s[9] + v[1] * s[10] + v[2] * (s[11] + 1.0f); } +static void matrix_cholesky_decomp_scale_f(unsigned int dim, float L[], const float A[], const float mul) +{ + /* + 9x9: + 900 mult + 72 div + 9 sqrt + */ + unsigned i; + unsigned in; + for (i = 0, in = 0; i < dim; i++, in += dim) { + + if (i == 0) { + L[i + 0] = sqrtf(A[i + in] * mul); + } else { + L[i + 0] = (1.f / L[0]) * (A[i] * mul); + } + + unsigned j; + unsigned jn; + for (j = 1, jn = dim; j <= i; j++, jn += dim) { + float s = 0; + + for (unsigned kn = 0; kn < j * dim; kn += dim) { + s += L[i + kn] * L[j + kn]; + } + + if (i == j) { + L[i + jn] = sqrtf(A[i + in] * mul - s); + + } else { + L[i + jn] = (1.f / (L[j + jn])) * (A[i + jn] * mul - s); + } + } + } +} + /* _trical_filter_iterate Generates a new calibration estimate for `instance` incorporating the raw sensor readings in `measurement`. */ -void _trical_filter_iterate(TRICAL_instance_t *instance, - float measurement[3], float field[3]) +void _trical_filter_iterate(TRICAL_instance_t *instance, float measurement[3], float field[3]) { - unsigned int i, j, k, l, col; - - float *restrict covariance = instance->state_covariance; - float *restrict state = instance->state; - /* LLT decomposition on state covariance matrix, with result multiplied by TRICAL_DIM_PLUS_LAMBDA */ - float covariance_llt[TRICAL_STATE_DIM * TRICAL_STATE_DIM]; - memset(covariance_llt, 0, sizeof(covariance_llt)); - matrix_cholesky_decomp_scale_f( - TRICAL_STATE_DIM, covariance_llt, covariance, TRICAL_DIM_PLUS_LAMBDA); + float *covariance = instance->state_covariance; + float covariance_llt[TRICAL_STATE_DIM * TRICAL_STATE_DIM]{}; - _print_matrix("LLT:\n", covariance_llt, TRICAL_STATE_DIM, - TRICAL_STATE_DIM); + matrix_cholesky_decomp_scale_f(TRICAL_STATE_DIM, covariance_llt, covariance, TRICAL_DIM_PLUS_LAMBDA); + + _print_matrix("LLT:\n", covariance_llt, TRICAL_STATE_DIM, TRICAL_STATE_DIM); /* Generate the sigma points, and use them as the basis of the measurement estimates */ - float temp_sigma[TRICAL_STATE_DIM], temp; - float measurement_estimates[TRICAL_NUM_SIGMA], measurement_estimate_mean; - - measurement_estimate_mean = 0.0; + float temp_sigma[TRICAL_STATE_DIM]{}; + float temp; + float measurement_estimates[TRICAL_NUM_SIGMA]{}; + float measurement_estimate_mean = 0.0; /* Handle central sigma point -- process the measurement based on the current state vector */ - measurement_estimates[0] = _trical_measurement_reduce(state, measurement, - field); - - __asm__ __volatile__(""); + float *state = instance->state; + measurement_estimates[0] = _trical_measurement_reduce(state, measurement, field); + unsigned i; + unsigned col; for (i = 0, col = 0; i < TRICAL_STATE_DIM; i++, col += TRICAL_STATE_DIM) { /* Handle the positive sigma point -- perturb the state vector based on the current column of the covariance matrix, and process the measurement based on the resulting state estimate */ - __asm__ __volatile__(""); - + int k; + int l; for (k = col, l = 0; l < TRICAL_STATE_DIM; k++, l++) { temp_sigma[l] = state[l] + covariance_llt[k]; } - measurement_estimates[i + 1] = - _trical_measurement_reduce(temp_sigma, measurement, field); + measurement_estimates[i + 1] = _trical_measurement_reduce(temp_sigma, measurement, field); /* Handle the negative sigma point -- mirror of the above */ - __asm__ __volatile__(""); - for (k = col, l = 0; l < TRICAL_STATE_DIM; k++, l++) { temp_sigma[l] = state[l] - covariance_llt[k]; } - measurement_estimates[i + 1 + TRICAL_STATE_DIM] = - _trical_measurement_reduce(temp_sigma, measurement, field); + measurement_estimates[i + 1 + TRICAL_STATE_DIM] = _trical_measurement_reduce(temp_sigma, measurement, field); /* Calculate the measurement estimate sum as we go */ - temp = measurement_estimates[i + 1] + - measurement_estimates[i + 1 + TRICAL_STATE_DIM]; + temp = measurement_estimates[i + 1] + measurement_estimates[i + 1 + TRICAL_STATE_DIM]; measurement_estimate_mean += temp; } - measurement_estimate_mean = measurement_estimate_mean * TRICAL_SIGMA_WMI + - measurement_estimates[0] * TRICAL_SIGMA_WM0; + measurement_estimate_mean = measurement_estimate_mean * TRICAL_SIGMA_WMI + measurement_estimates[0] * TRICAL_SIGMA_WM0; /* Convert estimates to deviation from mean (so measurement_estimates @@ -236,8 +260,6 @@ void _trical_filter_iterate(TRICAL_instance_t *instance, */ float measurement_estimate_covariance = 0.0; - __asm__ __volatile__(""); - for (i = 0; i < TRICAL_NUM_SIGMA; i++) { measurement_estimates[i] -= measurement_estimate_mean; @@ -245,70 +267,46 @@ void _trical_filter_iterate(TRICAL_instance_t *instance, measurement_estimate_covariance += temp; } - _print_matrix("Measurement estimates:\n", measurement_estimates, 1, - TRICAL_NUM_SIGMA); + _print_matrix("Measurement estimates:\n", measurement_estimates, 1, TRICAL_NUM_SIGMA); /* Add the sensor noise to the measurement estimate covariance */ temp = instance->measurement_noise * instance->measurement_noise; measurement_estimate_covariance += temp; /* Calculate cross-correlation matrix (1 x TRICAL_STATE_DIM) */ - float cross_correlation[TRICAL_STATE_DIM]; - memset(cross_correlation, 0, sizeof(cross_correlation)); + float cross_correlation[TRICAL_STATE_DIM]{}; - /* - Calculate the innovation (difference between the expected value, i.e. the - field norm, and the measurement estimate mean). - */ - float innovation; - innovation = instance->field_norm - measurement_estimate_mean; + // Calculate the innovation (difference between the expected value, i.e. the + // field norm, and the measurement estimate mean). + float innovation = instance->field_norm - measurement_estimate_mean; /* Iterate over sigma points, two at a time */ - __asm__ __volatile__(""); - for (i = 0; i < TRICAL_STATE_DIM; i++) { /* Iterate over the cross-correlation matrix */ - __asm__ __volatile__(""); - - for (j = 0; j < TRICAL_STATE_DIM; j++) { - /* - We're regenerating the sigma points as we go, so that we don't - need to store W'. - */ - temp = measurement_estimates[i + 1] * - (state[j] + covariance_llt[i * TRICAL_STATE_DIM + j]); + for (int j = 0; j < TRICAL_STATE_DIM; j++) { + // We're regenerating the sigma points as we go, so that we don't need to store W' + temp = measurement_estimates[i + 1] * (state[j] + covariance_llt[i * TRICAL_STATE_DIM + j]); cross_correlation[j] += temp; - temp = measurement_estimates[i + 1 + TRICAL_STATE_DIM] * - (state[j] - covariance_llt[i * TRICAL_STATE_DIM + j]); + temp = measurement_estimates[i + 1 + TRICAL_STATE_DIM] * (state[j] - covariance_llt[i * TRICAL_STATE_DIM + j]); cross_correlation[j] += temp; } } - /* - Scale the results of the previous step, and add in the scaled central - sigma point - */ - __asm__ __volatile__(""); - - for (j = 0; j < TRICAL_STATE_DIM; j++) { + // Scale the results of the previous step, and add in the scaled central sigma point + for (int j = 0; j < TRICAL_STATE_DIM; j++) { temp = TRICAL_SIGMA_WC0 * measurement_estimates[0] * state[j]; cross_correlation[j] = TRICAL_SIGMA_WCI * cross_correlation[j] + temp; } - _print_matrix("Cross-correlation:\n", cross_correlation, 1, - TRICAL_STATE_DIM); + _print_matrix("Cross-correlation:\n", cross_correlation, 1, TRICAL_STATE_DIM); - /* - Update the state -- since the measurement is a scalar, we can calculate - the Kalman gain and update in a single pass. - */ - float kalman_gain; - temp = recip(measurement_estimate_covariance); - __asm__ __volatile__(""); + // Update the state -- since the measurement is a scalar, we can calculate + // the Kalman gain and update in a single pass. + temp = 1.f / measurement_estimate_covariance; for (i = 0; i < TRICAL_STATE_DIM; i++) { - kalman_gain = cross_correlation[i] * temp; + float kalman_gain = cross_correlation[i] * temp; state[i] += kalman_gain * innovation; } @@ -316,8 +314,7 @@ void _trical_filter_iterate(TRICAL_instance_t *instance, /* Update the state covariance: - covariance = covariance - kalman gain * measurement estimate covariance * - (transpose of kalman gain) + covariance = covariance - kalman gain * measurement estimate covariance * (transpose of kalman gain) Since kalman gain is a 1 x TRICAL_STATE_DIM matrix, multiplying by its transpose is just a vector outer product accumulating onto state @@ -328,19 +325,13 @@ void _trical_filter_iterate(TRICAL_instance_t *instance, estimate covariance during the outer product, we can skip that whole step and just use cross correlation instead. */ - __asm__ __volatile__(""); - for (i = 0; i < TRICAL_STATE_DIM; i++) { temp = -cross_correlation[i]; - __asm__ __volatile__(""); - - for (j = 0; j < TRICAL_STATE_DIM; j++) { - covariance[i * TRICAL_STATE_DIM + j] += - temp * cross_correlation[j]; + for (int j = 0; j < TRICAL_STATE_DIM; j++) { + covariance[i * TRICAL_STATE_DIM + j] += temp * cross_correlation[j]; } } - _print_matrix("State covariance:\n", covariance, TRICAL_STATE_DIM, - TRICAL_STATE_DIM); + _print_matrix("State covariance:\n", covariance, TRICAL_STATE_DIM, TRICAL_STATE_DIM); } diff --git a/src/modules/mag_calibrator/filter.h b/src/modules/mag_calibrator/filter.h index 7e00ec0b35..ab24711f2b 100644 --- a/src/modules/mag_calibrator/filter.h +++ b/src/modules/mag_calibrator/filter.h @@ -33,14 +33,11 @@ extern "C" { #define TRICAL_ALPHA_2 (1.0f) #define TRICAL_BETA (0.0f) #define TRICAL_KAPPA (1.0f) -#define TRICAL_LAMBDA (TRICAL_ALPHA_2 * (TRICAL_STATE_DIM + TRICAL_KAPPA) - \ - TRICAL_STATE_DIM) -#define TRICAL_DIM_PLUS_LAMBDA (TRICAL_ALPHA_2 * \ - (TRICAL_STATE_DIM + TRICAL_KAPPA)) +#define TRICAL_LAMBDA (TRICAL_ALPHA_2 * (TRICAL_STATE_DIM + TRICAL_KAPPA) - TRICAL_STATE_DIM) +#define TRICAL_DIM_PLUS_LAMBDA (TRICAL_ALPHA_2 * (TRICAL_STATE_DIM + TRICAL_KAPPA)) #define TRICAL_SIGMA_WM0 (TRICAL_LAMBDA / TRICAL_DIM_PLUS_LAMBDA) -#define TRICAL_SIGMA_WC0 (TRICAL_SIGMA_WM0 + \ - (1.0f - TRICAL_ALPHA_2 + TRICAL_BETA)) +#define TRICAL_SIGMA_WC0 (TRICAL_SIGMA_WM0 + (1.0f - TRICAL_ALPHA_2 + TRICAL_BETA)) #define TRICAL_SIGMA_WMI (1.0f / (2.0f * TRICAL_DIM_PLUS_LAMBDA)) #define TRICAL_SIGMA_WCI (TRICAL_SIGMA_WMI) @@ -51,8 +48,7 @@ _trical_measurement_reduce Reduces `measurement` to a scalar value based on the calibration estimate in `state`. */ -float _trical_measurement_reduce(float state[TRICAL_STATE_DIM], float - measurement[3], float field[3]); +float _trical_measurement_reduce(float state[TRICAL_STATE_DIM], float measurement[3], float field[3]); /* _trical_measurement_calibrate @@ -60,16 +56,15 @@ Calibrates `measurement` based on the calibration estimate in `state` and copies the result to `calibrated_measurement`. The `measurement` and `calibrated_measurement` parameters may be pointers to the same vector. */ -void _trical_measurement_calibrate(float state[TRICAL_STATE_DIM], - float measurement[3], float calibrated_measurement[3]); +void _trical_measurement_calibrate(float state[TRICAL_STATE_DIM], float measurement[3], + float calibrated_measurement[3]); /* _trical_filter_iterate Generates a new calibration estimate for `instance` incorporating the raw sensor readings in `measurement`. */ -void _trical_filter_iterate(TRICAL_instance_t *instance, - float measurement[3], float field[3]); +void _trical_filter_iterate(TRICAL_instance_t *instance, float measurement[3], float field[3]); #ifdef __cplusplus } diff --git a/src/modules/mag_calibrator/test/test_3dmath.cpp b/src/modules/mag_calibrator/test/test_3dmath.cpp index 6ea2c60118..666a669020 100644 --- a/src/modules/mag_calibrator/test/test_3dmath.cpp +++ b/src/modules/mag_calibrator/test/test_3dmath.cpp @@ -28,7 +28,6 @@ SOFTWARE. #define restrict #include "TRICAL.h" -#include "3dmath.h" /* Test LLT decomposition of a square positive definite matrix via the Cholesky