forked from Archive/PX4-Autopilot
WIP: online mag cal using UKF
This commit is contained in:
Daniel Agar
parent
945c17bc3f
commit
2c6d664a54
|
|
@ -214,6 +214,7 @@ battery_simulator start
|
|||
tone_alarm start
|
||||
rc_update start
|
||||
sensors start
|
||||
mag_calibrator start
|
||||
commander start
|
||||
navigator start
|
||||
|
||||
|
|
|
|||
|
|
@ -139,3 +139,5 @@ fi
|
|||
###############################################################################
|
||||
|
||||
sensors start
|
||||
|
||||
mag_calibrator start
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -73,6 +73,7 @@ px4_add_board(
|
|||
load_mon
|
||||
local_position_estimator
|
||||
logger
|
||||
mag_calibrator
|
||||
mavlink
|
||||
mc_att_control
|
||||
mc_hover_thrust_estimator
|
||||
|
|
|
|||
|
|
@ -78,6 +78,7 @@ px4_add_board(
|
|||
load_mon
|
||||
local_position_estimator
|
||||
logger
|
||||
mag_calibrator
|
||||
mavlink
|
||||
mc_att_control
|
||||
mc_hover_thrust_estimator
|
||||
|
|
|
|||
|
|
@ -39,6 +39,7 @@ px4_add_board(
|
|||
#load_mon
|
||||
local_position_estimator
|
||||
logger
|
||||
mag_calibrator
|
||||
mavlink
|
||||
mc_att_control
|
||||
mc_hover_thrust_estimator
|
||||
|
|
|
|||
|
|
@ -38,6 +38,7 @@ px4_add_board(
|
|||
#load_mon
|
||||
local_position_estimator
|
||||
logger
|
||||
mag_calibrator
|
||||
mavlink
|
||||
mc_att_control
|
||||
mc_hover_thrust_estimator
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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};
|
||||
|
|
|
|||
|
|
@ -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");
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
)
|
||||
|
|
@ -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(¶m_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);
|
||||
}
|
||||
|
|
@ -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 */
|
||||
};
|
||||
|
|
@ -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);
|
||||
}
|
||||
|
|
@ -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
|
||||
|
|
@ -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);
|
||||
}
|
||||
|
|
@ -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
|
||||
|
|
@ -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)
|
||||
|
|
@ -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);
|
||||
}
|
||||
|
|
@ -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]);
|
||||
}
|
||||
|
|
@ -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);
|
||||
}
|
||||
Loading…
Reference in New Issue