WIP: online mag cal using UKF

This commit is contained in:
Daniel Agar 2020-09-29 21:06:39 -04:00
parent 945c17bc3f
commit 2c6d664a54
No known key found for this signature in database
GPG Key ID: FD3CBA98017A69DE
23 changed files with 2027 additions and 4 deletions

View File

@ -214,6 +214,7 @@ battery_simulator start
tone_alarm start
rc_update start
sensors start
mag_calibrator start
commander start
navigator start

View File

@ -139,3 +139,5 @@ fi
###############################################################################
sensors start
mag_calibrator start

View File

@ -15,7 +15,7 @@ px4_add_board(
optical_flow/pmw3901
pwm_out
MODULES
attitude_estimator_q
#attitude_estimator_q
#camera_feedback
commander
dataman
@ -24,7 +24,7 @@ px4_add_board(
land_detector
landing_target_estimator
load_mon
local_position_estimator
#local_position_estimator
logger
mavlink
mc_att_control
@ -41,7 +41,7 @@ px4_add_board(
dumpfile
esc_calib
hardfault_log
i2cdetect
#i2cdetect
led_control
mixer
motor_ramp

View File

@ -73,6 +73,7 @@ px4_add_board(
load_mon
local_position_estimator
logger
mag_calibrator
mavlink
mc_att_control
mc_hover_thrust_estimator

View File

@ -78,6 +78,7 @@ px4_add_board(
load_mon
local_position_estimator
logger
mag_calibrator
mavlink
mc_att_control
mc_hover_thrust_estimator

View File

@ -39,6 +39,7 @@ px4_add_board(
#load_mon
local_position_estimator
logger
mag_calibrator
mavlink
mc_att_control
mc_hover_thrust_estimator

View File

@ -38,6 +38,7 @@ px4_add_board(
#load_mon
local_position_estimator
logger
mag_calibrator
mavlink
mc_att_control
mc_hover_thrust_estimator

View File

@ -55,6 +55,7 @@ set(msg_files
ekf_gps_drift.msg
esc_report.msg
esc_status.msg
estimator_mag_cal.msg
estimator_innovations.msg
estimator_sensor_bias.msg
estimator_states.msg

29
msg/estimator_mag_cal.msg Normal file
View File

@ -0,0 +1,29 @@
uint64 timestamp # time since system start (microseconds)
uint8 instance
uint32 device_id # unique device ID for the selected magnetometer
float32[3] sample # magnetic field in the NED board axis in Gauss
float32[3] expected_field
float32[3] calibrated_sample
float32 calibrated_sample_magnitude
float32 error
float32 declination
float32 inclination
float32 mag_strength
float32[3] bias_estimate
float32[9] scale_estimate
float32[3] bias_estimate_variance
float32[9] scale_estimate_variance
float32[3] diagonal_scale
float32[3] offdiagonal_scale
float32[3] offsets

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", 1700, -50};
static constexpr wq_config_t lp_default{"wq:lp_default", 10700, -50};
static constexpr wq_config_t test1{"wq:test1", 2000, 0};
static constexpr wq_config_t test2{"wq:test2", 2000, 0};

View File

@ -130,6 +130,8 @@ void LoggedTopics::add_default_topics()
add_topic_multi("vehicle_imu", 500, 4);
add_topic_multi("vehicle_imu_status", 1000, 4);
add_topic_multi("estimator_mag_cal");
#ifdef CONFIG_ARCH_BOARD_PX4_SITL
add_topic("actuator_controls_virtual_fw");
add_topic("actuator_controls_virtual_mc");

View File

@ -0,0 +1,93 @@
/*
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

View File

@ -0,0 +1,56 @@
############################################################################
#
# Copyright (c) 2020 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_module(
MODULE modules__mag_calibrator
MAIN mag_calibrator
COMPILE_FLAGS
${MAX_CUSTOM_OPT_LEVEL}
#-O0
#-DDEBUG_BUILD
SRCS
MagCalibrator.cpp
MagCalibrator.hpp
# TRICAL
3dmath.h
filter.c
filter.h
TRICAL.c
TRICAL.h
DEPENDS
ecl_geo_lookup
mathlib
px4_work_queue
#UNITY_BUILD
)

View File

@ -0,0 +1,303 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "MagCalibrator.hpp"
#include <drivers/drv_hrt.h>
#include <circuit_breaker/circuit_breaker.h>
#include <mathlib/math/Limits.hpp>
#include <mathlib/math/Functions.hpp>
#include <lib/ecl/geo_lookup/geo_mag_declination.h>
using namespace matrix;
using namespace time_literals;
using math::radians;
MagCalibrator::MagCalibrator() :
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default),
_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
{
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);
}
}
MagCalibrator::~MagCalibrator()
{
perf_free(_loop_perf);
}
bool MagCalibrator::init()
{
ScheduleOnInterval(10_ms); // 100 Hz
return true;
}
void MagCalibrator::Run()
{
if (should_exit()) {
//_vehicle_angular_velocity_sub.unregisterCallback();
exit_and_cleanup();
return;
}
perf_begin(_loop_perf);
// Check if parameters have changed
if (_parameter_update_sub.updated()) {
// clear update
parameter_update_s param_update;
_parameter_update_sub.copy(&param_update);
updateParams();
//parameters_updated();
}
if (_vehicle_gps_position_sub.updated()) {
vehicle_gps_position_s gps;
if (_vehicle_gps_position_sub.copy(&gps)) {
if ((gps.fix_type >= 2) && (gps.eph < 1000)) {
const double lat = gps.lat / 1.e7;
const double lon = gps.lon / 1.e7;
// set the magnetic field data returned by the geo library using the current GPS position
_mag_declination_gps = get_mag_declination_radians(lat, lon);
_mag_inclination_gps = get_mag_inclination_radians(lat, lon);
const float mag_strength_gps = get_mag_strength_gauss(lat, lon);
_mag_earth_pred = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps)) * Vector3f(_mag_strength_gps, 0, 0);
// || (fabsf(mag_strength_gps - _mag_strength_gps) > 0.01f)
if (!_mag_earth_available) {
for (int mag_instance = 0; mag_instance < MAX_SENSOR_COUNT; mag_instance++) {
TRICAL_norm_set(&_trical_instance[mag_instance], mag_strength_gps);
}
}
_mag_strength_gps = mag_strength_gps;
_mag_earth_available = true;
}
}
}
if (_mag_earth_available) {
vehicle_attitude_s attitude;
for (int mag_instance = 0; mag_instance < MAX_SENSOR_COUNT; mag_instance++) {
sensor_mag_s mag;
while (_sensor_mag_subs[mag_instance].update(&mag)) {
_vehicle_attitude_sub.update(&attitude);
if (labs(mag.timestamp - attitude.timestamp) < 20_ms) {
_initialized[mag_instance] = true;
// expected earth field
const Vector3f expected_field_v = Dcmf(Quatf{attitude.q}).transpose() * _mag_earth_pred;
float sensor_reading[3] {mag.x, mag.y, mag.z};
float expected_field[3];
expected_field_v.copyTo(expected_field);
TRICAL_estimate_update(&_trical_instance[mag_instance], sensor_reading, expected_field);
// Calibrates `measurement` based on the current calibration estimates, and copies the result to `calibrated_measurement`.
float calibrated_reading[3];
TRICAL_measurement_calibrate(&_trical_instance[mag_instance], sensor_reading, calibrated_reading);
estimator_mag_cal_s mag_cal{};
mag_cal.instance = mag_instance;
mag_cal.device_id = mag.device_id;
mag_cal.sample[0] = mag.x;
mag_cal.sample[1] = mag.y;
mag_cal.sample[2] = mag.z;
expected_field_v.copyTo(mag_cal.expected_field);
mag_cal.calibrated_sample[0] = calibrated_reading[0];
mag_cal.calibrated_sample[1] = calibrated_reading[1];
mag_cal.calibrated_sample[2] = calibrated_reading[2];
mag_cal.calibrated_sample_magnitude = Vector3f(calibrated_reading).norm();
mag_cal.declination = _mag_declination_gps;
mag_cal.inclination = _mag_inclination_gps;
mag_cal.mag_strength = _mag_strength_gps;
// simple error metric
mag_cal.error = Vector3f(expected_field_v - Vector3f{calibrated_reading}).norm();
TRICAL_estimate_get_ext(&_trical_instance[mag_instance], mag_cal.bias_estimate, mag_cal.scale_estimate,
mag_cal.bias_estimate_variance, mag_cal.scale_estimate_variance);
// 0 1 2
// 3 4 5
// 6 7 8
SquareMatrix3f I3;
I3.setIdentity();
SquareMatrix3f D = SquareMatrix3f{I3 + SquareMatrix3f{mag_cal.scale_estimate}};
D.makeBlockSymmetric<3>(0);
mag_cal.diagonal_scale[0] = D(0, 0);
mag_cal.diagonal_scale[1] = D(1, 1);
mag_cal.diagonal_scale[2] = D(2, 2);
mag_cal.offdiagonal_scale[0] = (D(0, 1) + D(1, 0)) / 2;
mag_cal.offdiagonal_scale[1] = (D(0, 2) + D(2, 0)) / 2;
mag_cal.offdiagonal_scale[2] = (D(1, 2) + D(2, 1)) / 2;
// TODO:
// B' = (I_{3x3} + D)B - b
// D*(B+b) = D*B + D^-1*b
const Vector3f offsets = D.I() * Vector3f{mag_cal.bias_estimate};
offsets.copyTo(mag_cal.offsets);
mag_cal.timestamp = hrt_absolute_time();
_estimator_mag_cal_pub[mag_instance].publish(mag_cal);
// update calibration
//_calibration.set_offsets(offsets);
}
}
}
}
perf_end(_loop_perf);
}
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");
}
}
return 0;
}
int MagCalibrator::task_spawn(int argc, char *argv[])
{
MagCalibrator *instance = new MagCalibrator();
if (instance) {
_object.store(instance);
_task_id = task_id_is_work_queue;
if (instance->init()) {
return PX4_OK;
}
} else {
PX4_ERR("alloc failed");
}
delete instance;
_object.store(nullptr);
_task_id = -1;
return PX4_ERROR;
}
int MagCalibrator::custom_command(int argc, char *argv[])
{
return print_usage("unknown command");
}
int MagCalibrator::print_usage(const char *reason)
{
if (reason) {
PX4_WARN("%s\n", reason);
}
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("mag_calibrator", "controller");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0;
}
extern "C" __EXPORT int mag_calibrator_main(int argc, char *argv[])
{
return MagCalibrator::main(argc, argv);
}

View File

@ -0,0 +1,107 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include "TRICAL.h"
#include <lib/sensor_calibration/Magnetometer.hpp>
#include <lib/matrix/matrix/math.hpp>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/Publication.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/SubscriptionMultiArray.hpp>
#include <uORB/topics/estimator_mag_cal.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_mag.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_gps_position.h>
class MagCalibrator : public ModuleBase<MagCalibrator>, public ModuleParams, public px4::ScheduledWorkItem
{
public:
MagCalibrator();
~MagCalibrator() override;
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
/** @see ModuleBase */
static int custom_command(int argc, char *argv[]);
/** @see ModuleBase */
static int print_usage(const char *reason = nullptr);
bool init();
/** @see ModuleBase::print_status() */
int print_status() override;
private:
void Run() override;
static constexpr int MAX_SENSOR_COUNT = 4;
TRICAL_instance_t _trical_instance[MAX_SENSOR_COUNT];
bool _initialized[MAX_SENSOR_COUNT] {};
bool _mag_earth_available{false};
float _mag_declination_gps{0.f}; // magnetic declination returned by the geo library using the last valid GPS position (rad)
float _mag_inclination_gps{0.f}; // magnetic inclination returned by the geo library using the last valid GPS position (rad)
float _mag_strength_gps{0.f}; // magnetic strength returned by the geo library using the last valid GPS position (T)
matrix::Vector3f _mag_earth_pred{};
calibration::Magnetometer _calibration[MAX_SENSOR_COUNT];
uORB::PublicationMulti<estimator_mag_cal_s> _estimator_mag_cal_pub[MAX_SENSOR_COUNT] {
ORB_ID(estimator_mag_cal), ORB_ID(estimator_mag_cal), ORB_ID(estimator_mag_cal), ORB_ID(estimator_mag_cal)
};
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::SubscriptionMultiArray<sensor_mag_s, MAX_SENSOR_COUNT> _sensor_mag_subs{ORB_ID::sensor_mag};
perf_counter_t _loop_perf; /**< loop duration performance counter */
};

View File

@ -0,0 +1,237 @@
/*
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.
*/
#include <assert.h>
#include <math.h>
#include <float.h>
#include <string.h>
#include "TRICAL.h"
#include "filter.h"
/*
TRICAL_init:
Initializes `instance`. Must be called prior to any other TRICAL procedures
taking `instance` as a parameter.
If called on an instance that has already been initialized, TRICAL_init will
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;
instance->measurement_noise = 1e-6f;
/*
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)) {
instance->state_covariance[i] = 1e-2f;
}
}
/*
TRICAL_reset:
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));
/*
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)) {
instance->state_covariance[i] = 1e-2f;
}
}
/*
TRICAL_norm_set:
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;
}
/*
TRICAL_norm_get:
Returns the expected field norm (magnitude) of `instance`.
*/
float TRICAL_norm_get(TRICAL_instance_t *instance)
{
assert(instance);
return instance->field_norm;
}
/*
TRICAL_noise_set:
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;
}
/*
TRICAL_noise_get:
Returns the standard deviation in measurements supplied to `instance`.
*/
float TRICAL_noise_get(TRICAL_instance_t *instance)
{
assert(instance);
return instance->measurement_noise;
}
/*
TRICAL_measurement_count_get:
Returns the number of measurements previously provided to `instance` via
TRICAL_estimate_update.
*/
unsigned int TRICAL_measurement_count_get(TRICAL_instance_t *instance)
{
assert(instance);
return instance->measurement_count;
}
/*
TRICAL_estimate_update
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])
{
assert(instance);
assert(measurement);
assert(reference_field);
_trical_filter_iterate(instance, measurement, reference_field);
instance->measurement_count++;
}
/*
TRICAL_estimate_get
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])
{
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));
/* Copy the scale estimate to the destination matrix */
memcpy(scale_estimate, &instance->state[3], 9 * sizeof(float));
}
/*
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])
{
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];
bias_estimate_variance[2] = instance->state_covariance[2 * 12 + 2];
/* Now copy scale estimate covariance. */
scale_estimate_variance[0] = instance->state_covariance[3 * 12 + 3];
scale_estimate_variance[1] = instance->state_covariance[4 * 12 + 4];
scale_estimate_variance[2] = instance->state_covariance[5 * 12 + 5];
scale_estimate_variance[3] = instance->state_covariance[6 * 12 + 6];
scale_estimate_variance[4] = instance->state_covariance[7 * 12 + 7];
scale_estimate_variance[5] = instance->state_covariance[8 * 12 + 8];
scale_estimate_variance[6] = instance->state_covariance[9 * 12 + 9];
scale_estimate_variance[7] = instance->state_covariance[10 * 12 + 10];
scale_estimate_variance[8] = instance->state_covariance[11 * 12 + 11];
}
/*
TRICAL_measurement_calibrate
Calibrates `measurement` based on the current calibration estimates, and
copies the result to `calibrated_measurement`. The `measurement` and
`calibrated_measurement` parameters may be pointers to the same vector.
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])
{
assert(instance);
assert(measurement);
assert(calibrated_measurement);
/* Pass off to the internal function */
_trical_measurement_calibrate(instance->state, measurement,
calibrated_measurement);
}

View File

@ -0,0 +1,132 @@
/*
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 _TRICAL_H_
#define _TRICAL_H_
#ifdef __cplusplus
extern "C" {
#endif
#define TRICAL_STATE_DIM 12
typedef struct {
float field_norm;
float measurement_noise;
float state[TRICAL_STATE_DIM];
float state_covariance[TRICAL_STATE_DIM * TRICAL_STATE_DIM];
unsigned int measurement_count;
} TRICAL_instance_t;
/*
TRICAL_init:
Initializes `instance`. Must be called prior to any other TRICAL procedures
taking `instance` as a parameter.
If called on an instance that has already been initialized, TRICAL_init will
reset that instance to its default state.
*/
void TRICAL_init(TRICAL_instance_t *instance);
/*
TRICAL_reset:
Resets the state and state covariance of `instance`.
*/
void TRICAL_reset(TRICAL_instance_t *instance);
/*
TRICAL_norm_set:
Sets the expected field norm (magnitude) of `instance` to `norm`. If `norm`
differs from the instance's current field norm, all estimates are multiplied
by the ratio of new norm:old norm.
*/
void TRICAL_norm_set(TRICAL_instance_t *instance, float norm);
/*
TRICAL_norm_get:
Returns the expected field norm (magnitude) of `instance`.
*/
float TRICAL_norm_get(TRICAL_instance_t *instance);
/*
TRICAL_noise_set:
Sets the standard deviation in measurement supplied to `instance` to `noise`.
*/
void TRICAL_noise_set(TRICAL_instance_t *instance, float noise);
/*
TRICAL_noise_get:
Returns the standard deviation in measurements supplied to `instance`.
*/
float TRICAL_noise_get(TRICAL_instance_t *instance);
/*
TRICAL_measurement_count_get:
Returns the number of measurements previously provided to `instance` via
TRICAL_estimate_update.
*/
unsigned int TRICAL_measurement_count_get(TRICAL_instance_t *instance);
/*
TRICAL_estimate_update
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]);
/*
TRICAL_estimate_get
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 *instance, float bias_estimate[3],
float scale_estimate[9]);
/*
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 *instance,
float bias_estimate[3], float scale_estimate[9],
float bias_estimate_variance[3], float scale_estimate_variance[9]);
/*
TRICAL_measurement_calibrate
Calibrates `measurement` based on the current calibration estimates, and
copies the result to `calibrated_measurement`.
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 *instance,
float measurement[3], float calibrated_measurement[3]);
#ifdef __cplusplus
}
#endif
#endif

View File

@ -0,0 +1,346 @@
/*
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.
*/
#include <assert.h>
#include <math.h>
#include <float.h>
#include <string.h>
#include "TRICAL.h"
#include "filter.h"
#include "3dmath.h"
#ifdef DEBUG
#include <stdio.h>
static void _print_matrix(const char *label, float mat[], size_t rows,
size_t cols);
static void _print_matrix(const char *label, float mat[], size_t rows,
size_t cols)
{
printf("%s", label);
for (size_t i = 0; i < cols; i++) {
for (size_t j = 0; j < rows; j++) {
printf("%12.6f ", mat[j * cols + i]);
}
printf("\n");
}
}
#else
#define _print_matrix(a, b, c, d)
#endif
/*
A bit about the UKF formulation in this file: main references are
[1]: http://www.acsu.buffalo.edu/~johnc/mag_cal05.pdf
[2]: http://malcolmdshuster.com/Pub_2002c_J_scale_scan.pdf
Since we don't know the attitude matrix, we use scalar values for measurements
and scalar measurement noise.
The relevant equations are (2a) and (2b) in [2]. Basically, the measurement
value is:
z = -Bt x (2D + D^2) x B + 2Bt x (I + D) x b - |b|^2
where B is the 3-axis measurement vector, and H is the reference field (making
|H| the norm of the reference field, which we know). Here, t is used as the
transpose operator.
The measurement noise is a scalar:
v = 2[(I + D) x B - b]t x E - |E|^2
where B is the 3-axis measurement vector, b is the 3-axis bias estimate,
E is the measurement noise, I is the 3x3 identity matrix, and D is the 3x3
scale estimate.
The implementation follows the general approach of https://github.com/sfwa/ukf
however there a number of simplifications due to the restricted problem
domain.
Since there is no process model, the apriori mean is the same as the state at
the start of the update step, and W' is just the set of sigma points. That in
turn means that there's no need to calculate propagated state covariance from
W'; it's the same as the covariance was at the start of the update step.
The scalar measurement model also simplifies a lot of the cross-correlation
and Kalman gain calculation.
Like the full SFWA UKF, and unlike the papers upon which this approach is
based, we're actually using the scaled unscented Kalman filter. The main
differences are in the choices of scaling parameters, including alpha, beta
and kappa. Given the relatively small dimensionality of the filter it's
probably not strictly necessary to use the scaled formulation, but we'd
already implemented that so it seemed easier to continue with that approach.
*/
/*
_trical_measurement_reduce
Reduces `measurement` to a scalar value based on the calibration estimate in
`state` and the current field direction estimate `field` (if no absolute
orientation calibration is required, the same vector can be supplied for
`measurement` and `field`).
FIXME: It's not clear to me why the measurement model should be as in (2a)
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 temp[3];
_trical_measurement_calibrate(state, measurement, temp);
return sqrtf(fabsf(temp[X] * field[X] + temp[Y] * field[Y] + temp[Z] * field[Z]));
}
/*
_trical_measurement_calibrate
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.
Implements
B' = (I_{3x3} + D)B - b
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])
{
assert(state && measurement && calibrated_measurement);
float v[3];
float *restrict s = state;
float *restrict c = calibrated_measurement;
v[0] = measurement[0] - s[0];
v[1] = measurement[1] - s[1];
v[2] = measurement[2] - s[2];
/* 3x3 matrix multiply */
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);
}
/*
_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])
{
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);
_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;
/*
Handle central sigma point -- process the measurement based on the current
state vector
*/
measurement_estimates[0] = _trical_measurement_reduce(state, measurement,
field);
__asm__ __volatile__("");
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__("");
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);
/* 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);
/* Calculate the measurement estimate sum as we go */
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;
/*
Convert estimates to deviation from mean (so measurement_estimates
effectively becomes Z').
While we're at it, calculate the measurement estimate covariance (which
is a scalar quantity).
*/
float measurement_estimate_covariance = 0.0;
__asm__ __volatile__("");
for (i = 0; i < TRICAL_NUM_SIGMA; i++) {
measurement_estimates[i] -= measurement_estimate_mean;
temp = measurement_estimates[i] * measurement_estimates[i];
measurement_estimate_covariance += temp;
}
_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));
/*
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;
/* 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]);
cross_correlation[j] += temp;
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++) {
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);
/*
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__("");
for (i = 0; i < TRICAL_STATE_DIM; i++) {
kalman_gain = cross_correlation[i] * temp;
state[i] += kalman_gain * innovation;
}
_print_matrix("State:\n", state, 1, TRICAL_STATE_DIM);
/*
Update the state covariance:
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
covariance.
And, of course, since kalman gain is cross correlation *
(1 / measurement estimate covariance), and we multiply by measurement
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];
}
}
_print_matrix("State covariance:\n", covariance, TRICAL_STATE_DIM,
TRICAL_STATE_DIM);
}

View File

@ -0,0 +1,78 @@
/*
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 _FILTER_H_
#define _FILTER_H_
#ifdef __cplusplus
extern "C" {
#endif
/* Unscented Kalman filter sigma point and scaling parameters. */
#define TRICAL_NUM_SIGMA (2 * TRICAL_STATE_DIM + 1)
#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_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_WMI (1.0f / (2.0f * TRICAL_DIM_PLUS_LAMBDA))
#define TRICAL_SIGMA_WCI (TRICAL_SIGMA_WMI)
/* Internal function prototypes */
/*
_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]);
/*
_trical_measurement_calibrate
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]);
/*
_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]);
#ifdef __cplusplus
}
#endif
#endif

View File

@ -0,0 +1,49 @@
CMAKE_MINIMUM_REQUIRED(VERSION 2.8.7 FATAL_ERROR)
PROJECT(unittest)
# Enable ExternalProject CMake module
INCLUDE(ExternalProject)
set(CMAKE_CXX_FLAGS "-O3 -mssse3 -oaddmul")
set(CMAKE_C_FLAGS "-O3 -mssse3 -oaddmul-Weverything -Werror -Wno-documentation -Wno-padded -Wno-unknown-pragmas")
# Set default ExternalProject root directory
SET_DIRECTORY_PROPERTIES(PROPERTIES EP_PREFIX .)
# Add googletest
ExternalProject_Add(googletest
SVN_REPOSITORY http://googletest.googlecode.com/svn/trunk/
TIMEOUT 30
# Disable install step
INSTALL_COMMAND ""
# Wrap download, configure and build steps in a script to log output
LOG_DOWNLOAD ON
LOG_CONFIGURE ON
LOG_BUILD ON)
# Specify googletest include dir
ExternalProject_Get_Property(googletest source_dir)
SET(googletest_dir ${source_dir})
INCLUDE_DIRECTORIES(${googletest_dir}/include ../)
# Add test executable target
ADD_EXECUTABLE(unittest
../src/filter.c
../src/TRICAL.c
test_TRICAL.cpp
test_3dmath.cpp
test_filter.cpp)
# Create dependency of test on googletest
ADD_DEPENDENCIES(unittest googletest)
# Specify test's link libraries
ExternalProject_Get_Property(googletest binary_dir)
TARGET_LINK_LIBRARIES(unittest
${binary_dir}/${CMAKE_FIND_LIBRARY_PREFIXES}gtest.a
${binary_dir}/${CMAKE_FIND_LIBRARY_PREFIXES}gtest_main.a
pthread)
ADD_TEST(unittest unittest)
ADD_CUSTOM_TARGET(check COMMAND ${CMAKE_CTEST_COMMAND} DEPENDS unittest)

View File

@ -0,0 +1,65 @@
/*
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.
*/
#include <gtest/gtest.h>
#include <cmath>
#include <cfloat>
/* C++ complains about the C99 'restrict' qualifier. Just ignore it. */
#define restrict
#include "TRICAL.h"
#include "3dmath.h"
/*
Test LLT decomposition of a square positive definite matrix via the Cholesky
method.
The expected output is a lower-triangle matrix in column-major format.
*/
TEST(MatrixMath, CholeskyLLT)
{
float m1[16] = {
18, 22, 54, 42,
22, 70, 86, 62,
54, 86, 174, 134,
42, 62, 134, 106
};
float expected[16] = {
4.24264, 5.18545, 12.72792, 9.8994951,
0, 6.5659051, 3.0460384, 1.6245539,
0, 0, 1.6497422, 1.8497111,
0, 0, 0, 1.3926213
};
matrix_cholesky_decomp_scale_f(4, m1, m1, 1.0);
EXPECT_NEAR(expected[0], m1[0], 1e-5);
EXPECT_NEAR(expected[1], m1[1], 1e-5);
EXPECT_NEAR(expected[2], m1[2], 1e-5);
EXPECT_NEAR(expected[3], m1[3], 1e-5);
EXPECT_NEAR(expected[5], m1[5], 1e-5);
EXPECT_NEAR(expected[6], m1[6], 1e-5);
EXPECT_NEAR(expected[7], m1[7], 1e-5);
EXPECT_NEAR(expected[10], m1[10], 1e-5);
EXPECT_NEAR(expected[11], m1[11], 1e-5);
EXPECT_NEAR(expected[15], m1[15], 1e-5);
}

