dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

WIP: save cal if disarmed, C -> C++, cleanup

This commit is contained in:
Daniel Agar 2020-10-25 11:00:11 -04:00
parent 2c6d664a54
commit d602cf0fd1
No known key found for this signature in database
GPG Key ID: FD3CBA98017A69DE
11 changed files with 143 additions and 297 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp
View File

@ -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};

4
src/lib/sensor_calibration/Magnetometer.cpp
View File

@ -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
}

1
src/lib/sensor_calibration/Magnetometer.hpp
View File

@ -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; }

93
src/modules/mag_calibrator/3dmath.h
View File

@ -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

5
src/modules/mag_calibrator/CMakeLists.txt
View File

@ -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

61
src/modules/mag_calibrator/MagCalibrator.cpp
View File

@ -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();
}
}

9
src/modules/mag_calibrator/MagCalibrator.hpp
View File

@ -53,6 +53,7 @@
#include <uORB/topics/sensor_mag.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_status.h>
class MagCalibrator : public ModuleBase<MagCalibrator>, 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_s, MAX_SENSOR_COUNT> _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")};
};

64
src/modules/mag_calibrator/TRICAL.c → src/modules/mag_calibrator/TRICAL.cpp
View File

@ -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);
}

181
src/modules/mag_calibrator/filter.c → src/modules/mag_calibrator/filter.cpp
View File

@ -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 <stdio.h>
@ -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);
}

19
src/modules/mag_calibrator/filter.h
View File

@ -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
}

1
src/modules/mag_calibrator/test/test_3dmath.cpp
View File

@ -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