forked from Archive/PX4-Autopilot
Compare commits
2 Commits
main
...
pr-mag_cal
Author | SHA1 | Date |
---|---|---|
Daniel Agar | d602cf0fd1 | |
Daniel Agar | 2c6d664a54 |
|
@ -214,6 +214,7 @@ battery_simulator start
|
||||||
tone_alarm start
|
tone_alarm start
|
||||||
rc_update start
|
rc_update start
|
||||||
sensors start
|
sensors start
|
||||||
|
mag_calibrator start
|
||||||
commander start
|
commander start
|
||||||
navigator start
|
navigator start
|
||||||
|
|
||||||
|
|
|
@ -139,3 +139,5 @@ fi
|
||||||
###############################################################################
|
###############################################################################
|
||||||
|
|
||||||
sensors start
|
sensors start
|
||||||
|
|
||||||
|
mag_calibrator start
|
||||||
|
|
|
@ -15,7 +15,7 @@ px4_add_board(
|
||||||
optical_flow/pmw3901
|
optical_flow/pmw3901
|
||||||
pwm_out
|
pwm_out
|
||||||
MODULES
|
MODULES
|
||||||
attitude_estimator_q
|
#attitude_estimator_q
|
||||||
#camera_feedback
|
#camera_feedback
|
||||||
commander
|
commander
|
||||||
dataman
|
dataman
|
||||||
|
@ -24,7 +24,7 @@ px4_add_board(
|
||||||
land_detector
|
land_detector
|
||||||
landing_target_estimator
|
landing_target_estimator
|
||||||
load_mon
|
load_mon
|
||||||
local_position_estimator
|
#local_position_estimator
|
||||||
logger
|
logger
|
||||||
mavlink
|
mavlink
|
||||||
mc_att_control
|
mc_att_control
|
||||||
|
@ -41,7 +41,7 @@ px4_add_board(
|
||||||
dumpfile
|
dumpfile
|
||||||
esc_calib
|
esc_calib
|
||||||
hardfault_log
|
hardfault_log
|
||||||
i2cdetect
|
#i2cdetect
|
||||||
led_control
|
led_control
|
||||||
mixer
|
mixer
|
||||||
motor_ramp
|
motor_ramp
|
||||||
|
|
|
@ -73,6 +73,7 @@ px4_add_board(
|
||||||
load_mon
|
load_mon
|
||||||
local_position_estimator
|
local_position_estimator
|
||||||
logger
|
logger
|
||||||
|
mag_calibrator
|
||||||
mavlink
|
mavlink
|
||||||
mc_att_control
|
mc_att_control
|
||||||
mc_hover_thrust_estimator
|
mc_hover_thrust_estimator
|
||||||
|
|
|
@ -78,6 +78,7 @@ px4_add_board(
|
||||||
load_mon
|
load_mon
|
||||||
local_position_estimator
|
local_position_estimator
|
||||||
logger
|
logger
|
||||||
|
mag_calibrator
|
||||||
mavlink
|
mavlink
|
||||||
mc_att_control
|
mc_att_control
|
||||||
mc_hover_thrust_estimator
|
mc_hover_thrust_estimator
|
||||||
|
|
|
@ -39,6 +39,7 @@ px4_add_board(
|
||||||
#load_mon
|
#load_mon
|
||||||
local_position_estimator
|
local_position_estimator
|
||||||
logger
|
logger
|
||||||
|
mag_calibrator
|
||||||
mavlink
|
mavlink
|
||||||
mc_att_control
|
mc_att_control
|
||||||
mc_hover_thrust_estimator
|
mc_hover_thrust_estimator
|
||||||
|
|
|
@ -38,6 +38,7 @@ px4_add_board(
|
||||||
#load_mon
|
#load_mon
|
||||||
local_position_estimator
|
local_position_estimator
|
||||||
logger
|
logger
|
||||||
|
mag_calibrator
|
||||||
mavlink
|
mavlink
|
||||||
mc_att_control
|
mc_att_control
|
||||||
mc_hover_thrust_estimator
|
mc_hover_thrust_estimator
|
||||||
|
|
|
@ -55,6 +55,7 @@ set(msg_files
|
||||||
ekf_gps_drift.msg
|
ekf_gps_drift.msg
|
||||||
esc_report.msg
|
esc_report.msg
|
||||||
esc_status.msg
|
esc_status.msg
|
||||||
|
estimator_mag_cal.msg
|
||||||
estimator_innovations.msg
|
estimator_innovations.msg
|
||||||
estimator_sensor_bias.msg
|
estimator_sensor_bias.msg
|
||||||
estimator_states.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 UART8{"wq:UART8", 1400, -25};
|
||||||
static constexpr wq_config_t UART_UNKNOWN{"wq:UART_UNKNOWN", 1400, -26};
|
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", 2700, -50};
|
||||||
|
|
||||||
static constexpr wq_config_t test1{"wq:test1", 2000, 0};
|
static constexpr wq_config_t test1{"wq:test1", 2000, 0};
|
||||||
static constexpr wq_config_t test2{"wq:test2", 2000, 0};
|
static constexpr wq_config_t test2{"wq:test2", 2000, 0};
|
||||||
|
|
|
@ -269,8 +269,8 @@ void Magnetometer::PrintStatus()
|
||||||
(double)_scale(0, 0), (double)_scale(1, 1), (double)_scale(2, 2));
|
(double)_scale(0, 0), (double)_scale(1, 1), (double)_scale(2, 2));
|
||||||
}
|
}
|
||||||
|
|
||||||
#if defined(DEBUG_BUILD)
|
#if defined(DEBUG_BUILD) || true
|
||||||
_scale.print()
|
_scale.print();
|
||||||
#endif // DEBUG_BUILD
|
#endif // DEBUG_BUILD
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -69,6 +69,7 @@ public:
|
||||||
void set_rotation(Rotation rotation);
|
void set_rotation(Rotation rotation);
|
||||||
|
|
||||||
uint8_t calibration_count() const { return _calibration_count; }
|
uint8_t calibration_count() const { return _calibration_count; }
|
||||||
|
int8_t calibration_index() const { return _calibration_index; }
|
||||||
uint32_t device_id() const { return _device_id; }
|
uint32_t device_id() const { return _device_id; }
|
||||||
bool enabled() const { return (_priority > 0); }
|
bool enabled() const { return (_priority > 0); }
|
||||||
bool external() const { return _external; }
|
bool external() const { return _external; }
|
||||||
|
|
|
@ -130,6 +130,8 @@ void LoggedTopics::add_default_topics()
|
||||||
add_topic_multi("vehicle_imu", 500, 4);
|
add_topic_multi("vehicle_imu", 500, 4);
|
||||||
add_topic_multi("vehicle_imu_status", 1000, 4);
|
add_topic_multi("vehicle_imu_status", 1000, 4);
|
||||||
|
|
||||||
|
add_topic_multi("estimator_mag_cal");
|
||||||
|
|
||||||
#ifdef CONFIG_ARCH_BOARD_PX4_SITL
|
#ifdef CONFIG_ARCH_BOARD_PX4_SITL
|
||||||
add_topic("actuator_controls_virtual_fw");
|
add_topic("actuator_controls_virtual_fw");
|
||||||
add_topic("actuator_controls_virtual_mc");
|
add_topic("actuator_controls_virtual_mc");
|
||||||
|
|
|
@ -0,0 +1,55 @@
|
||||||
|
############################################################################
|
||||||
|
#
|
||||||
|
# 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
|
||||||
|
filter.cpp
|
||||||
|
filter.h
|
||||||
|
TRICAL.cpp
|
||||||
|
TRICAL.h
|
||||||
|
DEPENDS
|
||||||
|
ecl_geo_lookup
|
||||||
|
mathlib
|
||||||
|
px4_work_queue
|
||||||
|
#UNITY_BUILD
|
||||||
|
)
|
|
@ -0,0 +1,300 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* 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)
|
||||||
|
{
|
||||||
|
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], 0.01f);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
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_status_sub.updated()) {
|
||||||
|
vehicle_status_s vehicle_status;
|
||||||
|
|
||||||
|
if (_vehicle_status_sub.copy(&vehicle_status)) {
|
||||||
|
_armed = vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (_vehicle_gps_position_sub.updated()) {
|
||||||
|
vehicle_gps_position_s gps;
|
||||||
|
|
||||||
|
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
|
||||||
|
if ((mag_cal.error < 0.20f) && (mag_cal.error < _calibration_error[mag_instance])) {
|
||||||
|
_calibration[mag_instance].set_device_id(mag.device_id, mag.is_external);
|
||||||
|
_calibration[mag_instance].set_offset(offsets);
|
||||||
|
_calibration[mag_instance].set_scale(Vector3f{mag_cal.diagonal_scale});
|
||||||
|
_calibration[mag_instance].set_offdiagonal(Vector3f{mag_cal.offdiagonal_scale});
|
||||||
|
|
||||||
|
if (!_armed && hrt_elapsed_time(&_last_calibration_save[mag_instance]) > 10_s) {
|
||||||
|
if (_calibration[mag_instance].calibration_index() < 0) {
|
||||||
|
_calibration[mag_instance].set_calibration_index(mag_instance);
|
||||||
|
}
|
||||||
|
|
||||||
|
_calibration[mag_instance].ParametersSave();
|
||||||
|
param_notify_changes();
|
||||||
|
|
||||||
|
_last_calibration_save[mag_instance] = hrt_absolute_time();
|
||||||
|
_calibration_error[mag_instance] = mag_cal.error;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
perf_end(_loop_perf);
|
||||||
|
}
|
||||||
|
|
||||||
|
int MagCalibrator::print_status()
|
||||||
|
{
|
||||||
|
for (int mag_instance = 0; mag_instance < MAX_SENSOR_COUNT; mag_instance++) {
|
||||||
|
|
||||||
|
if (_initialized[mag_instance]) {
|
||||||
|
_calibration[mag_instance].PrintStatus();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
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,114 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* 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>
|
||||||
|
#include <uORB/topics/vehicle_status.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::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
|
||||||
|
|
||||||
|
uORB::SubscriptionMultiArray<sensor_mag_s, MAX_SENSOR_COUNT> _sensor_mag_subs{ORB_ID::sensor_mag};
|
||||||
|
|
||||||
|
hrt_abstime _last_calibration_save[MAX_SENSOR_COUNT] {};
|
||||||
|
float _calibration_error[MAX_SENSOR_COUNT] {INFINITY, INFINITY, INFINITY, INFINITY};
|
||||||
|
|
||||||
|
bool _armed{false};
|
||||||
|
|
||||||
|
perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
|
||||||
|
};
|
|
@ -0,0 +1,187 @@
|
||||||
|
/*
|
||||||
|
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)
|
||||||
|
{
|
||||||
|
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
|
||||||
|
*/
|
||||||
|
for (unsigned 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)
|
||||||
|
{
|
||||||
|
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
|
||||||
|
*/
|
||||||
|
for (unsigned 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)
|
||||||
|
{
|
||||||
|
instance->field_norm = norm;
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
TRICAL_norm_get:
|
||||||
|
Returns the expected field norm (magnitude) of `instance`.
|
||||||
|
*/
|
||||||
|
float TRICAL_norm_get(TRICAL_instance_t *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)
|
||||||
|
{
|
||||||
|
instance->measurement_noise = noise;
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
TRICAL_noise_get:
|
||||||
|
Returns the standard deviation in measurements supplied to `instance`.
|
||||||
|
*/
|
||||||
|
float TRICAL_noise_get(TRICAL_instance_t *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)
|
||||||
|
{
|
||||||
|
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])
|
||||||
|
{
|
||||||
|
_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 *instance, float bias_estimate[3], float scale_estimate[9])
|
||||||
|
{
|
||||||
|
/* 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 *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);
|
||||||
|
|
||||||
|
/* 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 * instance, float measurement[3], float calibrated_measurement[3])
|
||||||
|
{
|
||||||
|
/* 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,337 @@
|
||||||
|
/*
|
||||||
|
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"
|
||||||
|
|
||||||
|
#define X 0
|
||||||
|
#define Y 1
|
||||||
|
#define Z 2
|
||||||
|
#define W 3
|
||||||
|
|
||||||
|
#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])
|
||||||
|
{
|
||||||
|
float v[3];
|
||||||
|
float *s = state;
|
||||||
|
|
||||||
|
v[0] = measurement[0] - s[0];
|
||||||
|
v[1] = measurement[1] - s[1];
|
||||||
|
v[2] = measurement[2] - s[2];
|
||||||
|
|
||||||
|
/* 3x3 matrix multiply */
|
||||||
|
float *c = calibrated_measurement;
|
||||||
|
c[0] = v[0] * (s[3] + 1.0f) + v[1] * s[4] + v[2] * s[5];
|
||||||
|
c[1] = v[0] * s[6] + v[1] * (s[7] + 1.0f) + v[2] * s[8];
|
||||||
|
c[2] = v[0] * s[9] + v[1] * s[10] + v[2] * (s[11] + 1.0f);
|
||||||
|
}
|
||||||
|
|
||||||
|
static void matrix_cholesky_decomp_scale_f(unsigned int dim, float L[], const float A[], const float mul)
|
||||||
|
{
|
||||||
|
/*
|
||||||
|
9x9:
|
||||||
|
900 mult
|
||||||
|
72 div
|
||||||
|
9 sqrt
|
||||||
|
*/
|
||||||
|
unsigned i;
|
||||||
|
unsigned in;
|
||||||
|
for (i = 0, in = 0; i < dim; i++, in += dim) {
|
||||||
|
|
||||||
|
if (i == 0) {
|
||||||
|
L[i + 0] = sqrtf(A[i + in] * mul);
|
||||||
|
} else {
|
||||||
|
L[i + 0] = (1.f / L[0]) * (A[i] * mul);
|
||||||
|
}
|
||||||
|
|
||||||
|
unsigned j;
|
||||||
|
unsigned jn;
|
||||||
|
for (j = 1, jn = dim; j <= i; j++, jn += dim) {
|
||||||
|
float s = 0;
|
||||||
|
|
||||||
|
for (unsigned kn = 0; kn < j * dim; kn += dim) {
|
||||||
|
s += L[i + kn] * L[j + kn];
|
||||||
|
}
|
||||||
|
|
||||||
|
if (i == j) {
|
||||||
|
L[i + jn] = sqrtf(A[i + in] * mul - s);
|
||||||
|
|
||||||
|
} else {
|
||||||
|
L[i + jn] = (1.f / (L[j + jn])) * (A[i + jn] * mul - s);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
_trical_filter_iterate
|
||||||
|
Generates a new calibration estimate for `instance` incorporating the raw
|
||||||
|
sensor readings in `measurement`.
|
||||||
|
*/
|
||||||
|
void _trical_filter_iterate(TRICAL_instance_t *instance, float measurement[3], float field[3])
|
||||||
|
{
|
||||||
|
/*
|
||||||
|
LLT decomposition on state covariance matrix, with result multiplied by
|
||||||
|
TRICAL_DIM_PLUS_LAMBDA
|
||||||
|
*/
|
||||||
|
float *covariance = instance->state_covariance;
|
||||||
|
float covariance_llt[TRICAL_STATE_DIM * TRICAL_STATE_DIM]{};
|
||||||
|
|
||||||
|
matrix_cholesky_decomp_scale_f(TRICAL_STATE_DIM, covariance_llt, covariance, TRICAL_DIM_PLUS_LAMBDA);
|
||||||
|
|
||||||
|
_print_matrix("LLT:\n", covariance_llt, TRICAL_STATE_DIM, TRICAL_STATE_DIM);
|
||||||
|
|
||||||
|
/*
|
||||||
|
Generate the sigma points, and use them as the basis of the measurement
|
||||||
|
estimates
|
||||||
|
*/
|
||||||
|
float temp_sigma[TRICAL_STATE_DIM]{};
|
||||||
|
float temp;
|
||||||
|
float measurement_estimates[TRICAL_NUM_SIGMA]{};
|
||||||
|
float measurement_estimate_mean = 0.0;
|
||||||
|
|
||||||
|
/*
|
||||||
|
Handle central sigma point -- process the measurement based on the current
|
||||||
|
state vector
|
||||||
|
*/
|
||||||
|
float *state = instance->state;
|
||||||
|
measurement_estimates[0] = _trical_measurement_reduce(state, measurement, field);
|
||||||
|
|
||||||
|
unsigned i;
|
||||||
|
unsigned col;
|
||||||
|
for (i = 0, col = 0; i < TRICAL_STATE_DIM; i++, col += TRICAL_STATE_DIM) {
|
||||||
|
/*
|
||||||
|
Handle the positive sigma point -- perturb the state vector based on
|
||||||
|
the current column of the covariance matrix, and process the
|
||||||
|
measurement based on the resulting state estimate
|
||||||
|
*/
|
||||||
|
int k;
|
||||||
|
int l;
|
||||||
|
for (k = col, l = 0; l < TRICAL_STATE_DIM; k++, l++) {
|
||||||
|
temp_sigma[l] = state[l] + covariance_llt[k];
|
||||||
|
}
|
||||||
|
|
||||||
|
measurement_estimates[i + 1] = _trical_measurement_reduce(temp_sigma, measurement, field);
|
||||||
|
|
||||||
|
/* Handle the negative sigma point -- mirror of the above */
|
||||||
|
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;
|
||||||
|
|
||||||
|
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]{};
|
||||||
|
|
||||||
|
// Calculate the innovation (difference between the expected value, i.e. the
|
||||||
|
// field norm, and the measurement estimate mean).
|
||||||
|
float innovation = instance->field_norm - measurement_estimate_mean;
|
||||||
|
|
||||||
|
/* Iterate over sigma points, two at a time */
|
||||||
|
for (i = 0; i < TRICAL_STATE_DIM; i++) {
|
||||||
|
/* Iterate over the cross-correlation matrix */
|
||||||
|
for (int j = 0; j < TRICAL_STATE_DIM; j++) {
|
||||||
|
// We're regenerating the sigma points as we go, so that we don't need to store W'
|
||||||
|
temp = measurement_estimates[i + 1] * (state[j] + covariance_llt[i * TRICAL_STATE_DIM + j]);
|
||||||
|
cross_correlation[j] += temp;
|
||||||
|
|
||||||
|
temp = measurement_estimates[i + 1 + TRICAL_STATE_DIM] * (state[j] - covariance_llt[i * TRICAL_STATE_DIM + j]);
|
||||||
|
cross_correlation[j] += temp;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Scale the results of the previous step, and add in the scaled central sigma point
|
||||||
|
for (int j = 0; j < TRICAL_STATE_DIM; j++) {
|
||||||
|
temp = TRICAL_SIGMA_WC0 * measurement_estimates[0] * state[j];
|
||||||
|
cross_correlation[j] = TRICAL_SIGMA_WCI * cross_correlation[j] + temp;
|
||||||
|
}
|
||||||
|
|
||||||
|
_print_matrix("Cross-correlation:\n", cross_correlation, 1, TRICAL_STATE_DIM);
|
||||||
|
|
||||||
|
// Update the state -- since the measurement is a scalar, we can calculate
|
||||||
|
// the Kalman gain and update in a single pass.
|
||||||
|
temp = 1.f / measurement_estimate_covariance;
|
||||||
|
|
||||||
|
for (i = 0; i < TRICAL_STATE_DIM; i++) {
|
||||||
|
float 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.
|
||||||
|
*/
|
||||||
|
for (i = 0; i < TRICAL_STATE_DIM; i++) {
|
||||||
|
temp = -cross_correlation[i];
|
||||||
|
|
||||||
|
for (int j = 0; j < TRICAL_STATE_DIM; j++) {
|
||||||
|
covariance[i * TRICAL_STATE_DIM + j] += temp * cross_correlation[j];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
_print_matrix("State covariance:\n", covariance, TRICAL_STATE_DIM, TRICAL_STATE_DIM);
|
||||||
|
}
|
|
@ -0,0 +1,73 @@
|
||||||
|
/*
|
||||||
|
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,64 @@
|
||||||
|
/*
|
||||||
|
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"
|
||||||
|
|
||||||
|
/*
|
||||||
|
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