View File

@ -0,0 +1,353 @@
/*
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.
*/
#include <gtest/gtest.h>
/* C++ complains about the C99 'restrict' qualifier. Just ignore it. */
#define restrict
#include "TRICAL.h"
TEST(TRICAL, Initialisation)
{
TRICAL_instance_t cal;
TRICAL_init(&cal);
EXPECT_FLOAT_EQ(1.0f, TRICAL_norm_get(&cal));
}
/* Check that the field norm can be set and read */
TEST(TRICAL, NormGetSet)
{
TRICAL_instance_t cal;
TRICAL_init(&cal);
TRICAL_norm_set(&cal, 2.0f);
EXPECT_FLOAT_EQ(2.0f, TRICAL_norm_get(&cal));
}
/* Check that the measurement noise can be set and read */
TEST(TRICAL, NoiseGetSet)
{
TRICAL_instance_t cal;
TRICAL_init(&cal);
TRICAL_noise_set(&cal, 2.0f);
EXPECT_FLOAT_EQ(2.0f, TRICAL_noise_get(&cal));
}
/* Check that the measurement count can be read */
TEST(TRICAL, MeasurementCountGet)
{
TRICAL_instance_t cal;
TRICAL_init(&cal);
cal.measurement_count = 15;
EXPECT_EQ(15, TRICAL_measurement_count_get(&cal));
}
/*
Check that estimate update works with a series of measurements that should
result in the identity matrix with zero bias
*/
TEST(TRICAL, EstimateUpdateIdentity)
{
TRICAL_instance_t cal;
TRICAL_init(&cal);
float measurement[3];
unsigned int i;
for (i = 0; i < 100; i++) {
measurement[0] = 1.0;
measurement[1] = 0.0;
measurement[2] = 0.0;
TRICAL_estimate_update(&cal, measurement, measurement);
measurement[0] = 0.0;
measurement[1] = 1.0;
measurement[2] = 0.0;
TRICAL_estimate_update(&cal, measurement, measurement);
measurement[0] = 0.0;
measurement[1] = 0.0;
measurement[2] = 1.0;
TRICAL_estimate_update(&cal, measurement, measurement);
measurement[0] = -1.0;
measurement[1] = 0.0;
measurement[2] = 0.0;
TRICAL_estimate_update(&cal, measurement, measurement);
measurement[0] = 0.0;
measurement[1] = -1.0;
measurement[2] = 0.0;
TRICAL_estimate_update(&cal, measurement, measurement);
measurement[0] = 0.0;
measurement[1] = 0.0;
measurement[2] = -1.0;
TRICAL_estimate_update(&cal, measurement, measurement);
}
float bias_estimate[3], scale_estimate[9];
TRICAL_estimate_get(&cal, bias_estimate, scale_estimate);
EXPECT_NEAR(0.0, bias_estimate[0], 2e-2);
EXPECT_NEAR(0.0, bias_estimate[1], 2e-2);
EXPECT_NEAR(0.0, bias_estimate[2], 2e-2);
EXPECT_NEAR(0.0, scale_estimate[0], 2e-2);
EXPECT_NEAR(0.0, scale_estimate[1], 2e-2);
EXPECT_NEAR(0.0, scale_estimate[2], 2e-2);
EXPECT_NEAR(0.0, scale_estimate[3], 2e-2);
EXPECT_NEAR(0.0, scale_estimate[4], 2e-2);
EXPECT_NEAR(0.0, scale_estimate[5], 2e-2);
EXPECT_NEAR(0.0, scale_estimate[6], 2e-2);
EXPECT_NEAR(0.0, scale_estimate[7], 2e-2);
EXPECT_NEAR(0.0, scale_estimate[8], 2e-2);
}
/*
Check that estimate update works with a series of measurements that should
result in the identity matrix with a bias in the positive X axis
*/
TEST(TRICAL, EstimateUpdateBias)
{
TRICAL_instance_t cal;
TRICAL_init(&cal);
float measurement[3], ref[3];
unsigned int i;
for (i = 0; i < 200; i++) {
measurement[0] = 2.0;
measurement[1] = 0.0;
measurement[2] = 0.0;
ref[0] = 1.0;
ref[1] = 0.0;
ref[2] = 0.0;
TRICAL_estimate_update(&cal, measurement, ref);
measurement[0] = 1.0;
measurement[1] = 1.0;
measurement[2] = 0.0;
ref[0] = 0.0;
ref[1] = 1.0;
ref[2] = 0.0;
TRICAL_estimate_update(&cal, measurement, ref);
measurement[0] = 1.0;
measurement[1] = 0.0;
measurement[2] = 1.0;
ref[0] = 0.0;
ref[1] = 0.0;
ref[2] = 1.0;
TRICAL_estimate_update(&cal, measurement, ref);
measurement[0] = 0.0;
measurement[1] = 0.0;
measurement[2] = 0.0;
ref[0] = -1.0;
ref[1] = 0.0;
ref[2] = 0.0;
TRICAL_estimate_update(&cal, measurement, ref);
measurement[0] = 1.0;
measurement[1] = -1.0;
measurement[2] = 0.0;
ref[0] = 0.0;
ref[1] = -1.0;
ref[2] = 0.0;
TRICAL_estimate_update(&cal, measurement, ref);
measurement[0] = 1.0;
measurement[1] = 0.0;
measurement[2] = -1.0;
ref[0] = 0.0;
ref[1] = 0.0;
ref[2] = -1.0;
TRICAL_estimate_update(&cal, measurement, ref);
}
float bias_estimate[3], scale_estimate[9];
TRICAL_estimate_get(&cal, bias_estimate, scale_estimate);
EXPECT_NEAR(1.0, bias_estimate[0], 2e-2);
EXPECT_NEAR(0.0, bias_estimate[1], 2e-2);
EXPECT_NEAR(0.0, bias_estimate[2], 2e-2);
EXPECT_NEAR(0.0, scale_estimate[0], 2e-2);
EXPECT_NEAR(0.0, scale_estimate[1], 2e-2);
EXPECT_NEAR(0.0, scale_estimate[2], 2e-2);
EXPECT_NEAR(0.0, scale_estimate[3], 2e-2);
EXPECT_NEAR(0.0, scale_estimate[4], 2e-2);
EXPECT_NEAR(0.0, scale_estimate[5], 2e-2);
EXPECT_NEAR(0.0, scale_estimate[6], 2e-2);
EXPECT_NEAR(0.0, scale_estimate[7], 2e-2);
EXPECT_NEAR(0.0, scale_estimate[8], 2e-2);
}
/* Check that state estimates can be accessed */
TEST(TRICAL, EstimateGet)
{
TRICAL_instance_t cal;
TRICAL_init(&cal);
cal.state[0] = 3.0;
cal.state[1] = 2.0;
cal.state[2] = 1.0;
cal.state[3] = 1.0;
cal.state[4] = 0.5;
cal.state[5] = 0.1;
cal.state[6] = 0.5;
cal.state[7] = 1.0;
cal.state[8] = 0.5;
cal.state[9] = 0.1;
cal.state[10] = 0.5;
cal.state[11] = 1.0;
float bias_estimate[3], scale_estimate[9];
TRICAL_estimate_get(&cal, bias_estimate, scale_estimate);
EXPECT_FLOAT_EQ(3.0, bias_estimate[0]);
EXPECT_FLOAT_EQ(2.0, bias_estimate[1]);
EXPECT_FLOAT_EQ(1.0, bias_estimate[2]);
EXPECT_FLOAT_EQ(1.0, scale_estimate[0]);
EXPECT_FLOAT_EQ(0.5, scale_estimate[1]);
EXPECT_FLOAT_EQ(0.1, scale_estimate[2]);
EXPECT_FLOAT_EQ(0.5, scale_estimate[3]);
EXPECT_FLOAT_EQ(1.0, scale_estimate[4]);
EXPECT_FLOAT_EQ(0.5, scale_estimate[5]);
EXPECT_FLOAT_EQ(0.1, scale_estimate[6]);
EXPECT_FLOAT_EQ(0.5, scale_estimate[7]);
EXPECT_FLOAT_EQ(1.0, scale_estimate[8]);
}
/* Check that state and covariance estimates can be accessed */
TEST(TRICAL, EstimateGetExt)
{
TRICAL_instance_t cal;
TRICAL_init(&cal);
cal.state[0] = 3.0;
cal.state[1] = 2.0;
cal.state[2] = 1.0;
cal.state[3] = 1.0;
cal.state[4] = 0.5;
cal.state[5] = 0.1;
cal.state[6] = 0.5;
cal.state[7] = 1.0;
cal.state[8] = 0.5;
cal.state[9] = 0.1;
cal.state[10] = 0.5;
cal.state[11] = 1.0;
cal.state_covariance[0] = 10.0;
cal.state_covariance[13] = 20.0;
cal.state_covariance[26] = 30.0;
cal.state_covariance[39] = 40.0;
cal.state_covariance[52] = 50.0;
cal.state_covariance[65] = 60.0;
cal.state_covariance[78] = 70.0;
cal.state_covariance[91] = 80.0;
cal.state_covariance[104] = 90.0;
cal.state_covariance[117] = 100.0;
cal.state_covariance[130] = 110.0;
cal.state_covariance[143] = 120.0;
float bias_estimate[3], scale_estimate[9], bias_estimate_variance[3],
scale_estimate_variance[9];
TRICAL_estimate_get_ext(&cal, bias_estimate, scale_estimate,
bias_estimate_variance, scale_estimate_variance);
EXPECT_FLOAT_EQ(3.0, bias_estimate[0]);
EXPECT_FLOAT_EQ(2.0, bias_estimate[1]);
EXPECT_FLOAT_EQ(1.0, bias_estimate[2]);
EXPECT_FLOAT_EQ(1.0, scale_estimate[0]);
EXPECT_FLOAT_EQ(0.5, scale_estimate[1]);
EXPECT_FLOAT_EQ(0.1, scale_estimate[2]);
EXPECT_FLOAT_EQ(0.5, scale_estimate[3]);
EXPECT_FLOAT_EQ(1.0, scale_estimate[4]);
EXPECT_FLOAT_EQ(0.5, scale_estimate[5]);
EXPECT_FLOAT_EQ(0.1, scale_estimate[6]);
EXPECT_FLOAT_EQ(0.5, scale_estimate[7]);
EXPECT_FLOAT_EQ(1.0, scale_estimate[8]);
EXPECT_FLOAT_EQ(10.0, bias_estimate_variance[0]);
EXPECT_FLOAT_EQ(20.0, bias_estimate_variance[1]);
EXPECT_FLOAT_EQ(30.0, bias_estimate_variance[2]);
EXPECT_FLOAT_EQ(40.0, scale_estimate_variance[0]);
EXPECT_FLOAT_EQ(50.0, scale_estimate_variance[1]);
EXPECT_FLOAT_EQ(60.0, scale_estimate_variance[2]);
EXPECT_FLOAT_EQ(70.0, scale_estimate_variance[3]);
EXPECT_FLOAT_EQ(80.0, scale_estimate_variance[4]);
EXPECT_FLOAT_EQ(90.0, scale_estimate_variance[5]);
EXPECT_FLOAT_EQ(100.0, scale_estimate_variance[6]);
EXPECT_FLOAT_EQ(110.0, scale_estimate_variance[7]);
EXPECT_FLOAT_EQ(120.0, scale_estimate_variance[8]);
}
/*
Check that measurement calibration can be performed based on the instance
state.
This mirrors Filter.CalibrateFull.
*/
TEST(TRICAL, Calibrate)
{
TRICAL_instance_t cal;
TRICAL_init(&cal);
cal.state[0] = 3.0;
cal.state[1] = 2.0;
cal.state[2] = 1.0;
cal.state[3] = 1.0;
cal.state[4] = 0.5;
cal.state[5] = 0.1;
cal.state[6] = 0.5;
cal.state[7] = 1.0;
cal.state[8] = 0.5;
cal.state[9] = 0.1;
cal.state[10] = 0.5;
cal.state[11] = 1.0;
float measurement[3] = { 1.0, 2.0, 3.0 }, result[3];
TRICAL_measurement_calibrate(&cal, measurement, result);
EXPECT_FLOAT_EQ(-3.8, result[0]);
EXPECT_FLOAT_EQ(0.0, result[1]);
EXPECT_FLOAT_EQ(3.8, result[2]);
}

View File

@ -0,0 +1,165 @@
/*
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.
*/
#include <gtest/gtest.h>
/* C++ complains about the C99 'restrict' qualifier. Just ignore it. */
#define restrict
#include "TRICAL.h"
#include "filter.h"
/*
Test measurement calibration with the identity matrix -- all state values are
zero because the identity matrix is added to the symmetric scale factor matrix
during measurement calibration.
*/
TEST(Filter, CalibrateIdentity)
{
float state[] = {
0.0, 0.0, 0.0,
0.0, 0.0, 0.0,
0.0, 0.0, 0.0,
0.0, 0.0, 0.0
};
float measurement[] = { 1.0, 2.0, 3.0 }, result[3];
_trical_measurement_calibrate(state, measurement, result);
EXPECT_FLOAT_EQ(measurement[0], result[0]);
EXPECT_FLOAT_EQ(measurement[1], result[1]);
EXPECT_FLOAT_EQ(measurement[2], result[2]);
}
/*
Test measurement calibration with a bias, but the identity matrix for scale
factors.
This test would permit calibration for zero-point bias and hard iron
distortion only.
*/
TEST(Filter, CalibrateBias)
{
float state[] = {
1.0, 2.0, 3.0,
0.0, 0.0, 0.0,
0.0, 0.0, 0.0,
0.0, 0.0, 0.0
};
float measurement[] = { 1.0, 2.0, 3.0 }, result[3];
_trical_measurement_calibrate(state, measurement, result);
EXPECT_FLOAT_EQ(0.0, result[0]);
EXPECT_FLOAT_EQ(0.0, result[1]);
EXPECT_FLOAT_EQ(0.0, result[2]);
}
/*
Test measurement calibration with an orthogonal scale factor matrix -- this
is added to the identity matrix during the calculation, so the actual scale
factor matrix used will be:
2 0 0
0 2 0
0 0 2
This test would permit calibration for soft iron distortion of the magnetic
field, as well as scale factor error in the magnetometer.
*/
TEST(Filter, CalibrateOrthogonal)
{
float state[] = {
0.0, 0.0, 0.0,
1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0
};
float measurement[] = { 1.0, 2.0, 3.0 }, result[3];
_trical_measurement_calibrate(state, measurement, result);
EXPECT_FLOAT_EQ(2.0, result[0]);
EXPECT_FLOAT_EQ(4.0, result[1]);
EXPECT_FLOAT_EQ(6.0, result[2]);
}
/*
Test measurement calibration with a full symmetric scale factor matrix. This
test would permit calibration for soft iron distortion, as well as scale
factor error and axis misalignment in the sensor.
*/
TEST(Filter, CalibrateSymmetric)
{
float state[] = {
0.0, 0.0, 0.0,
1.0, 0.5, 0.1,
0.5, 1.0, 0.5,
0.1, 0.5, 1.0
};
float measurement[] = { 1.0, 2.0, 3.0 }, result[3];
_trical_measurement_calibrate(state, measurement, result);
EXPECT_FLOAT_EQ(3.3, result[0]);
EXPECT_FLOAT_EQ(6.0, result[1]);
EXPECT_FLOAT_EQ(7.1, result[2]);
}
/*
Test measurement calibration with a full symmetric scale factor matrix as well
as bias. This test would permit calibration for hard and soft iron distortion,
as well as bias, scale factor error and axis misalignment in the sensor.
(The general case, including body/sensor axis misalignment, would require
non-symmetric matrices as well as knowledge of the body attitude at each
calibration step, and is easier to do elsewhere -- e.g. by measurement.)
*/
TEST(Filter, CalibrateFull)
{
float state[] = {
3.0, 2.0, 1.0,
1.0, 0.5, 0.1,
0.5, 1.0, 0.5,
0.1, 0.5, 1.0
};
float measurement[] = { 1.0, 2.0, 3.0 }, result[3];
_trical_measurement_calibrate(state, measurement, result);
EXPECT_FLOAT_EQ(-3.8, result[0]);
EXPECT_FLOAT_EQ(0.0, result[1]);
EXPECT_FLOAT_EQ(3.8, result[2]);
}
/*
Now test measurement reduction, from a raw 3-axis sensor reading to a scalar
output based on the current calibration estimate. This is used in the UKF
implementation to generate a measurement estimate for each sigma point.
*/
TEST(Filter, MeasurementReduction)
{
float state[] = {
0.0, 0.0, 0.0,
1.0, 0.5, 0.1,
0.5, 1.0, 0.5,
0.1, 0.5, 1.0
};
float measurement[] = { 1.0, 2.0, 3.0 }, result;
result = _trical_measurement_reduce(state, measurement, measurement);
EXPECT_FLOAT_EQ(6.0497932, result);
}