mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-16 17:17:36 +08:00
EKF: fix code style
This commit is contained in:
bugobliterator
+1
-1
@@ -130,7 +130,7 @@ public:
|
||||
if (timestamp >= _buffer[index].time_us && timestamp - _buffer[index].time_us < 100000) {
|
||||
|
||||
// TODO Re-evaluate the static cast and usage patterns
|
||||
memcpy(static_cast<void*>(sample), static_cast<void*>(&_buffer[index]), sizeof(*sample));
|
||||
memcpy(static_cast<void *>(sample), static_cast<void *>(&_buffer[index]), sizeof(*sample));
|
||||
|
||||
// Now we can set the tail to the item which comes after the one we removed
|
||||
// since we don't want to have any older data in the buffer
|
||||
|
||||
+146
-145
@@ -39,168 +39,169 @@
|
||||
* @author Siddharth Bharat Purohit <siddharthbharatpurohit@gmail.com>
|
||||
*
|
||||
*/
|
||||
namespace estimator {
|
||||
struct gps_message {
|
||||
uint64_t time_usec;
|
||||
int32_t lat; // Latitude in 1E-7 degrees
|
||||
int32_t lon; // Longitude in 1E-7 degrees
|
||||
int32_t alt; // Altitude in 1E-3 meters (millimeters) above MSL
|
||||
uint8_t fix_type; // 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time
|
||||
float eph; // GPS horizontal position accuracy in m
|
||||
float epv; // GPS vertical position accuracy in m
|
||||
float sacc; // GPS speed accuracy in m/s
|
||||
uint64_t time_usec_vel; // Timestamp for velocity informations
|
||||
float vel_m_s; // GPS ground speed (m/s)
|
||||
float vel_ned[3]; // GPS ground speed NED
|
||||
bool vel_ned_valid; // GPS ground speed is valid
|
||||
uint8_t nsats; // number of satellites used
|
||||
float gdop; // geometric dilution of precision
|
||||
};
|
||||
namespace estimator
|
||||
{
|
||||
struct gps_message {
|
||||
uint64_t time_usec;
|
||||
int32_t lat; // Latitude in 1E-7 degrees
|
||||
int32_t lon; // Longitude in 1E-7 degrees
|
||||
int32_t alt; // Altitude in 1E-3 meters (millimeters) above MSL
|
||||
uint8_t fix_type; // 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time
|
||||
float eph; // GPS horizontal position accuracy in m
|
||||
float epv; // GPS vertical position accuracy in m
|
||||
float sacc; // GPS speed accuracy in m/s
|
||||
uint64_t time_usec_vel; // Timestamp for velocity informations
|
||||
float vel_m_s; // GPS ground speed (m/s)
|
||||
float vel_ned[3]; // GPS ground speed NED
|
||||
bool vel_ned_valid; // GPS ground speed is valid
|
||||
uint8_t nsats; // number of satellites used
|
||||
float gdop; // geometric dilution of precision
|
||||
};
|
||||
|
||||
typedef matrix::Vector<float, 2> Vector2f;
|
||||
typedef matrix::Vector<float, 3> Vector3f;
|
||||
typedef matrix::Quaternion<float> Quaternion;
|
||||
typedef matrix::Matrix<float, 3, 3> Matrix3f;
|
||||
typedef matrix::Vector<float, 2> Vector2f;
|
||||
typedef matrix::Vector<float, 3> Vector3f;
|
||||
typedef matrix::Quaternion<float> Quaternion;
|
||||
typedef matrix::Matrix<float, 3, 3> Matrix3f;
|
||||
|
||||
struct outputSample {
|
||||
Quaternion quat_nominal;
|
||||
Vector3f vel;
|
||||
Vector3f pos;
|
||||
uint64_t time_us;
|
||||
};
|
||||
struct outputSample {
|
||||
Quaternion quat_nominal;
|
||||
Vector3f vel;
|
||||
Vector3f pos;
|
||||
uint64_t time_us;
|
||||
};
|
||||
|
||||
struct imuSample {
|
||||
Vector3f delta_ang;
|
||||
Vector3f delta_vel;
|
||||
float delta_ang_dt;
|
||||
float delta_vel_dt;
|
||||
uint64_t time_us;
|
||||
};
|
||||
struct imuSample {
|
||||
Vector3f delta_ang;
|
||||
Vector3f delta_vel;
|
||||
float delta_ang_dt;
|
||||
float delta_vel_dt;
|
||||
uint64_t time_us;
|
||||
};
|
||||
|
||||
struct gpsSample {
|
||||
Vector2f pos;
|
||||
float hgt;
|
||||
Vector3f vel;
|
||||
uint64_t time_us;
|
||||
};
|
||||
struct gpsSample {
|
||||
Vector2f pos;
|
||||
float hgt;
|
||||
Vector3f vel;
|
||||
uint64_t time_us;
|
||||
};
|
||||
|
||||
struct magSample {
|
||||
Vector3f mag;
|
||||
uint64_t time_us;
|
||||
};
|
||||
struct magSample {
|
||||
Vector3f mag;
|
||||
uint64_t time_us;
|
||||
};
|
||||
|
||||
struct baroSample {
|
||||
float hgt;
|
||||
uint64_t time_us;
|
||||
};
|
||||
struct baroSample {
|
||||
float hgt;
|
||||
uint64_t time_us;
|
||||
};
|
||||
|
||||
struct rangeSample {
|
||||
float rng;
|
||||
uint64_t time_us;
|
||||
};
|
||||
struct rangeSample {
|
||||
float rng;
|
||||
uint64_t time_us;
|
||||
};
|
||||
|
||||
struct airspeedSample {
|
||||
float airspeed;
|
||||
uint64_t time_us;
|
||||
};
|
||||
struct airspeedSample {
|
||||
float airspeed;
|
||||
uint64_t time_us;
|
||||
};
|
||||
|
||||
struct flowSample {
|
||||
Vector2f flowRadXY;
|
||||
Vector2f flowRadXYcomp;
|
||||
uint64_t time_us;
|
||||
};
|
||||
struct flowSample {
|
||||
Vector2f flowRadXY;
|
||||
Vector2f flowRadXYcomp;
|
||||
uint64_t time_us;
|
||||
};
|
||||
|
||||
struct parameters {
|
||||
float mag_delay_ms = 0.0f;
|
||||
float baro_delay_ms = 0.0f;
|
||||
float gps_delay_ms = 200.0f;
|
||||
float airspeed_delay_ms = 200.0f;
|
||||
struct parameters {
|
||||
float mag_delay_ms = 0.0f;
|
||||
float baro_delay_ms = 0.0f;
|
||||
float gps_delay_ms = 200.0f;
|
||||
float airspeed_delay_ms = 200.0f;
|
||||
|
||||
// input noise
|
||||
float gyro_noise = 0.001f;
|
||||
float accel_noise = 0.1f;
|
||||
// input noise
|
||||
float gyro_noise = 0.001f;
|
||||
float accel_noise = 0.1f;
|
||||
|
||||
// process noise
|
||||
float gyro_bias_p_noise = 1e-5f;
|
||||
float accel_bias_p_noise = 1e-3f;
|
||||
float gyro_scale_p_noise = 1e-4f;
|
||||
float mag_p_noise = 1e-2f;
|
||||
float wind_vel_p_noise = 0.05f;
|
||||
// process noise
|
||||
float gyro_bias_p_noise = 1e-5f;
|
||||
float accel_bias_p_noise = 1e-3f;
|
||||
float gyro_scale_p_noise = 1e-4f;
|
||||
float mag_p_noise = 1e-2f;
|
||||
float wind_vel_p_noise = 0.05f;
|
||||
|
||||
float gps_vel_noise = 0.05f;
|
||||
float gps_pos_noise = 1.0f;
|
||||
float baro_noise = 0.1f;
|
||||
float baro_innov_gate = 5.0f; // barometric height innovation consistency gate size in standard deviations
|
||||
float posNE_innov_gate = 5.0f; // GPS horizontal position innovation consistency gate size in standard deviations
|
||||
float vel_innov_gate = 3.0f; // GPS velocity innovation consistency gate size in standard deviations
|
||||
float gps_vel_noise = 0.05f;
|
||||
float gps_pos_noise = 1.0f;
|
||||
float baro_noise = 0.1f;
|
||||
float baro_innov_gate = 5.0f; // barometric height innovation consistency gate size in standard deviations
|
||||
float posNE_innov_gate = 5.0f; // GPS horizontal position innovation consistency gate size in standard deviations
|
||||
float vel_innov_gate = 3.0f; // GPS velocity innovation consistency gate size in standard deviations
|
||||
|
||||
float mag_heading_noise = 3e-2f; // measurement noise used for simple heading fusion
|
||||
float mag_declination_deg = 0.0f; // magnetic declination in degrees
|
||||
float heading_innov_gate = 3.0f; // heading fusion innovation consistency gate size in standard deviations
|
||||
float mag_innov_gate = 3.0f; // magnetometer fusion innovation consistency gate size in standard deviations
|
||||
float mag_heading_noise = 3e-2f; // measurement noise used for simple heading fusion
|
||||
float mag_declination_deg = 0.0f; // magnetic declination in degrees
|
||||
float heading_innov_gate = 3.0f; // heading fusion innovation consistency gate size in standard deviations
|
||||
float mag_innov_gate = 3.0f; // magnetometer fusion innovation consistency gate size in standard deviations
|
||||
|
||||
// these parameters control the strictness of GPS quality checks used to determine uf the GPS is
|
||||
// good enough to set a local origin and commence aiding
|
||||
int gps_check_mask = 21; // bitmask used to control which GPS quality checks are used
|
||||
float req_hacc = 5.0f; // maximum acceptable horizontal position error
|
||||
float req_vacc = 8.0f; // maximum acceptable vertical position error
|
||||
float req_sacc = 1.0f; // maximum acceptable speed error
|
||||
int req_nsats = 6; // minimum acceptable satellite count
|
||||
float req_gdop = 2.0f; // maximum acceptable geometric dilution of precision
|
||||
float req_hdrift = 0.3f; // maximum acceptable horizontal drift speed
|
||||
float req_vdrift = 0.5f; // maximum acceptable vertical drift speed
|
||||
};
|
||||
// these parameters control the strictness of GPS quality checks used to determine uf the GPS is
|
||||
// good enough to set a local origin and commence aiding
|
||||
int gps_check_mask = 21; // bitmask used to control which GPS quality checks are used
|
||||
float req_hacc = 5.0f; // maximum acceptable horizontal position error
|
||||
float req_vacc = 8.0f; // maximum acceptable vertical position error
|
||||
float req_sacc = 1.0f; // maximum acceptable speed error
|
||||
int req_nsats = 6; // minimum acceptable satellite count
|
||||
float req_gdop = 2.0f; // maximum acceptable geometric dilution of precision
|
||||
float req_hdrift = 0.3f; // maximum acceptable horizontal drift speed
|
||||
float req_vdrift = 0.5f; // maximum acceptable vertical drift speed
|
||||
};
|
||||
|
||||
struct stateSample {
|
||||
Vector3f ang_error;
|
||||
Vector3f vel;
|
||||
Vector3f pos;
|
||||
Vector3f gyro_bias;
|
||||
Vector3f gyro_scale;
|
||||
float accel_z_bias;
|
||||
Vector3f mag_I;
|
||||
Vector3f mag_B;
|
||||
Vector2f wind_vel;
|
||||
Quaternion quat_nominal;
|
||||
};
|
||||
struct stateSample {
|
||||
Vector3f ang_error;
|
||||
Vector3f vel;
|
||||
Vector3f pos;
|
||||
Vector3f gyro_bias;
|
||||
Vector3f gyro_scale;
|
||||
float accel_z_bias;
|
||||
Vector3f mag_I;
|
||||
Vector3f mag_B;
|
||||
Vector2f wind_vel;
|
||||
Quaternion quat_nominal;
|
||||
};
|
||||
|
||||
struct fault_status_t {
|
||||
bool bad_mag_x: 1;
|
||||
bool bad_mag_y: 1;
|
||||
bool bad_mag_z: 1;
|
||||
bool bad_airspeed: 1;
|
||||
bool bad_sideslip: 1;
|
||||
};
|
||||
struct fault_status_t {
|
||||
bool bad_mag_x: 1;
|
||||
bool bad_mag_y: 1;
|
||||
bool bad_mag_z: 1;
|
||||
bool bad_airspeed: 1;
|
||||
bool bad_sideslip: 1;
|
||||
};
|
||||
|
||||
// publish the status of various GPS quality checks
|
||||
union gps_check_fail_status_u {
|
||||
struct {
|
||||
uint16_t fix : 1; // 0 - true if the fix type is insufficient (no 3D solution)
|
||||
uint16_t nsats : 1; // 1 - true if number of satellites used is insufficient
|
||||
uint16_t gdop : 1; // 2 - true if geometric dilution of precision is insufficient
|
||||
uint16_t hacc : 1; // 3 - true if reported horizontal accuracy is insufficient
|
||||
uint16_t vacc : 1; // 4 - true if reported vertical accuracy is insufficient
|
||||
uint16_t sacc : 1; // 5 - true if reported speed accuracy is insufficient
|
||||
uint16_t hdrift : 1; // 6 - true if horizontal drift is excessive (can only be used when stationary on ground)
|
||||
uint16_t vdrift : 1; // 7 - true if vertical drift is excessive (can only be used when stationary on ground)
|
||||
uint16_t hspeed : 1; // 8 - true if horizontal speed is excessive (can only be used when stationary on ground)
|
||||
uint16_t vspeed : 1; // 9 - true if vertical speed error is excessive
|
||||
} flags;
|
||||
uint16_t value;
|
||||
};
|
||||
// publish the status of various GPS quality checks
|
||||
union gps_check_fail_status_u {
|
||||
struct {
|
||||
uint16_t fix : 1; // 0 - true if the fix type is insufficient (no 3D solution)
|
||||
uint16_t nsats : 1; // 1 - true if number of satellites used is insufficient
|
||||
uint16_t gdop : 1; // 2 - true if geometric dilution of precision is insufficient
|
||||
uint16_t hacc : 1; // 3 - true if reported horizontal accuracy is insufficient
|
||||
uint16_t vacc : 1; // 4 - true if reported vertical accuracy is insufficient
|
||||
uint16_t sacc : 1; // 5 - true if reported speed accuracy is insufficient
|
||||
uint16_t hdrift : 1; // 6 - true if horizontal drift is excessive (can only be used when stationary on ground)
|
||||
uint16_t vdrift : 1; // 7 - true if vertical drift is excessive (can only be used when stationary on ground)
|
||||
uint16_t hspeed : 1; // 8 - true if horizontal speed is excessive (can only be used when stationary on ground)
|
||||
uint16_t vspeed : 1; // 9 - true if vertical speed error is excessive
|
||||
} flags;
|
||||
uint16_t value;
|
||||
};
|
||||
|
||||
// bitmask containing filter control status
|
||||
union filter_control_status_u {
|
||||
struct {
|
||||
uint8_t angle_align : 1; // 0 - true if the filter angular alignment is complete
|
||||
uint8_t gps : 1; // 1 - true if GPS measurements are being fused
|
||||
uint8_t opt_flow : 1; // 2 - true if optical flow measurements are being fused
|
||||
uint8_t mag_hdg : 1; // 3 - true if a simple magnetic heading is being fused
|
||||
uint8_t mag_3D : 1; // 4 - true if 3-axis magnetometer measurement are being fused
|
||||
uint8_t mag_dec : 1; // 5 - true if synthetic magnetic declination measurements are being fused
|
||||
uint8_t in_air : 1; // 6 - true when the vehicle is airborne
|
||||
uint8_t armed : 1; // 7 - true when the vehicle motors are armed
|
||||
} flags;
|
||||
uint16_t value;
|
||||
};
|
||||
// bitmask containing filter control status
|
||||
union filter_control_status_u {
|
||||
struct {
|
||||
uint8_t angle_align : 1; // 0 - true if the filter angular alignment is complete
|
||||
uint8_t gps : 1; // 1 - true if GPS measurements are being fused
|
||||
uint8_t opt_flow : 1; // 2 - true if optical flow measurements are being fused
|
||||
uint8_t mag_hdg : 1; // 3 - true if a simple magnetic heading is being fused
|
||||
uint8_t mag_3D : 1; // 4 - true if 3-axis magnetometer measurement are being fused
|
||||
uint8_t mag_dec : 1; // 5 - true if synthetic magnetic declination measurements are being fused
|
||||
uint8_t in_air : 1; // 6 - true when the vehicle is airborne
|
||||
uint8_t armed : 1; // 7 - true when the vehicle motors are armed
|
||||
} flags;
|
||||
uint16_t value;
|
||||
};
|
||||
}
|
||||
|
||||
+74
-70
@@ -43,87 +43,91 @@
|
||||
|
||||
void Ekf::controlFusionModes()
|
||||
{
|
||||
// Determine the vehicle status
|
||||
calculateVehicleStatus();
|
||||
// Determine the vehicle status
|
||||
calculateVehicleStatus();
|
||||
|
||||
// optical flow fusion mode selection logic
|
||||
_control_status.flags.opt_flow = false;
|
||||
// optical flow fusion mode selection logic
|
||||
_control_status.flags.opt_flow = false;
|
||||
|
||||
// GPS fusion mode selection logic
|
||||
// To start use GPS we need angular alignment completed, the local NED origin set and fresh GPS data
|
||||
if (!_control_status.flags.gps) {
|
||||
if (_control_status.flags.angle_align && (_time_last_imu - _time_last_gps) < 5e5 && _NED_origin_initialised && (_time_last_imu - _last_gps_fail_us > 5e6)) {
|
||||
_control_status.flags.gps = true;
|
||||
resetPosition();
|
||||
resetVelocity();
|
||||
}
|
||||
}
|
||||
// GPS fusion mode selection logic
|
||||
// To start use GPS we need angular alignment completed, the local NED origin set and fresh GPS data
|
||||
if (!_control_status.flags.gps) {
|
||||
if (_control_status.flags.angle_align && (_time_last_imu - _time_last_gps) < 5e5 && _NED_origin_initialised
|
||||
&& (_time_last_imu - _last_gps_fail_us > 5e6)) {
|
||||
_control_status.flags.gps = true;
|
||||
resetPosition();
|
||||
resetVelocity();
|
||||
}
|
||||
}
|
||||
|
||||
// decide when to start using optical flow data
|
||||
if (!_control_status.flags.opt_flow) {
|
||||
// TODO optical flow start logic
|
||||
}
|
||||
// decide when to start using optical flow data
|
||||
if (!_control_status.flags.opt_flow) {
|
||||
// TODO optical flow start logic
|
||||
}
|
||||
|
||||
// handle the case when we are relying on GPS fusion and lose it
|
||||
if (_control_status.flags.gps && !_control_status.flags.opt_flow) {
|
||||
// We are relying on GPS aiding to constrain attitude drift so after 10 seconds without aiding we need to do something
|
||||
if ((_time_last_imu - _time_last_pos_fuse > 10e6) && (_time_last_imu - _time_last_vel_fuse > 10e6)) {
|
||||
if (_time_last_imu - _time_last_gps > 5e5) {
|
||||
// if we don't have gps then we need to switch to the non-aiding mode, zero the veloity states
|
||||
// and set the synthetic GPS position to the current estimate
|
||||
_control_status.flags.gps = false;
|
||||
_last_known_posNE(0) = _state.pos(0);
|
||||
_last_known_posNE(1) = _state.pos(1);
|
||||
_state.vel.setZero();
|
||||
} else {
|
||||
// Reset states to the last GPS measurement
|
||||
resetPosition();
|
||||
resetVelocity();
|
||||
}
|
||||
}
|
||||
}
|
||||
// handle the case when we are relying on GPS fusion and lose it
|
||||
if (_control_status.flags.gps && !_control_status.flags.opt_flow) {
|
||||
// We are relying on GPS aiding to constrain attitude drift so after 10 seconds without aiding we need to do something
|
||||
if ((_time_last_imu - _time_last_pos_fuse > 10e6) && (_time_last_imu - _time_last_vel_fuse > 10e6)) {
|
||||
if (_time_last_imu - _time_last_gps > 5e5) {
|
||||
// if we don't have gps then we need to switch to the non-aiding mode, zero the veloity states
|
||||
// and set the synthetic GPS position to the current estimate
|
||||
_control_status.flags.gps = false;
|
||||
_last_known_posNE(0) = _state.pos(0);
|
||||
_last_known_posNE(1) = _state.pos(1);
|
||||
_state.vel.setZero();
|
||||
|
||||
// handle the case when we are relying on optical flow fusion and lose it
|
||||
if (_control_status.flags.opt_flow && !_control_status.flags.gps) {
|
||||
// TODO
|
||||
}
|
||||
} else {
|
||||
// Reset states to the last GPS measurement
|
||||
resetPosition();
|
||||
resetVelocity();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Determine if we should use simple magnetic heading fusion which works better when there are large external disturbances
|
||||
// or the more accurate 3-axis fusion
|
||||
if (!_control_status.flags.armed) {
|
||||
// always use simple mag fusion for initial startup
|
||||
_control_status.flags.mag_hdg = true;
|
||||
_control_status.flags.mag_3D = false;
|
||||
} else {
|
||||
if (_control_status.flags.in_air) {
|
||||
// always use 3-axis mag fusion when airborne
|
||||
_control_status.flags.mag_hdg = false;
|
||||
_control_status.flags.mag_3D = true;
|
||||
} else {
|
||||
// always use simple heading fusion when on the ground
|
||||
_control_status.flags.mag_hdg = true;
|
||||
_control_status.flags.mag_3D = false;
|
||||
}
|
||||
}
|
||||
// handle the case when we are relying on optical flow fusion and lose it
|
||||
if (_control_status.flags.opt_flow && !_control_status.flags.gps) {
|
||||
// TODO
|
||||
}
|
||||
|
||||
// Determine if we should use simple magnetic heading fusion which works better when there are large external disturbances
|
||||
// or the more accurate 3-axis fusion
|
||||
if (!_control_status.flags.armed) {
|
||||
// always use simple mag fusion for initial startup
|
||||
_control_status.flags.mag_hdg = true;
|
||||
_control_status.flags.mag_3D = false;
|
||||
|
||||
} else {
|
||||
if (_control_status.flags.in_air) {
|
||||
// always use 3-axis mag fusion when airborne
|
||||
_control_status.flags.mag_hdg = false;
|
||||
_control_status.flags.mag_3D = true;
|
||||
|
||||
} else {
|
||||
// always use simple heading fusion when on the ground
|
||||
_control_status.flags.mag_hdg = true;
|
||||
_control_status.flags.mag_3D = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::calculateVehicleStatus()
|
||||
{
|
||||
// determine if the vehicle is armed
|
||||
_control_status.flags.armed = _vehicle_armed;
|
||||
// determine if the vehicle is armed
|
||||
_control_status.flags.armed = _vehicle_armed;
|
||||
|
||||
// record vertical position whilst disarmed to use as a height change reference
|
||||
if (!_control_status.flags.armed) {
|
||||
_last_disarmed_posD = _state.pos(2);
|
||||
}
|
||||
// record vertical position whilst disarmed to use as a height change reference
|
||||
if (!_control_status.flags.armed) {
|
||||
_last_disarmed_posD = _state.pos(2);
|
||||
}
|
||||
|
||||
// Transition to in-air occurs when armed and when altitude has increased sufficiently from the altitude at arming
|
||||
if (!_control_status.flags.in_air && _control_status.flags.armed && (_state.pos(2) - _last_disarmed_posD) < -1.0f) {
|
||||
_control_status.flags.in_air = true;
|
||||
}
|
||||
// Transition to in-air occurs when armed and when altitude has increased sufficiently from the altitude at arming
|
||||
if (!_control_status.flags.in_air && _control_status.flags.armed && (_state.pos(2) - _last_disarmed_posD) < -1.0f) {
|
||||
_control_status.flags.in_air = true;
|
||||
}
|
||||
|
||||
// Transition to on-ground occurs when disarmed.
|
||||
if (_control_status.flags.in_air && !_control_status.flags.armed) {
|
||||
_control_status.flags.in_air = false;
|
||||
}
|
||||
// Transition to on-ground occurs when disarmed.
|
||||
if (_control_status.flags.in_air && !_control_status.flags.armed) {
|
||||
_control_status.flags.in_air = false;
|
||||
}
|
||||
}
|
||||
|
||||
+6
-1
@@ -42,7 +42,7 @@
|
||||
|
||||
#include "ekf.h"
|
||||
#include <math.h>
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <mathlib/mathlib.h>
|
||||
|
||||
#define sq(_arg) powf(_arg, 2.0f)
|
||||
|
||||
@@ -140,20 +140,25 @@ void Ekf::predictCovariance()
|
||||
for (unsigned i = 0; i < 9; i++) {
|
||||
process_noise[i] = 0.0f;
|
||||
}
|
||||
|
||||
for (unsigned i = 9; i < 12; i++) {
|
||||
process_noise[i] = sq(d_ang_bias_sig);
|
||||
}
|
||||
|
||||
for (unsigned i = 12; i < 15; i++) {
|
||||
process_noise[i] = sq(d_ang_scale_sig);
|
||||
}
|
||||
|
||||
process_noise[15] = sq(d_vel_bias_sig);
|
||||
|
||||
for (unsigned i = 16; i < 19; i++) {
|
||||
process_noise[i] = sq(mag_I_sig);
|
||||
}
|
||||
|
||||
for (unsigned i = 19; i < 22; i++) {
|
||||
process_noise[i] = sq(mag_B_sig);
|
||||
}
|
||||
|
||||
for (unsigned i = 22; i < 24; i++) {
|
||||
process_noise[i] = sq(wind_vel_sig);
|
||||
}
|
||||
|
||||
+43
-38
@@ -43,31 +43,31 @@
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
Ekf::Ekf():
|
||||
_control_status{},
|
||||
_filter_initialised(false),
|
||||
_control_status{},
|
||||
_filter_initialised(false),
|
||||
_earth_rate_initialised(false),
|
||||
_fuse_height(false),
|
||||
_fuse_pos(false),
|
||||
_fuse_hor_vel(false),
|
||||
_fuse_vert_vel(false),
|
||||
_time_last_fake_gps(0),
|
||||
_time_last_pos_fuse(0),
|
||||
_time_last_vel_fuse(0),
|
||||
_time_last_hgt_fuse(0),
|
||||
_time_last_of_fuse(0),
|
||||
_vel_pos_innov{},
|
||||
_time_last_fake_gps(0),
|
||||
_time_last_pos_fuse(0),
|
||||
_time_last_vel_fuse(0),
|
||||
_time_last_hgt_fuse(0),
|
||||
_time_last_of_fuse(0),
|
||||
_vel_pos_innov{},
|
||||
_mag_innov{},
|
||||
_heading_innov{},
|
||||
_vel_pos_innov_var{},
|
||||
_mag_innov_var{},
|
||||
_heading_innov_var{}
|
||||
_heading_innov_var{}
|
||||
{
|
||||
_earth_rate_NED.setZero();
|
||||
_R_prev = matrix::Dcm<float>();
|
||||
_delta_angle_corr.setZero();
|
||||
_delta_vel_corr.setZero();
|
||||
_vel_corr.setZero();
|
||||
_last_known_posNE.setZero();
|
||||
_last_known_posNE.setZero();
|
||||
}
|
||||
|
||||
|
||||
@@ -127,6 +127,7 @@ bool Ekf::update()
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
//printStates();
|
||||
//printStatesFast();
|
||||
// prediction
|
||||
@@ -136,41 +137,44 @@ bool Ekf::update()
|
||||
predictCovariance();
|
||||
}
|
||||
|
||||
// control logic
|
||||
controlFusionModes();
|
||||
// control logic
|
||||
controlFusionModes();
|
||||
|
||||
// measurement updates
|
||||
// measurement updates
|
||||
|
||||
// Fuse magnetometer data using the selected fuson method and only if angular alignment is complete
|
||||
// Fuse magnetometer data using the selected fuson method and only if angular alignment is complete
|
||||
if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) {
|
||||
if (_control_status.flags.mag_3D && _control_status.flags.angle_align) {
|
||||
fuseMag();
|
||||
if (_control_status.flags.mag_dec) {
|
||||
// TODO need to fuse synthetic declination measurements if there is no GPS or equivalent aiding
|
||||
// otherwise heading will slowly drift
|
||||
}
|
||||
} else if (_control_status.flags.mag_hdg && _control_status.flags.angle_align) {
|
||||
fuseHeading();
|
||||
}
|
||||
if (_control_status.flags.mag_3D && _control_status.flags.angle_align) {
|
||||
fuseMag();
|
||||
|
||||
if (_control_status.flags.mag_dec) {
|
||||
// TODO need to fuse synthetic declination measurements if there is no GPS or equivalent aiding
|
||||
// otherwise heading will slowly drift
|
||||
}
|
||||
} else if (_control_status.flags.mag_hdg && _control_status.flags.angle_align) {
|
||||
fuseHeading();
|
||||
}
|
||||
}
|
||||
|
||||
if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) {
|
||||
_fuse_height = true;
|
||||
}
|
||||
|
||||
// If we are using GPs aiding and data has fallen behind the fusion time horizon then fuse it
|
||||
// if we aren't doing any aiding, fake GPS measurements at the last known position to constrain drift
|
||||
// Coincide fake measurements with baro data for efficiency with a minimum fusion rate of 5Hz
|
||||
if (_gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed) && _control_status.flags.gps) {
|
||||
// If we are using GPs aiding and data has fallen behind the fusion time horizon then fuse it
|
||||
// if we aren't doing any aiding, fake GPS measurements at the last known position to constrain drift
|
||||
// Coincide fake measurements with baro data for efficiency with a minimum fusion rate of 5Hz
|
||||
if (_gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed) && _control_status.flags.gps) {
|
||||
_fuse_pos = true;
|
||||
_fuse_vert_vel = true;
|
||||
_fuse_hor_vel = true;
|
||||
} else if (!_control_status.flags.gps && !_control_status.flags.opt_flow && ((_time_last_imu - _time_last_fake_gps > 2e5) || _fuse_height)) {
|
||||
_fuse_pos = true;
|
||||
_gps_sample_delayed.pos(0) = _last_known_posNE(0);
|
||||
_gps_sample_delayed.pos(1) = _last_known_posNE(1);
|
||||
_time_last_fake_gps = _time_last_imu;
|
||||
}
|
||||
|
||||
} else if (!_control_status.flags.gps && !_control_status.flags.opt_flow
|
||||
&& ((_time_last_imu - _time_last_fake_gps > 2e5) || _fuse_height)) {
|
||||
_fuse_pos = true;
|
||||
_gps_sample_delayed.pos(0) = _last_known_posNE(0);
|
||||
_gps_sample_delayed.pos(1) = _last_known_posNE(1);
|
||||
_time_last_fake_gps = _time_last_imu;
|
||||
}
|
||||
|
||||
if (_fuse_height || _fuse_pos || _fuse_hor_vel || _fuse_vert_vel) {
|
||||
fuseVelPosHeight();
|
||||
@@ -236,8 +240,8 @@ bool Ekf::initialiseFilter(void)
|
||||
_state.quat_nominal = Quaternion(euler_init);
|
||||
_output_new.quat_nominal = _state.quat_nominal;
|
||||
|
||||
// TODO replace this with a conditional test based on fitered angle error states.
|
||||
_control_status.flags.angle_align = true;
|
||||
// TODO replace this with a conditional test based on fitered angle error states.
|
||||
_control_status.flags.angle_align = true;
|
||||
|
||||
// calculate initial earth magnetic field states
|
||||
matrix::Dcm<float> R_to_earth(euler_init);
|
||||
@@ -264,8 +268,8 @@ bool Ekf::initialiseFilter(void)
|
||||
void Ekf::predictState()
|
||||
{
|
||||
if (!_earth_rate_initialised) {
|
||||
if (_NED_origin_initialised) {
|
||||
calcEarthRateNED(_earth_rate_NED, _pos_ref.lat_rad);
|
||||
if (_NED_origin_initialised) {
|
||||
calcEarthRateNED(_earth_rate_NED, _pos_ref.lat_rad);
|
||||
_earth_rate_initialised = true;
|
||||
}
|
||||
}
|
||||
@@ -341,8 +345,9 @@ bool Ekf::collect_imu(imuSample &imu)
|
||||
_imu_down_sampled.delta_vel_dt = 0.0f;
|
||||
_q_down_sampled(0) = 1.0f;
|
||||
_q_down_sampled(1) = _q_down_sampled(2) = _q_down_sampled(3) = 0.0f;
|
||||
return true;
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
@@ -51,7 +51,7 @@ public:
|
||||
Ekf();
|
||||
~Ekf();
|
||||
|
||||
bool init(uint64_t timestamp);
|
||||
bool init(uint64_t timestamp);
|
||||
bool update();
|
||||
|
||||
// gets the innovations of velocity and position measurements
|
||||
@@ -80,21 +80,21 @@ public:
|
||||
// get the diagonal elements of the covariance matrix
|
||||
void get_covariances(float *covariances);
|
||||
|
||||
// ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined
|
||||
bool collect_gps(uint64_t time_usec, struct gps_message *gps);
|
||||
bool collect_imu(imuSample &imu);
|
||||
// ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined
|
||||
bool collect_gps(uint64_t time_usec, struct gps_message *gps);
|
||||
bool collect_imu(imuSample &imu);
|
||||
|
||||
filter_control_status_u _control_status={};
|
||||
filter_control_status_u _control_status = {};
|
||||
|
||||
// get the ekf WGS-84 origin positoin and height and the system time it was last set
|
||||
void get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt);
|
||||
// get the ekf WGS-84 origin positoin and height and the system time it was last set
|
||||
void get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt);
|
||||
|
||||
private:
|
||||
|
||||
static const uint8_t _k_num_states = 24;
|
||||
static constexpr float _k_earth_rate = 0.000072921f;
|
||||
|
||||
stateSample _state;
|
||||
stateSample _state;
|
||||
|
||||
bool _filter_initialised;
|
||||
bool _earth_rate_initialised;
|
||||
@@ -104,14 +104,14 @@ private:
|
||||
bool _fuse_hor_vel; // gps horizontal velocity measurement should be fused
|
||||
bool _fuse_vert_vel; // gps vertical velocity measurement should be fused
|
||||
|
||||
uint64_t _time_last_fake_gps;
|
||||
uint64_t _time_last_fake_gps;
|
||||
|
||||
uint64_t _time_last_pos_fuse; // time the last fusion of horizotal position measurements was performed (usec)
|
||||
uint64_t _time_last_vel_fuse; // time the last fusion of velocity measurements was performed (usec)
|
||||
uint64_t _time_last_hgt_fuse; // time the last fusion of height measurements was performed (usec)
|
||||
uint64_t _time_last_of_fuse; // time the last fusion of optical flow measurements were performed (usec)
|
||||
Vector2f _last_known_posNE; // last known local NE position vector (m)
|
||||
float _last_disarmed_posD; // vertical position recorded at arming (m)
|
||||
uint64_t _time_last_pos_fuse; // time the last fusion of horizotal position measurements was performed (usec)
|
||||
uint64_t _time_last_vel_fuse; // time the last fusion of velocity measurements was performed (usec)
|
||||
uint64_t _time_last_hgt_fuse; // time the last fusion of height measurements was performed (usec)
|
||||
uint64_t _time_last_of_fuse; // time the last fusion of optical flow measurements were performed (usec)
|
||||
Vector2f _last_known_posNE; // last known local NE position vector (m)
|
||||
float _last_disarmed_posD; // vertical position recorded at arming (m)
|
||||
|
||||
Vector3f _earth_rate_NED;
|
||||
|
||||
@@ -131,26 +131,26 @@ private:
|
||||
Vector3f _delta_angle_corr;
|
||||
Vector3f _delta_vel_corr;
|
||||
Vector3f _vel_corr;
|
||||
imuSample _imu_down_sampled;
|
||||
Quaternion _q_down_sampled;
|
||||
imuSample _imu_down_sampled;
|
||||
Quaternion _q_down_sampled;
|
||||
|
||||
// variables used for the GPS quality checks
|
||||
float _gpsDriftVelN = 0.0f; // GPS north position derivative (m/s)
|
||||
float _gpsDriftVelE = 0.0f; // GPS east position derivative (m/s)
|
||||
float _gps_drift_velD = 0.0f; // GPS down position derivative (m/s)
|
||||
float _gps_velD_diff_filt = 0.0f; // GPS filtered Down velocity (m/s)
|
||||
float _gps_velN_filt = 0.0f; // GPS filtered North velocity (m/s)
|
||||
float _gps_velE_filt = 0.0f; // GPS filtered East velocity (m/s)
|
||||
uint64_t _last_gps_fail_us = 0; // last system time in usec that the GPS failed it's checks
|
||||
// variables used for the GPS quality checks
|
||||
float _gpsDriftVelN = 0.0f; // GPS north position derivative (m/s)
|
||||
float _gpsDriftVelE = 0.0f; // GPS east position derivative (m/s)
|
||||
float _gps_drift_velD = 0.0f; // GPS down position derivative (m/s)
|
||||
float _gps_velD_diff_filt = 0.0f; // GPS filtered Down velocity (m/s)
|
||||
float _gps_velN_filt = 0.0f; // GPS filtered North velocity (m/s)
|
||||
float _gps_velE_filt = 0.0f; // GPS filtered East velocity (m/s)
|
||||
uint64_t _last_gps_fail_us = 0; // last system time in usec that the GPS failed it's checks
|
||||
|
||||
// Variables used to publish the WGS-84 location of the EKF local NED origin
|
||||
uint64_t _last_gps_origin_time_us = 0; // time the origin was last set (uSec)
|
||||
float _gps_alt_ref = 0.0f; // WGS-84 height (m)
|
||||
// Variables used to publish the WGS-84 location of the EKF local NED origin
|
||||
uint64_t _last_gps_origin_time_us = 0; // time the origin was last set (uSec)
|
||||
float _gps_alt_ref = 0.0f; // WGS-84 height (m)
|
||||
|
||||
|
||||
gps_check_fail_status_u _gps_check_fail_status;
|
||||
gps_check_fail_status_u _gps_check_fail_status;
|
||||
|
||||
void calculateOutputStates();
|
||||
void calculateOutputStates();
|
||||
|
||||
bool initialiseFilter(void);
|
||||
|
||||
@@ -160,7 +160,7 @@ private:
|
||||
|
||||
void predictCovariance();
|
||||
|
||||
void fuseMag();
|
||||
void fuseMag();
|
||||
|
||||
void fuseHeading();
|
||||
|
||||
@@ -194,12 +194,12 @@ private:
|
||||
|
||||
void calcEarthRateNED(Vector3f &omega, double lat_rad) const;
|
||||
|
||||
// return true id the GPS quality is good enough to set an origin and start aiding
|
||||
bool gps_is_good(struct gps_message *gps);
|
||||
// return true id the GPS quality is good enough to set an origin and start aiding
|
||||
bool gps_is_good(struct gps_message *gps);
|
||||
|
||||
// Control the filter fusion modes
|
||||
void controlFusionModes();
|
||||
// Control the filter fusion modes
|
||||
void controlFusionModes();
|
||||
|
||||
// Determine if we are airborne or motors are armed
|
||||
void calculateVehicleStatus();
|
||||
// Determine if we are airborne or motors are armed
|
||||
void calculateVehicleStatus();
|
||||
};
|
||||
|
||||
+4
-4
@@ -158,7 +158,7 @@ void Ekf::constrainStates()
|
||||
_state.mag_B(i) = math::constrain(_state.mag_B(i), -0.5f, 0.5f);
|
||||
}
|
||||
|
||||
for (int i = 0; i < 2; i++) {
|
||||
for (int i = 0; i < 2; i++) {
|
||||
_state.wind_vel(i) = math::constrain(_state.wind_vel(i), -100.0f, 100.0f);
|
||||
}
|
||||
}
|
||||
@@ -258,7 +258,7 @@ void Ekf::get_covariances(float *covariances)
|
||||
// get the position and height of the ekf origin in WGS-84 coordinates and time the origin was set
|
||||
void Ekf::get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt)
|
||||
{
|
||||
memcpy(origin_time, &_last_gps_origin_time_us, sizeof(uint64_t));
|
||||
memcpy(origin_pos, &_pos_ref, sizeof(map_projection_reference_s));
|
||||
memcpy(origin_alt, &_gps_alt_ref, sizeof(float));
|
||||
memcpy(origin_time, &_last_gps_origin_time_us, sizeof(uint64_t));
|
||||
memcpy(origin_pos, &_pos_ref, sizeof(map_projection_reference_s));
|
||||
memcpy(origin_alt, &_gps_alt_ref, sizeof(float));
|
||||
}
|
||||
|
||||
+21
-18
@@ -57,7 +57,8 @@ EstimatorInterface::~EstimatorInterface()
|
||||
}
|
||||
|
||||
// Accumulate imu data and store to buffer at desired rate
|
||||
void EstimatorInterface::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, uint64_t delta_vel_dt, float *delta_ang, float *delta_vel)
|
||||
void EstimatorInterface::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, uint64_t delta_vel_dt, float *delta_ang,
|
||||
float *delta_vel)
|
||||
{
|
||||
if (!_initialised) {
|
||||
init(time_usec);
|
||||
@@ -119,12 +120,12 @@ void EstimatorInterface::setMagData(uint64_t time_usec, float *data)
|
||||
|
||||
void EstimatorInterface::setGpsData(uint64_t time_usec, struct gps_message *gps)
|
||||
{
|
||||
if(!collect_gps(time_usec, gps) || !_initialised) {
|
||||
if (!collect_gps(time_usec, gps) || !_initialised) {
|
||||
return;
|
||||
}
|
||||
|
||||
// Only use GPS data if we have a 3D fix and limit the GPS data rate to a maximum of 14Hz
|
||||
if (time_usec - _time_last_gps > 70000 && gps->fix_type >= 3) {
|
||||
// Only use GPS data if we have a 3D fix and limit the GPS data rate to a maximum of 14Hz
|
||||
if (time_usec - _time_last_gps > 70000 && gps->fix_type >= 3) {
|
||||
gpsSample gps_sample_new = {};
|
||||
gps_sample_new.time_us = gps->time_usec - _params.gps_delay_ms * 1000;
|
||||
|
||||
@@ -139,7 +140,7 @@ void EstimatorInterface::setGpsData(uint64_t time_usec, struct gps_message *gps)
|
||||
|
||||
float lpos_x = 0.0f;
|
||||
float lpos_y = 0.0f;
|
||||
map_projection_project(&_pos_ref, (gps->lat / 1.0e7), (gps->lon / 1.0e7), &lpos_x, &lpos_y);
|
||||
map_projection_project(&_pos_ref, (gps->lat / 1.0e7), (gps->lon / 1.0e7), &lpos_x, &lpos_y);
|
||||
gps_sample_new.pos(0) = lpos_x;
|
||||
gps_sample_new.pos(1) = lpos_y;
|
||||
gps_sample_new.hgt = gps->alt / 1e3f;
|
||||
@@ -150,9 +151,10 @@ void EstimatorInterface::setGpsData(uint64_t time_usec, struct gps_message *gps)
|
||||
|
||||
void EstimatorInterface::setBaroData(uint64_t time_usec, float *data)
|
||||
{
|
||||
if(!collect_baro(time_usec, data) || !_initialised) {
|
||||
if (!collect_baro(time_usec, data) || !_initialised) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (time_usec - _time_last_baro > 70000) {
|
||||
|
||||
baroSample baro_sample_new;
|
||||
@@ -170,9 +172,10 @@ void EstimatorInterface::setBaroData(uint64_t time_usec, float *data)
|
||||
|
||||
void EstimatorInterface::setAirspeedData(uint64_t time_usec, float *data)
|
||||
{
|
||||
if(!collect_airspeed(time_usec, data) || !_initialised) {
|
||||
if (!collect_airspeed(time_usec, data) || !_initialised) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (time_usec > _time_last_airspeed) {
|
||||
airspeedSample airspeed_sample_new;
|
||||
airspeed_sample_new.airspeed = *data;
|
||||
@@ -188,7 +191,7 @@ void EstimatorInterface::setAirspeedData(uint64_t time_usec, float *data)
|
||||
// set range data
|
||||
void EstimatorInterface::setRangeData(uint64_t time_usec, float *data)
|
||||
{
|
||||
if(!collect_range(time_usec, data) || !_initialised) {
|
||||
if (!collect_range(time_usec, data) || !_initialised) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
@@ -196,7 +199,7 @@ void EstimatorInterface::setRangeData(uint64_t time_usec, float *data)
|
||||
// set optical flow data
|
||||
void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, float *data)
|
||||
{
|
||||
if(!collect_opticalflow(time_usec, data) || !_initialised) {
|
||||
if (!collect_opticalflow(time_usec, data) || !_initialised) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
@@ -204,14 +207,14 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, float *data)
|
||||
bool EstimatorInterface::initialise_interface(uint64_t timestamp)
|
||||
{
|
||||
|
||||
if(!(_imu_buffer.allocate(IMU_BUFFER_LENGTH) &&
|
||||
_gps_buffer.allocate(OBS_BUFFER_LENGTH) &&
|
||||
_mag_buffer.allocate(OBS_BUFFER_LENGTH) &&
|
||||
_baro_buffer.allocate(OBS_BUFFER_LENGTH) &&
|
||||
_range_buffer.allocate(OBS_BUFFER_LENGTH) &&
|
||||
_airspeed_buffer.allocate(OBS_BUFFER_LENGTH) &&
|
||||
_flow_buffer.allocate(OBS_BUFFER_LENGTH) &&
|
||||
_output_buffer.allocate(IMU_BUFFER_LENGTH))) {
|
||||
if (!(_imu_buffer.allocate(IMU_BUFFER_LENGTH) &&
|
||||
_gps_buffer.allocate(OBS_BUFFER_LENGTH) &&
|
||||
_mag_buffer.allocate(OBS_BUFFER_LENGTH) &&
|
||||
_baro_buffer.allocate(OBS_BUFFER_LENGTH) &&
|
||||
_range_buffer.allocate(OBS_BUFFER_LENGTH) &&
|
||||
_airspeed_buffer.allocate(OBS_BUFFER_LENGTH) &&
|
||||
_flow_buffer.allocate(OBS_BUFFER_LENGTH) &&
|
||||
_output_buffer.allocate(IMU_BUFFER_LENGTH))) {
|
||||
PX4_WARN("Estimator Buffer Allocation failed!");
|
||||
unallocate_buffers();
|
||||
return false;
|
||||
@@ -259,7 +262,7 @@ bool EstimatorInterface::position_is_valid()
|
||||
{
|
||||
// return true if the position estimate is valid
|
||||
// TOTO implement proper check based on published GPS accuracy, innovaton consistency checks and timeout status
|
||||
return _NED_origin_initialised && (_time_last_imu - _time_last_gps) < 5e6;
|
||||
return _NED_origin_initialised && (_time_last_imu - _time_last_gps) < 5e6;
|
||||
}
|
||||
|
||||
void EstimatorInterface::printStoredIMU()
|
||||
|
||||
+20
-20
@@ -81,24 +81,24 @@ public:
|
||||
|
||||
virtual void get_covariances(float *covariances) = 0;
|
||||
|
||||
// get the ekf WGS-84 origin positoin and height and the system time it was last set
|
||||
virtual void get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt) = 0;
|
||||
// get the ekf WGS-84 origin positoin and height and the system time it was last set
|
||||
virtual void get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt) = 0;
|
||||
|
||||
|
||||
// ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined
|
||||
virtual bool collect_gps(uint64_t time_usec, struct gps_message *gps) { return true; }
|
||||
// ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined
|
||||
virtual bool collect_gps(uint64_t time_usec, struct gps_message *gps) { return true; }
|
||||
|
||||
virtual bool collect_imu(imuSample &imu) { return true; }
|
||||
virtual bool collect_imu(imuSample &imu) { return true; }
|
||||
|
||||
virtual bool collect_mag(uint64_t time_usec, float *data) { return true; }
|
||||
virtual bool collect_mag(uint64_t time_usec, float *data) { return true; }
|
||||
|
||||
virtual bool collect_baro(uint64_t time_usec, float *data) { return true; }
|
||||
virtual bool collect_baro(uint64_t time_usec, float *data) { return true; }
|
||||
|
||||
virtual bool collect_airspeed(uint64_t time_usec, float *data) { return true; }
|
||||
virtual bool collect_airspeed(uint64_t time_usec, float *data) { return true; }
|
||||
|
||||
virtual bool collect_range(uint64_t time_usec, float *data) { return true; }
|
||||
virtual bool collect_range(uint64_t time_usec, float *data) { return true; }
|
||||
|
||||
virtual bool collect_opticalflow(uint64_t time_usec, float *data) { return true; }
|
||||
virtual bool collect_opticalflow(uint64_t time_usec, float *data) { return true; }
|
||||
|
||||
// set delta angle imu data
|
||||
void setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, uint64_t delta_vel_dt, float *delta_ang, float *delta_vel);
|
||||
@@ -125,8 +125,8 @@ public:
|
||||
// in order to give access to the application
|
||||
parameters *getParamHandle() {return &_params;}
|
||||
|
||||
// set vehicle arm status data
|
||||
void set_arm_status(bool data){ _vehicle_armed = data; }
|
||||
// set vehicle arm status data
|
||||
void set_arm_status(bool data) { _vehicle_armed = data; }
|
||||
|
||||
void printIMU(struct imuSample *data);
|
||||
void printStoredIMU();
|
||||
@@ -189,21 +189,21 @@ protected:
|
||||
outputSample _output_new;
|
||||
imuSample _imu_sample_new;
|
||||
|
||||
uint64_t _imu_ticks;
|
||||
uint64_t _imu_ticks;
|
||||
|
||||
bool _imu_updated = false;
|
||||
bool _initialised = false;
|
||||
bool _vehicle_armed = false; // vehicle arm status used to turn off functionality used on the ground
|
||||
bool _vehicle_armed = false; // vehicle arm status used to turn off functionality used on the ground
|
||||
|
||||
bool _NED_origin_initialised = false;
|
||||
bool _gps_speed_valid = false;
|
||||
struct map_projection_reference_s _pos_ref = {}; // Contains WGS-84 position latitude and longitude (radians)
|
||||
bool _NED_origin_initialised = false;
|
||||
bool _gps_speed_valid = false;
|
||||
struct map_projection_reference_s _pos_ref = {}; // Contains WGS-84 position latitude and longitude (radians)
|
||||
|
||||
bool _mag_healthy = false; // computed by mag innovation test
|
||||
float _yaw_test_ratio; // yaw innovation consistency check ratio
|
||||
float _mag_test_ratio[3]; // magnetometer XYZ innovation consistency check ratios
|
||||
float _yaw_test_ratio; // yaw innovation consistency check ratio
|
||||
float _mag_test_ratio[3]; // magnetometer XYZ innovation consistency check ratios
|
||||
|
||||
float _vel_pos_test_ratio[6]; // velocity and position innovation consistency check ratios
|
||||
float _vel_pos_test_ratio[6]; // velocity and position innovation consistency check ratios
|
||||
|
||||
RingBuffer<imuSample> _imu_buffer;
|
||||
RingBuffer<gpsSample> _gps_buffer;
|
||||
|
||||
+130
-122
@@ -54,28 +54,30 @@
|
||||
|
||||
bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps)
|
||||
{
|
||||
// run gps checks if we have not yet set the NED origin or have not started using GPS
|
||||
if(!_NED_origin_initialised || !_control_status.flags.gps) {
|
||||
// if we have good GPS data and the NED origin is not set, set to the last GPS fix
|
||||
if (gps_is_good(gps) && !_NED_origin_initialised) {
|
||||
printf("gps is good - setting EKF origin\n");
|
||||
// Initialise projection
|
||||
double lat = gps->lat / 1.0e7;
|
||||
double lon = gps->lon / 1.0e7;
|
||||
map_projection_init(&_pos_ref, lat, lon);
|
||||
_gps_alt_ref = gps->alt / 1e3f;
|
||||
_NED_origin_initialised = true;
|
||||
_last_gps_origin_time_us = _time_last_imu;
|
||||
}
|
||||
}
|
||||
// run gps checks if we have not yet set the NED origin or have not started using GPS
|
||||
if (!_NED_origin_initialised || !_control_status.flags.gps) {
|
||||
// if we have good GPS data and the NED origin is not set, set to the last GPS fix
|
||||
if (gps_is_good(gps) && !_NED_origin_initialised) {
|
||||
printf("gps is good - setting EKF origin\n");
|
||||
// Initialise projection
|
||||
double lat = gps->lat / 1.0e7;
|
||||
double lon = gps->lon / 1.0e7;
|
||||
map_projection_init(&_pos_ref, lat, lon);
|
||||
_gps_alt_ref = gps->alt / 1e3f;
|
||||
_NED_origin_initialised = true;
|
||||
_last_gps_origin_time_us = _time_last_imu;
|
||||
}
|
||||
}
|
||||
|
||||
// start collecting GPS if there is a 3D fix and the NED origin has been set
|
||||
if (_NED_origin_initialised && gps->fix_type >= 3) {
|
||||
return true;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
return false;
|
||||
// start collecting GPS if there is a 3D fix and the NED origin has been set
|
||||
if (_NED_origin_initialised && gps->fix_type >= 3) {
|
||||
return true;
|
||||
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
/*
|
||||
@@ -87,126 +89,132 @@ bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps)
|
||||
*/
|
||||
bool Ekf::gps_is_good(struct gps_message *gps)
|
||||
{
|
||||
// Check the fix type
|
||||
_gps_check_fail_status.flags.fix = (gps->fix_type < 3);
|
||||
// Check the fix type
|
||||
_gps_check_fail_status.flags.fix = (gps->fix_type < 3);
|
||||
|
||||
// Check the number of satellites
|
||||
_gps_check_fail_status.flags.nsats = (gps->nsats < _params.req_nsats);
|
||||
// Check the number of satellites
|
||||
_gps_check_fail_status.flags.nsats = (gps->nsats < _params.req_nsats);
|
||||
|
||||
// Check the geometric dilution of precision
|
||||
_gps_check_fail_status.flags.gdop = (gps->gdop > _params.req_gdop);
|
||||
// Check the geometric dilution of precision
|
||||
_gps_check_fail_status.flags.gdop = (gps->gdop > _params.req_gdop);
|
||||
|
||||
// Check the reported horizontal position accuracy
|
||||
_gps_check_fail_status.flags.hacc = (gps->eph > _params.req_hacc);
|
||||
// Check the reported horizontal position accuracy
|
||||
_gps_check_fail_status.flags.hacc = (gps->eph > _params.req_hacc);
|
||||
|
||||
// Check the reported vertical position accuracy
|
||||
_gps_check_fail_status.flags.vacc = (gps->epv > _params.req_vacc);
|
||||
// Check the reported vertical position accuracy
|
||||
_gps_check_fail_status.flags.vacc = (gps->epv > _params.req_vacc);
|
||||
|
||||
// Check the reported speed accuracy
|
||||
_gps_check_fail_status.flags.sacc = (gps->sacc > _params.req_sacc);
|
||||
// Check the reported speed accuracy
|
||||
_gps_check_fail_status.flags.sacc = (gps->sacc > _params.req_sacc);
|
||||
|
||||
// Calculate position movement since last measurement
|
||||
float delta_posN = 0.0f;
|
||||
float delta_PosE = 0.0f;
|
||||
double lat = gps->lat * 1.0e-7;
|
||||
double lon = gps->lon * 1.0e-7;
|
||||
if (_pos_ref.init_done) {
|
||||
map_projection_project(&_pos_ref, lat, lon, &delta_posN, &delta_PosE);
|
||||
} else {
|
||||
map_projection_init(&_pos_ref, lat, lon);
|
||||
_gps_alt_ref = gps->alt * 1e-3f;
|
||||
}
|
||||
// Calculate position movement since last measurement
|
||||
float delta_posN = 0.0f;
|
||||
float delta_PosE = 0.0f;
|
||||
double lat = gps->lat * 1.0e-7;
|
||||
double lon = gps->lon * 1.0e-7;
|
||||
|
||||
// Calculate time lapsed since last update, limit to prevent numerical errors and calculate the lowpass filter coefficient
|
||||
const float filt_time_const = 10.0f;
|
||||
float dt = fminf(fmaxf(float(_time_last_imu - _last_gps_origin_time_us)*1e-6f,0.001f),filt_time_const);
|
||||
float filter_coef = dt/filt_time_const;
|
||||
if (_pos_ref.init_done) {
|
||||
map_projection_project(&_pos_ref, lat, lon, &delta_posN, &delta_PosE);
|
||||
|
||||
// Calculate the horizontal drift velocity components and limit to 10x the threshold
|
||||
float vel_limit = 10.0f * _params.req_hdrift;
|
||||
float velN = fminf(fmaxf(delta_posN / dt, -vel_limit), vel_limit);
|
||||
float velE = fminf(fmaxf(delta_PosE / dt, -vel_limit), vel_limit);
|
||||
} else {
|
||||
map_projection_init(&_pos_ref, lat, lon);
|
||||
_gps_alt_ref = gps->alt * 1e-3f;
|
||||
}
|
||||
|
||||
// Apply a low pass filter
|
||||
_gpsDriftVelN = velN * filter_coef + _gpsDriftVelN * (1.0f - filter_coef);
|
||||
_gpsDriftVelE = velE * filter_coef + _gpsDriftVelE * (1.0f - filter_coef);
|
||||
// Calculate time lapsed since last update, limit to prevent numerical errors and calculate the lowpass filter coefficient
|
||||
const float filt_time_const = 10.0f;
|
||||
float dt = fminf(fmaxf(float(_time_last_imu - _last_gps_origin_time_us) * 1e-6f, 0.001f), filt_time_const);
|
||||
float filter_coef = dt / filt_time_const;
|
||||
|
||||
// Calculate the horizontal drift speed and fail if too high
|
||||
// This check can only be used if the vehicle is stationary during alignment
|
||||
if(!_control_status.flags.armed) {
|
||||
float drift_speed = sqrtf(_gpsDriftVelN * _gpsDriftVelN + _gpsDriftVelE * _gpsDriftVelE);
|
||||
_gps_check_fail_status.flags.hdrift = (drift_speed > _params.req_hdrift);
|
||||
} else {
|
||||
_gps_check_fail_status.flags.hdrift = false;
|
||||
}
|
||||
// Calculate the horizontal drift velocity components and limit to 10x the threshold
|
||||
float vel_limit = 10.0f * _params.req_hdrift;
|
||||
float velN = fminf(fmaxf(delta_posN / dt, -vel_limit), vel_limit);
|
||||
float velE = fminf(fmaxf(delta_PosE / dt, -vel_limit), vel_limit);
|
||||
|
||||
// Save current position as the reference for next time
|
||||
map_projection_init(&_pos_ref, lat, lon);
|
||||
_last_gps_origin_time_us = _time_last_imu;
|
||||
// Apply a low pass filter
|
||||
_gpsDriftVelN = velN * filter_coef + _gpsDriftVelN * (1.0f - filter_coef);
|
||||
_gpsDriftVelE = velE * filter_coef + _gpsDriftVelE * (1.0f - filter_coef);
|
||||
|
||||
// Calculate the vertical drift velocity and limit to 10x the threshold
|
||||
vel_limit = 10.0f * _params.req_vdrift;
|
||||
float velD = fminf(fmaxf((_gps_alt_ref - gps->alt * 1e-3f) / dt, -vel_limit), vel_limit);
|
||||
// Calculate the horizontal drift speed and fail if too high
|
||||
// This check can only be used if the vehicle is stationary during alignment
|
||||
if (!_control_status.flags.armed) {
|
||||
float drift_speed = sqrtf(_gpsDriftVelN * _gpsDriftVelN + _gpsDriftVelE * _gpsDriftVelE);
|
||||
_gps_check_fail_status.flags.hdrift = (drift_speed > _params.req_hdrift);
|
||||
|
||||
// Save the current height as the reference for next time
|
||||
_gps_alt_ref = gps->alt * 1e-3f;
|
||||
} else {
|
||||
_gps_check_fail_status.flags.hdrift = false;
|
||||
}
|
||||
|
||||
// Apply a low pass filter to the vertical velocity
|
||||
_gps_drift_velD = velD * filter_coef + _gps_drift_velD * (1.0f - filter_coef);
|
||||
// Save current position as the reference for next time
|
||||
map_projection_init(&_pos_ref, lat, lon);
|
||||
_last_gps_origin_time_us = _time_last_imu;
|
||||
|
||||
// Fail if the vertical drift speed is too high
|
||||
// This check can only be used if the vehicle is stationary during alignment
|
||||
if(!_control_status.flags.armed) {
|
||||
_gps_check_fail_status.flags.vdrift = (fabsf(_gps_drift_velD) > _params.req_vdrift);
|
||||
} else {
|
||||
_gps_check_fail_status.flags.vdrift = false;
|
||||
}
|
||||
// Calculate the vertical drift velocity and limit to 10x the threshold
|
||||
vel_limit = 10.0f * _params.req_vdrift;
|
||||
float velD = fminf(fmaxf((_gps_alt_ref - gps->alt * 1e-3f) / dt, -vel_limit), vel_limit);
|
||||
|
||||
// Check the magnitude of the filtered horizontal GPS velocity
|
||||
// This check can only be used if the vehicle is stationary during alignment
|
||||
if (!_control_status.flags.armed) {
|
||||
vel_limit = 10.0f * _params.req_hdrift;
|
||||
float velN = fminf(fmaxf(gps->vel_ned[0],-vel_limit),vel_limit);
|
||||
float velE = fminf(fmaxf(gps->vel_ned[1],-vel_limit),vel_limit);
|
||||
_gps_velN_filt = velN * filter_coef + _gps_velN_filt * (1.0f - filter_coef);
|
||||
_gps_velE_filt = velE * filter_coef + _gps_velE_filt * (1.0f - filter_coef);
|
||||
float horiz_speed = sqrtf(_gps_velN_filt * _gps_velN_filt + _gps_velE_filt * _gps_velE_filt);
|
||||
_gps_check_fail_status.flags.hspeed = (horiz_speed > _params.req_hdrift);
|
||||
} else {
|
||||
_gps_check_fail_status.flags.hspeed = false;
|
||||
}
|
||||
// Save the current height as the reference for next time
|
||||
_gps_alt_ref = gps->alt * 1e-3f;
|
||||
|
||||
// Check the filtered difference between GPS and EKF vertical velocity
|
||||
vel_limit = 10.0f * _params.req_vdrift;
|
||||
float vertVel = fminf(fmaxf((gps->vel_ned[2] - _state.vel(2)), -vel_limit), vel_limit);
|
||||
_gps_velD_diff_filt = vertVel * filter_coef + _gps_velD_diff_filt * (1.0f - filter_coef);
|
||||
_gps_check_fail_status.flags.vspeed = (fabsf(_gps_velD_diff_filt) > _params.req_vdrift);
|
||||
// Apply a low pass filter to the vertical velocity
|
||||
_gps_drift_velD = velD * filter_coef + _gps_drift_velD * (1.0f - filter_coef);
|
||||
|
||||
// assume failed first time through
|
||||
if (_last_gps_fail_us == 0) {
|
||||
_last_gps_fail_us = _time_last_imu;
|
||||
}
|
||||
// Fail if the vertical drift speed is too high
|
||||
// This check can only be used if the vehicle is stationary during alignment
|
||||
if (!_control_status.flags.armed) {
|
||||
_gps_check_fail_status.flags.vdrift = (fabsf(_gps_drift_velD) > _params.req_vdrift);
|
||||
|
||||
// if any user selected checks have failed, record the fail time
|
||||
if (
|
||||
_gps_check_fail_status.flags.fix ||
|
||||
(_gps_check_fail_status.flags.nsats && (_params.gps_check_mask & MASK_GPS_NSATS)) ||
|
||||
(_gps_check_fail_status.flags.gdop && (_params.gps_check_mask & MASK_GPS_GDOP)) ||
|
||||
(_gps_check_fail_status.flags.hacc && (_params.gps_check_mask & MASK_GPS_HACC)) ||
|
||||
(_gps_check_fail_status.flags.vacc && (_params.gps_check_mask & MASK_GPS_VACC)) ||
|
||||
(_gps_check_fail_status.flags.sacc && (_params.gps_check_mask & MASK_GPS_SACC)) ||
|
||||
(_gps_check_fail_status.flags.hdrift && (_params.gps_check_mask & MASK_GPS_HDRIFT)) ||
|
||||
(_gps_check_fail_status.flags.vdrift && (_params.gps_check_mask & MASK_GPS_VDRIFT)) ||
|
||||
(_gps_check_fail_status.flags.hspeed && (_params.gps_check_mask & MASK_GPS_HSPD)) ||
|
||||
(_gps_check_fail_status.flags.vspeed && (_params.gps_check_mask & MASK_GPS_VSPD))
|
||||
) {
|
||||
_last_gps_fail_us = _time_last_imu;
|
||||
}
|
||||
} else {
|
||||
_gps_check_fail_status.flags.vdrift = false;
|
||||
}
|
||||
|
||||
// continuous period without fail of 10 seconds required to return a healthy status
|
||||
if (_time_last_imu - _last_gps_fail_us > 1e7) {
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
// Check the magnitude of the filtered horizontal GPS velocity
|
||||
// This check can only be used if the vehicle is stationary during alignment
|
||||
if (!_control_status.flags.armed) {
|
||||
vel_limit = 10.0f * _params.req_hdrift;
|
||||
float velN = fminf(fmaxf(gps->vel_ned[0], -vel_limit), vel_limit);
|
||||
float velE = fminf(fmaxf(gps->vel_ned[1], -vel_limit), vel_limit);
|
||||
_gps_velN_filt = velN * filter_coef + _gps_velN_filt * (1.0f - filter_coef);
|
||||
_gps_velE_filt = velE * filter_coef + _gps_velE_filt * (1.0f - filter_coef);
|
||||
float horiz_speed = sqrtf(_gps_velN_filt * _gps_velN_filt + _gps_velE_filt * _gps_velE_filt);
|
||||
_gps_check_fail_status.flags.hspeed = (horiz_speed > _params.req_hdrift);
|
||||
|
||||
} else {
|
||||
_gps_check_fail_status.flags.hspeed = false;
|
||||
}
|
||||
|
||||
// Check the filtered difference between GPS and EKF vertical velocity
|
||||
vel_limit = 10.0f * _params.req_vdrift;
|
||||
float vertVel = fminf(fmaxf((gps->vel_ned[2] - _state.vel(2)), -vel_limit), vel_limit);
|
||||
_gps_velD_diff_filt = vertVel * filter_coef + _gps_velD_diff_filt * (1.0f - filter_coef);
|
||||
_gps_check_fail_status.flags.vspeed = (fabsf(_gps_velD_diff_filt) > _params.req_vdrift);
|
||||
|
||||
// assume failed first time through
|
||||
if (_last_gps_fail_us == 0) {
|
||||
_last_gps_fail_us = _time_last_imu;
|
||||
}
|
||||
|
||||
// if any user selected checks have failed, record the fail time
|
||||
if (
|
||||
_gps_check_fail_status.flags.fix ||
|
||||
(_gps_check_fail_status.flags.nsats && (_params.gps_check_mask & MASK_GPS_NSATS)) ||
|
||||
(_gps_check_fail_status.flags.gdop && (_params.gps_check_mask & MASK_GPS_GDOP)) ||
|
||||
(_gps_check_fail_status.flags.hacc && (_params.gps_check_mask & MASK_GPS_HACC)) ||
|
||||
(_gps_check_fail_status.flags.vacc && (_params.gps_check_mask & MASK_GPS_VACC)) ||
|
||||
(_gps_check_fail_status.flags.sacc && (_params.gps_check_mask & MASK_GPS_SACC)) ||
|
||||
(_gps_check_fail_status.flags.hdrift && (_params.gps_check_mask & MASK_GPS_HDRIFT)) ||
|
||||
(_gps_check_fail_status.flags.vdrift && (_params.gps_check_mask & MASK_GPS_VDRIFT)) ||
|
||||
(_gps_check_fail_status.flags.hspeed && (_params.gps_check_mask & MASK_GPS_HSPD)) ||
|
||||
(_gps_check_fail_status.flags.vspeed && (_params.gps_check_mask & MASK_GPS_VSPD))
|
||||
) {
|
||||
_last_gps_fail_us = _time_last_imu;
|
||||
}
|
||||
|
||||
// continuous period without fail of 10 seconds required to return a healthy status
|
||||
if (_time_last_imu - _last_gps_fail_us > 1e7) {
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
+382
-371
@@ -44,408 +44,419 @@
|
||||
|
||||
void Ekf::fuseMag()
|
||||
{
|
||||
// assign intermediate variables
|
||||
float q0 = _state.quat_nominal(0);
|
||||
float q1 = _state.quat_nominal(1);
|
||||
float q2 = _state.quat_nominal(2);
|
||||
float q3 = _state.quat_nominal(3);
|
||||
// assign intermediate variables
|
||||
float q0 = _state.quat_nominal(0);
|
||||
float q1 = _state.quat_nominal(1);
|
||||
float q2 = _state.quat_nominal(2);
|
||||
float q3 = _state.quat_nominal(3);
|
||||
|
||||
float magN = _state.mag_I(0);
|
||||
float magE = _state.mag_I(1);
|
||||
float magD = _state.mag_I(2);
|
||||
float magN = _state.mag_I(0);
|
||||
float magE = _state.mag_I(1);
|
||||
float magD = _state.mag_I(2);
|
||||
|
||||
// XYZ Measurement uncertainty. Need to consider timing errors for fast rotations
|
||||
float R_MAG = 1e-3f;
|
||||
// XYZ Measurement uncertainty. Need to consider timing errors for fast rotations
|
||||
float R_MAG = 1e-3f;
|
||||
|
||||
// intermediate variables from algebraic optimisation
|
||||
float SH_MAG[9];
|
||||
SH_MAG[0] = sq(q0) - sq(q1) + sq(q2) - sq(q3);
|
||||
SH_MAG[1] = sq(q0) + sq(q1) - sq(q2) - sq(q3);
|
||||
SH_MAG[2] = sq(q0) - sq(q1) - sq(q2) + sq(q3);
|
||||
SH_MAG[3] = 2 * q0 * q1 + 2 * q2 * q3;
|
||||
SH_MAG[4] = 2 * q0 * q3 + 2 * q1 * q2;
|
||||
SH_MAG[5] = 2 * q0 * q2 + 2 * q1 * q3;
|
||||
SH_MAG[6] = magE * (2 * q0 * q1 - 2 * q2 * q3);
|
||||
SH_MAG[7] = 2 * q1 * q3 - 2 * q0 * q2;
|
||||
SH_MAG[8] = 2 * q0 * q3;
|
||||
// intermediate variables from algebraic optimisation
|
||||
float SH_MAG[9];
|
||||
SH_MAG[0] = sq(q0) - sq(q1) + sq(q2) - sq(q3);
|
||||
SH_MAG[1] = sq(q0) + sq(q1) - sq(q2) - sq(q3);
|
||||
SH_MAG[2] = sq(q0) - sq(q1) - sq(q2) + sq(q3);
|
||||
SH_MAG[3] = 2 * q0 * q1 + 2 * q2 * q3;
|
||||
SH_MAG[4] = 2 * q0 * q3 + 2 * q1 * q2;
|
||||
SH_MAG[5] = 2 * q0 * q2 + 2 * q1 * q3;
|
||||
SH_MAG[6] = magE * (2 * q0 * q1 - 2 * q2 * q3);
|
||||
SH_MAG[7] = 2 * q1 * q3 - 2 * q0 * q2;
|
||||
SH_MAG[8] = 2 * q0 * q3;
|
||||
|
||||
// rotate magnetometer earth field state into body frame
|
||||
matrix::Dcm<float> R_to_body(_state.quat_nominal);
|
||||
R_to_body = R_to_body.transpose();
|
||||
// rotate magnetometer earth field state into body frame
|
||||
matrix::Dcm<float> R_to_body(_state.quat_nominal);
|
||||
R_to_body = R_to_body.transpose();
|
||||
|
||||
Vector3f mag_I_rot = R_to_body * _state.mag_I;
|
||||
Vector3f mag_I_rot = R_to_body * _state.mag_I;
|
||||
|
||||
// compute magnetometer innovations
|
||||
_mag_innov[0] = (mag_I_rot(0) + _state.mag_B(0)) - _mag_sample_delayed.mag(0);
|
||||
_mag_innov[1] = (mag_I_rot(1) + _state.mag_B(1)) - _mag_sample_delayed.mag(1);
|
||||
_mag_innov[2] = (mag_I_rot(2) + _state.mag_B(2)) - _mag_sample_delayed.mag(2);
|
||||
// compute magnetometer innovations
|
||||
_mag_innov[0] = (mag_I_rot(0) + _state.mag_B(0)) - _mag_sample_delayed.mag(0);
|
||||
_mag_innov[1] = (mag_I_rot(1) + _state.mag_B(1)) - _mag_sample_delayed.mag(1);
|
||||
_mag_innov[2] = (mag_I_rot(2) + _state.mag_B(2)) - _mag_sample_delayed.mag(2);
|
||||
|
||||
// Note that although the observation jacobians and kalman gains are decalred as arrays
|
||||
// sequential fusion of the X,Y and Z components is used.
|
||||
float H_MAG[3][24] = {};
|
||||
float Kfusion[24] = {};
|
||||
// Note that although the observation jacobians and kalman gains are decalred as arrays
|
||||
// sequential fusion of the X,Y and Z components is used.
|
||||
float H_MAG[3][24] = {};
|
||||
float Kfusion[24] = {};
|
||||
|
||||
// Calculate observation Jacobians and kalman gains for each magentoemter axis
|
||||
// X Axis
|
||||
H_MAG[0][1] = SH_MAG[6] - magD * SH_MAG[2] - magN * SH_MAG[5];
|
||||
H_MAG[0][2] = magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2);
|
||||
H_MAG[0][16] = SH_MAG[1];
|
||||
H_MAG[0][17] = SH_MAG[4];
|
||||
H_MAG[0][18] = SH_MAG[7];
|
||||
H_MAG[0][19] = 1;
|
||||
// Calculate observation Jacobians and kalman gains for each magentoemter axis
|
||||
// X Axis
|
||||
H_MAG[0][1] = SH_MAG[6] - magD * SH_MAG[2] - magN * SH_MAG[5];
|
||||
H_MAG[0][2] = magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2);
|
||||
H_MAG[0][16] = SH_MAG[1];
|
||||
H_MAG[0][17] = SH_MAG[4];
|
||||
H_MAG[0][18] = SH_MAG[7];
|
||||
H_MAG[0][19] = 1;
|
||||
|
||||
// intermediate variables
|
||||
float SK_MX[4] = {};
|
||||
// innovation variance
|
||||
_mag_innov_var[0] = (P[19][19] + R_MAG - P[1][19] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[16][19] * SH_MAG[1]
|
||||
+ P[17][19] * SH_MAG[4] + P[18][19] * SH_MAG[7] + P[2][19] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN *
|
||||
(SH_MAG[8] - 2 * q1 * q2)) - (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) * (P[19][1] - P[1][1] *
|
||||
(magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[16][1] * SH_MAG[1] + P[17][1] * SH_MAG[4] + P[18][1] * SH_MAG[7] +
|
||||
P[2][1] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2))) + SH_MAG[1] *
|
||||
(P[19][16] - P[1][16] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[16][16] * SH_MAG[1] + P[17][16] *
|
||||
SH_MAG[4] + P[18][16] * SH_MAG[7] + P[2][16] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN *
|
||||
(SH_MAG[8] - 2 * q1 * q2))) + SH_MAG[4] * (P[19][17] - P[1][17] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) +
|
||||
P[16][17] * SH_MAG[1] + P[17][17] * SH_MAG[4] + P[18][17] * SH_MAG[7] + P[2][17] * (magE * SH_MAG[0] + magD * SH_MAG[3]
|
||||
- magN * (SH_MAG[8] - 2 * q1 * q2))) + SH_MAG[7] * (P[19][18] - P[1][18] * (magD * SH_MAG[2] - SH_MAG[6] + magN *
|
||||
SH_MAG[5]) + P[16][18] * SH_MAG[1] + P[17][18] * SH_MAG[4] + P[18][18] * SH_MAG[7] + P[2][18] *
|
||||
(magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2))) + (magE * SH_MAG[0] + magD * SH_MAG[3] -
|
||||
magN * (SH_MAG[8] - 2 * q1 * q2)) * (P[19][2] - P[1][2] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[16][2] *
|
||||
SH_MAG[1] + P[17][2] * SH_MAG[4] + P[18][2] * SH_MAG[7] + P[2][2] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN *
|
||||
(SH_MAG[8] - 2 * q1 * q2))));
|
||||
// intermediate variables
|
||||
float SK_MX[4] = {};
|
||||
// innovation variance
|
||||
_mag_innov_var[0] = (P[19][19] + R_MAG - P[1][19] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[16][19] *
|
||||
SH_MAG[1]
|
||||
+ P[17][19] * SH_MAG[4] + P[18][19] * SH_MAG[7] + P[2][19] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN *
|
||||
(SH_MAG[8] - 2 * q1 * q2)) - (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) * (P[19][1] - P[1][1] *
|
||||
(magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[16][1] * SH_MAG[1] + P[17][1] * SH_MAG[4] + P[18][1] * SH_MAG[7] +
|
||||
P[2][1] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2))) + SH_MAG[1] *
|
||||
(P[19][16] - P[1][16] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[16][16] * SH_MAG[1] + P[17][16] *
|
||||
SH_MAG[4] + P[18][16] * SH_MAG[7] + P[2][16] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN *
|
||||
(SH_MAG[8] - 2 * q1 * q2))) + SH_MAG[4] * (P[19][17] - P[1][17] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) +
|
||||
P[16][17] * SH_MAG[1] + P[17][17] * SH_MAG[4] + P[18][17] * SH_MAG[7] + P[2][17] * (magE * SH_MAG[0] + magD * SH_MAG[3]
|
||||
- magN * (SH_MAG[8] - 2 * q1 * q2))) + SH_MAG[7] * (P[19][18] - P[1][18] * (magD * SH_MAG[2] - SH_MAG[6] + magN *
|
||||
SH_MAG[5]) + P[16][18] * SH_MAG[1] + P[17][18] * SH_MAG[4] + P[18][18] * SH_MAG[7] + P[2][18] *
|
||||
(magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2))) + (magE * SH_MAG[0] + magD * SH_MAG[3] -
|
||||
magN * (SH_MAG[8] - 2 * q1 * q2)) * (P[19][2] - P[1][2] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[16][2] *
|
||||
SH_MAG[1] + P[17][2] * SH_MAG[4] + P[18][2] * SH_MAG[7] + P[2][2] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN *
|
||||
(SH_MAG[8] - 2 * q1 * q2))));
|
||||
|
||||
// check for a badly conditioned covariance matrix
|
||||
if (_mag_innov_var[0] >= R_MAG) {
|
||||
// the innovation variance contribution from the state covariances is non-negative - no fault
|
||||
_fault_status.bad_mag_x = false;
|
||||
} else {
|
||||
// the innovation variance contribution from the state covariances is negtive which means the covariance matrix is badly conditioned
|
||||
_fault_status.bad_mag_x = true;
|
||||
// we need to reinitialise the covariance matrix and abort this fusion step
|
||||
initialiseCovariance();
|
||||
return;
|
||||
}
|
||||
// check for a badly conditioned covariance matrix
|
||||
if (_mag_innov_var[0] >= R_MAG) {
|
||||
// the innovation variance contribution from the state covariances is non-negative - no fault
|
||||
_fault_status.bad_mag_x = false;
|
||||
|
||||
} else {
|
||||
// the innovation variance contribution from the state covariances is negtive which means the covariance matrix is badly conditioned
|
||||
_fault_status.bad_mag_x = true;
|
||||
// we need to reinitialise the covariance matrix and abort this fusion step
|
||||
initialiseCovariance();
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
// Y axis
|
||||
// Y axis
|
||||
|
||||
H_MAG[1][0] = magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5];
|
||||
H_MAG[1][2] = - magE * SH_MAG[4] - magD * SH_MAG[7] - magN * SH_MAG[1];
|
||||
H_MAG[1][16] = 2 * q1 * q2 - SH_MAG[8];
|
||||
H_MAG[1][17] = SH_MAG[0];
|
||||
H_MAG[1][18] = SH_MAG[3];
|
||||
H_MAG[1][20] = 1;
|
||||
H_MAG[1][0] = magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5];
|
||||
H_MAG[1][2] = - magE * SH_MAG[4] - magD * SH_MAG[7] - magN * SH_MAG[1];
|
||||
H_MAG[1][16] = 2 * q1 * q2 - SH_MAG[8];
|
||||
H_MAG[1][17] = SH_MAG[0];
|
||||
H_MAG[1][18] = SH_MAG[3];
|
||||
H_MAG[1][20] = 1;
|
||||
|
||||
// intermediate variables - note SK_MY[0] is 1/(innovation variance)
|
||||
float SK_MY[4];
|
||||
_mag_innov_var[1] = (P[20][20] + R_MAG + P[0][20] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[17][20] * SH_MAG[0]
|
||||
+ P[18][20] * SH_MAG[3] - (SH_MAG[8] - 2 * q1 * q2) * (P[20][16] + P[0][16] * (magD * SH_MAG[2] - SH_MAG[6] + magN *
|
||||
SH_MAG[5]) + P[17][16] * SH_MAG[0] + P[18][16] * SH_MAG[3] - P[2][16] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN *
|
||||
SH_MAG[1]) - P[16][16] * (SH_MAG[8] - 2 * q1 * q2)) - P[2][20] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN *
|
||||
SH_MAG[1]) + (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) * (P[20][0] + P[0][0] *
|
||||
(magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[17][0] * SH_MAG[0] + P[18][0] * SH_MAG[3] - P[2][0] *
|
||||
(magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[16][0] * (SH_MAG[8] - 2 * q1 * q2)) + SH_MAG[0] *
|
||||
(P[20][17] + P[0][17] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[17][17] * SH_MAG[0] + P[18][17] *
|
||||
SH_MAG[3] - P[2][17] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[16][17] *
|
||||
(SH_MAG[8] - 2 * q1 * q2)) + SH_MAG[3] * (P[20][18] + P[0][18] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) +
|
||||
P[17][18] * SH_MAG[0] + P[18][18] * SH_MAG[3] - P[2][18] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) -
|
||||
P[16][18] * (SH_MAG[8] - 2 * q1 * q2)) - P[16][20] * (SH_MAG[8] - 2 * q1 * q2) - (magE * SH_MAG[4] + magD * SH_MAG[7] +
|
||||
magN * SH_MAG[1]) * (P[20][2] + P[0][2] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[17][2] * SH_MAG[0] +
|
||||
P[18][2] * SH_MAG[3] - P[2][2] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[16][2] *
|
||||
(SH_MAG[8] - 2 * q1 * q2)));
|
||||
// intermediate variables - note SK_MY[0] is 1/(innovation variance)
|
||||
float SK_MY[4];
|
||||
_mag_innov_var[1] = (P[20][20] + R_MAG + P[0][20] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[17][20] *
|
||||
SH_MAG[0]
|
||||
+ P[18][20] * SH_MAG[3] - (SH_MAG[8] - 2 * q1 * q2) * (P[20][16] + P[0][16] * (magD * SH_MAG[2] - SH_MAG[6] + magN *
|
||||
SH_MAG[5]) + P[17][16] * SH_MAG[0] + P[18][16] * SH_MAG[3] - P[2][16] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN *
|
||||
SH_MAG[1]) - P[16][16] * (SH_MAG[8] - 2 * q1 * q2)) - P[2][20] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN *
|
||||
SH_MAG[1]) + (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) * (P[20][0] + P[0][0] *
|
||||
(magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[17][0] * SH_MAG[0] + P[18][0] * SH_MAG[3] - P[2][0] *
|
||||
(magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[16][0] * (SH_MAG[8] - 2 * q1 * q2)) + SH_MAG[0] *
|
||||
(P[20][17] + P[0][17] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[17][17] * SH_MAG[0] + P[18][17] *
|
||||
SH_MAG[3] - P[2][17] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[16][17] *
|
||||
(SH_MAG[8] - 2 * q1 * q2)) + SH_MAG[3] * (P[20][18] + P[0][18] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) +
|
||||
P[17][18] * SH_MAG[0] + P[18][18] * SH_MAG[3] - P[2][18] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) -
|
||||
P[16][18] * (SH_MAG[8] - 2 * q1 * q2)) - P[16][20] * (SH_MAG[8] - 2 * q1 * q2) - (magE * SH_MAG[4] + magD * SH_MAG[7] +
|
||||
magN * SH_MAG[1]) * (P[20][2] + P[0][2] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[17][2] * SH_MAG[0] +
|
||||
P[18][2] * SH_MAG[3] - P[2][2] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[16][2] *
|
||||
(SH_MAG[8] - 2 * q1 * q2)));
|
||||
|
||||
// check for a badly conditioned covariance matrix
|
||||
if (_mag_innov_var[1] >= R_MAG) {
|
||||
// the innovation variance contribution from the state covariances is non-negative - no fault
|
||||
_fault_status.bad_mag_y = false;
|
||||
} else {
|
||||
// the innovation variance contribution from the state covariances is negtive which means the covariance matrix is badly conditioned
|
||||
_fault_status.bad_mag_y = true;
|
||||
// we need to reinitialise the covariance matrix and abort this fusion step
|
||||
initialiseCovariance();
|
||||
return;
|
||||
}
|
||||
// check for a badly conditioned covariance matrix
|
||||
if (_mag_innov_var[1] >= R_MAG) {
|
||||
// the innovation variance contribution from the state covariances is non-negative - no fault
|
||||
_fault_status.bad_mag_y = false;
|
||||
|
||||
} else {
|
||||
// the innovation variance contribution from the state covariances is negtive which means the covariance matrix is badly conditioned
|
||||
_fault_status.bad_mag_y = true;
|
||||
// we need to reinitialise the covariance matrix and abort this fusion step
|
||||
initialiseCovariance();
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
// Z axis
|
||||
// Z axis
|
||||
|
||||
H_MAG[2][0] = magN * (SH_MAG[8] - 2 * q1 * q2) - magD * SH_MAG[3] - magE * SH_MAG[0];
|
||||
H_MAG[2][1] = magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1];
|
||||
H_MAG[2][16] = SH_MAG[5];
|
||||
H_MAG[2][17] = 2 * q2 * q3 - 2 * q0 * q1;
|
||||
H_MAG[2][18] = SH_MAG[2];
|
||||
H_MAG[2][21] = 1;
|
||||
H_MAG[2][0] = magN * (SH_MAG[8] - 2 * q1 * q2) - magD * SH_MAG[3] - magE * SH_MAG[0];
|
||||
H_MAG[2][1] = magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1];
|
||||
H_MAG[2][16] = SH_MAG[5];
|
||||
H_MAG[2][17] = 2 * q2 * q3 - 2 * q0 * q1;
|
||||
H_MAG[2][18] = SH_MAG[2];
|
||||
H_MAG[2][21] = 1;
|
||||
|
||||
// intermediate variables
|
||||
float SK_MZ[4];
|
||||
_mag_innov_var[2] = (P[21][21] + R_MAG + P[16][21] * SH_MAG[5] + P[18][21] * SH_MAG[2] - (2 * q0 * q1 - 2 * q2 * q3) *
|
||||
(P[21][17] + P[16][17] * SH_MAG[5] + P[18][17] * SH_MAG[2] - P[0][17] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN *
|
||||
(SH_MAG[8] - 2 * q1 * q2)) + P[1][17] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[17][17] *
|
||||
(2 * q0 * q1 - 2 * q2 * q3)) - P[0][21] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN *
|
||||
(SH_MAG[8] - 2 * q1 * q2)) + P[1][21] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) + SH_MAG[5] *
|
||||
(P[21][16] + P[16][16] * SH_MAG[5] + P[18][16] * SH_MAG[2] - P[0][16] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN *
|
||||
(SH_MAG[8] - 2 * q1 * q2)) + P[1][16] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[17][16] *
|
||||
(2 * q0 * q1 - 2 * q2 * q3)) + SH_MAG[2] * (P[21][18] + P[16][18] * SH_MAG[5] + P[18][18] * SH_MAG[2] - P[0][18] *
|
||||
(magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2)) + P[1][18] *
|
||||
(magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[17][18] * (2 * q0 * q1 - 2 * q2 * q3)) -
|
||||
(magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2)) * (P[21][0] + P[16][0] * SH_MAG[5] + P[18][0] *
|
||||
SH_MAG[2] - P[0][0] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2)) + P[1][0] *
|
||||
(magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[17][0] * (2 * q0 * q1 - 2 * q2 * q3)) - P[17][21] *
|
||||
(2 * q0 * q1 - 2 * q2 * q3) + (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) *
|
||||
(P[21][1] + P[16][1] * SH_MAG[5] + P[18][1] * SH_MAG[2] - P[0][1] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN *
|
||||
(SH_MAG[8] - 2 * q1 * q2)) + P[1][1] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[17][1] *
|
||||
(2 * q0 * q1 - 2 * q2 * q3)));
|
||||
// intermediate variables
|
||||
float SK_MZ[4];
|
||||
_mag_innov_var[2] = (P[21][21] + R_MAG + P[16][21] * SH_MAG[5] + P[18][21] * SH_MAG[2] - (2 * q0 * q1 - 2 * q2 * q3) *
|
||||
(P[21][17] + P[16][17] * SH_MAG[5] + P[18][17] * SH_MAG[2] - P[0][17] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN *
|
||||
(SH_MAG[8] - 2 * q1 * q2)) + P[1][17] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[17][17] *
|
||||
(2 * q0 * q1 - 2 * q2 * q3)) - P[0][21] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN *
|
||||
(SH_MAG[8] - 2 * q1 * q2)) + P[1][21] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) + SH_MAG[5] *
|
||||
(P[21][16] + P[16][16] * SH_MAG[5] + P[18][16] * SH_MAG[2] - P[0][16] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN *
|
||||
(SH_MAG[8] - 2 * q1 * q2)) + P[1][16] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[17][16] *
|
||||
(2 * q0 * q1 - 2 * q2 * q3)) + SH_MAG[2] * (P[21][18] + P[16][18] * SH_MAG[5] + P[18][18] * SH_MAG[2] - P[0][18] *
|
||||
(magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2)) + P[1][18] *
|
||||
(magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[17][18] * (2 * q0 * q1 - 2 * q2 * q3)) -
|
||||
(magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2)) * (P[21][0] + P[16][0] * SH_MAG[5] + P[18][0] *
|
||||
SH_MAG[2] - P[0][0] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2)) + P[1][0] *
|
||||
(magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[17][0] * (2 * q0 * q1 - 2 * q2 * q3)) - P[17][21] *
|
||||
(2 * q0 * q1 - 2 * q2 * q3) + (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) *
|
||||
(P[21][1] + P[16][1] * SH_MAG[5] + P[18][1] * SH_MAG[2] - P[0][1] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN *
|
||||
(SH_MAG[8] - 2 * q1 * q2)) + P[1][1] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[17][1] *
|
||||
(2 * q0 * q1 - 2 * q2 * q3)));
|
||||
|
||||
// check for a badly conditioned covariance matrix
|
||||
if (_mag_innov_var[2] >= R_MAG) {
|
||||
// the innovation variance contribution from the state covariances is non-negative - no fault
|
||||
_fault_status.bad_mag_z = false;
|
||||
} else {
|
||||
// the innovation variance contribution from the state covariances is negtive which means the covariance matrix is badly conditioned
|
||||
_fault_status.bad_mag_z = true;
|
||||
// we need to reinitialise the covariance matrix and abort this fusion step
|
||||
initialiseCovariance();
|
||||
return;
|
||||
}
|
||||
// check for a badly conditioned covariance matrix
|
||||
if (_mag_innov_var[2] >= R_MAG) {
|
||||
// the innovation variance contribution from the state covariances is non-negative - no fault
|
||||
_fault_status.bad_mag_z = false;
|
||||
|
||||
} else {
|
||||
// the innovation variance contribution from the state covariances is negtive which means the covariance matrix is badly conditioned
|
||||
_fault_status.bad_mag_z = true;
|
||||
// we need to reinitialise the covariance matrix and abort this fusion step
|
||||
initialiseCovariance();
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
// Perform an innovation consistency check on each measurement and if one axis fails
|
||||
// do not fuse any data from the sensor because the most common errors affect multiple axes.
|
||||
_mag_healthy = true;
|
||||
for (uint8_t index=0; index<=2; index++) {
|
||||
_mag_test_ratio[index] = sq(_mag_innov[index]) / (sq(math::max(_params.mag_innov_gate, 1.0f)) * _mag_innov_var[index]);
|
||||
if (_mag_test_ratio[index] > 1.0f) {
|
||||
_mag_healthy = false;
|
||||
}
|
||||
}
|
||||
if (!_mag_healthy) {
|
||||
return;
|
||||
}
|
||||
// Perform an innovation consistency check on each measurement and if one axis fails
|
||||
// do not fuse any data from the sensor because the most common errors affect multiple axes.
|
||||
_mag_healthy = true;
|
||||
|
||||
// update the states and covariance usinng sequential fusion of the magnetometer components
|
||||
for (uint8_t index=0; index<=2; index++) {
|
||||
// Calculate Kalman gains
|
||||
if (index == 0) {
|
||||
// Calculate X axis Kalman gains
|
||||
SK_MX[0] = 1.0f / _mag_innov_var[0];
|
||||
SK_MX[1] = magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2);
|
||||
SK_MX[2] = magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5];
|
||||
SK_MX[3] = SH_MAG[7];
|
||||
for (uint8_t index = 0; index <= 2; index++) {
|
||||
_mag_test_ratio[index] = sq(_mag_innov[index]) / (sq(math::max(_params.mag_innov_gate, 1.0f)) * _mag_innov_var[index]);
|
||||
|
||||
Kfusion[0] = SK_MX[0] * (P[0][19] + P[0][16] * SH_MAG[1] + P[0][17] * SH_MAG[4] - P[0][1] * SK_MX[2] + P[0][2] *
|
||||
SK_MX[1] + P[0][18] * SK_MX[3]);
|
||||
Kfusion[1] = SK_MX[0] * (P[1][19] + P[1][16] * SH_MAG[1] + P[1][17] * SH_MAG[4] - P[1][1] * SK_MX[2] + P[1][2] *
|
||||
SK_MX[1] + P[1][18] * SK_MX[3]);
|
||||
Kfusion[2] = SK_MX[0] * (P[2][19] + P[2][16] * SH_MAG[1] + P[2][17] * SH_MAG[4] - P[2][1] * SK_MX[2] + P[2][2] *
|
||||
SK_MX[1] + P[2][18] * SK_MX[3]);
|
||||
Kfusion[3] = SK_MX[0] * (P[3][19] + P[3][16] * SH_MAG[1] + P[3][17] * SH_MAG[4] - P[3][1] * SK_MX[2] + P[3][2] *
|
||||
SK_MX[1] + P[3][18] * SK_MX[3]);
|
||||
Kfusion[4] = SK_MX[0] * (P[4][19] + P[4][16] * SH_MAG[1] + P[4][17] * SH_MAG[4] - P[4][1] * SK_MX[2] + P[4][2] *
|
||||
SK_MX[1] + P[4][18] * SK_MX[3]);
|
||||
Kfusion[5]= SK_MX[0] * (P[5][19] + P[5][16] * SH_MAG[1] + P[5][17] * SH_MAG[4] - P[5][1] * SK_MX[2] + P[5][2] *
|
||||
SK_MX[1] + P[5][18] * SK_MX[3]);
|
||||
Kfusion[6] = SK_MX[0] * (P[6][19] + P[6][16] * SH_MAG[1] + P[6][17] * SH_MAG[4] - P[6][1] * SK_MX[2] + P[6][2] *
|
||||
SK_MX[1] + P[6][18] * SK_MX[3]);
|
||||
Kfusion[7] = SK_MX[0] * (P[7][19] + P[7][16] * SH_MAG[1] + P[7][17] * SH_MAG[4] - P[7][1] * SK_MX[2] + P[7][2] *
|
||||
SK_MX[1] + P[7][18] * SK_MX[3]);
|
||||
Kfusion[8] = SK_MX[0] * (P[8][19] + P[8][16] * SH_MAG[1] + P[8][17] * SH_MAG[4] - P[8][1] * SK_MX[2] + P[8][2] *
|
||||
SK_MX[1] + P[8][18] * SK_MX[3]);
|
||||
Kfusion[9] = SK_MX[0] * (P[9][19] + P[9][16] * SH_MAG[1] + P[9][17] * SH_MAG[4] - P[9][1] * SK_MX[2] + P[9][2] *
|
||||
SK_MX[1] + P[9][18] * SK_MX[3]);
|
||||
Kfusion[10] = SK_MX[0] * (P[10][19] + P[10][16] * SH_MAG[1] + P[10][17] * SH_MAG[4] - P[10][1] * SK_MX[2] + P[10][2] *
|
||||
SK_MX[1] + P[10][18] * SK_MX[3]);
|
||||
Kfusion[11] = SK_MX[0] * (P[11][19] + P[11][16] * SH_MAG[1] + P[11][17] * SH_MAG[4] - P[11][1] * SK_MX[2] + P[11][2] *
|
||||
SK_MX[1] + P[11][18] * SK_MX[3]);
|
||||
Kfusion[12] = SK_MX[0] * (P[12][19] + P[12][16] * SH_MAG[1] + P[12][17] * SH_MAG[4] - P[12][1] * SK_MX[2] + P[12][2] *
|
||||
SK_MX[1] + P[12][18] * SK_MX[3]);
|
||||
Kfusion[13] = SK_MX[0] * (P[13][19] + P[13][16] * SH_MAG[1] + P[13][17] * SH_MAG[4] - P[13][1] * SK_MX[2] + P[13][2] *
|
||||
SK_MX[1] + P[13][18] * SK_MX[3]);
|
||||
Kfusion[14] = SK_MX[0] * (P[14][19] + P[14][16] * SH_MAG[1] + P[14][17] * SH_MAG[4] - P[14][1] * SK_MX[2] + P[14][2] *
|
||||
SK_MX[1] + P[14][18] * SK_MX[3]);
|
||||
Kfusion[15] = SK_MX[0] * (P[15][19] + P[15][16] * SH_MAG[1] + P[15][17] * SH_MAG[4] - P[15][1] * SK_MX[2] + P[15][2] *
|
||||
SK_MX[1] + P[15][18] * SK_MX[3]);
|
||||
Kfusion[16] = SK_MX[0] * (P[16][19] + P[16][16] * SH_MAG[1] + P[16][17] * SH_MAG[4] - P[16][1] * SK_MX[2] + P[16][2] *
|
||||
SK_MX[1] + P[16][18] * SK_MX[3]);
|
||||
Kfusion[17] = SK_MX[0] * (P[17][19] + P[17][16] * SH_MAG[1] + P[17][17] * SH_MAG[4] - P[17][1] * SK_MX[2] + P[17][2] *
|
||||
SK_MX[1] + P[17][18] * SK_MX[3]);
|
||||
Kfusion[18] = SK_MX[0] * (P[18][19] + P[18][16] * SH_MAG[1] + P[18][17] * SH_MAG[4] - P[18][1] * SK_MX[2] + P[18][2] *
|
||||
SK_MX[1] + P[18][18] * SK_MX[3]);
|
||||
Kfusion[19] = SK_MX[0] * (P[19][19] + P[19][16] * SH_MAG[1] + P[19][17] * SH_MAG[4] - P[19][1] * SK_MX[2] + P[19][2] *
|
||||
SK_MX[1] + P[19][18] * SK_MX[3]);
|
||||
Kfusion[20] = SK_MX[0] * (P[20][19] + P[20][16] * SH_MAG[1] + P[20][17] * SH_MAG[4] - P[20][1] * SK_MX[2] + P[20][2] *
|
||||
SK_MX[1] + P[20][18] * SK_MX[3]);
|
||||
Kfusion[21] = SK_MX[0] * (P[21][19] + P[21][16] * SH_MAG[1] + P[21][17] * SH_MAG[4] - P[21][1] * SK_MX[2] + P[21][2] *
|
||||
SK_MX[1] + P[21][18] * SK_MX[3]);
|
||||
Kfusion[22] = SK_MX[0] * (P[22][19] + P[22][16] * SH_MAG[1] + P[22][17] * SH_MAG[4] - P[22][1] * SK_MX[2] + P[22][2] *
|
||||
SK_MX[1] + P[22][18] * SK_MX[3]);
|
||||
Kfusion[23] = SK_MX[0] * (P[23][19] + P[23][16] * SH_MAG[1] + P[23][17] * SH_MAG[4] - P[23][1] * SK_MX[2] + P[23][2] *
|
||||
SK_MX[1] + P[23][18] * SK_MX[3]);
|
||||
} else if (index == 1) {
|
||||
// Calculate Y axis Kalman gains
|
||||
SK_MY[0] = 1.0f / _mag_innov_var[1];
|
||||
SK_MY[1] = magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1];
|
||||
SK_MY[2] = magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5];
|
||||
SK_MY[3] = SH_MAG[8] - 2 * q1 * q2;
|
||||
if (_mag_test_ratio[index] > 1.0f) {
|
||||
_mag_healthy = false;
|
||||
}
|
||||
}
|
||||
|
||||
Kfusion[0] = SK_MY[0] * (P[0][20] + P[0][17] * SH_MAG[0] + P[0][18] * SH_MAG[3] + P[0][0] * SK_MY[2] - P[0][2] *
|
||||
SK_MY[1] - P[0][16] * SK_MY[3]);
|
||||
Kfusion[1] = SK_MY[0] * (P[1][20] + P[1][17] * SH_MAG[0] + P[1][18] * SH_MAG[3] + P[1][0] * SK_MY[2] - P[1][2] *
|
||||
SK_MY[1] - P[1][16] * SK_MY[3]);
|
||||
Kfusion[2] = SK_MY[0] * (P[2][20] + P[2][17] * SH_MAG[0] + P[2][18] * SH_MAG[3] + P[2][0] * SK_MY[2] - P[2][2] *
|
||||
SK_MY[1] - P[2][16] * SK_MY[3]);
|
||||
Kfusion[3] = SK_MY[0] * (P[3][20] + P[3][17] * SH_MAG[0] + P[3][18] * SH_MAG[3] + P[3][0] * SK_MY[2] - P[3][2] *
|
||||
SK_MY[1] - P[3][16] * SK_MY[3]);
|
||||
Kfusion[4] = SK_MY[0] * (P[4][20] + P[4][17] * SH_MAG[0] + P[4][18] * SH_MAG[3] + P[4][0] * SK_MY[2] - P[4][2] *
|
||||
SK_MY[1] - P[4][16] * SK_MY[3]);
|
||||
Kfusion[5] = SK_MY[0] * (P[5][20] + P[5][17] * SH_MAG[0] + P[5][18] * SH_MAG[3] + P[5][0] * SK_MY[2] - P[5][2] *
|
||||
SK_MY[1] - P[5][16] * SK_MY[3]);
|
||||
Kfusion[6] = SK_MY[0] * (P[6][20] + P[6][17] * SH_MAG[0] + P[6][18] * SH_MAG[3] + P[6][0] * SK_MY[2] - P[6][2] *
|
||||
SK_MY[1] - P[6][16] * SK_MY[3]);
|
||||
Kfusion[7] = SK_MY[0] * (P[7][20] + P[7][17] * SH_MAG[0] + P[7][18] * SH_MAG[3] + P[7][0] * SK_MY[2] - P[7][2] *
|
||||
SK_MY[1] - P[7][16] * SK_MY[3]);
|
||||
Kfusion[8] = SK_MY[0] * (P[8][20] + P[8][17] * SH_MAG[0] + P[8][18] * SH_MAG[3] + P[8][0] * SK_MY[2] - P[8][2] *
|
||||
SK_MY[1] - P[8][16] * SK_MY[3]);
|
||||
Kfusion[9] = SK_MY[0] * (P[9][20] + P[9][17] * SH_MAG[0] + P[9][18] * SH_MAG[3] + P[9][0] * SK_MY[2] - P[9][2] *
|
||||
SK_MY[1] - P[9][16] * SK_MY[3]);
|
||||
Kfusion[10] = SK_MY[0] * (P[10][20] + P[10][17] * SH_MAG[0] + P[10][18] * SH_MAG[3] + P[10][0] * SK_MY[2] - P[10][2] *
|
||||
SK_MY[1] - P[10][16] * SK_MY[3]);
|
||||
Kfusion[11] = SK_MY[0] * (P[11][20] + P[11][17] * SH_MAG[0] + P[11][18] * SH_MAG[3] + P[11][0] * SK_MY[2] - P[11][2] *
|
||||
SK_MY[1] - P[11][16] * SK_MY[3]);
|
||||
Kfusion[12] = SK_MY[0] * (P[12][20] + P[12][17] * SH_MAG[0] + P[12][18] * SH_MAG[3] + P[12][0] * SK_MY[2] - P[12][2] *
|
||||
SK_MY[1] - P[12][16] * SK_MY[3]);
|
||||
Kfusion[13] = SK_MY[0] * (P[13][20] + P[13][17] * SH_MAG[0] + P[13][18] * SH_MAG[3] + P[13][0] * SK_MY[2] - P[13][2] *
|
||||
SK_MY[1] - P[13][16] * SK_MY[3]);
|
||||
Kfusion[14] = SK_MY[0] * (P[14][20] + P[14][17] * SH_MAG[0] + P[14][18] * SH_MAG[3] + P[14][0] * SK_MY[2] - P[14][2] *
|
||||
SK_MY[1] - P[14][16] * SK_MY[3]);
|
||||
Kfusion[15] = SK_MY[0] * (P[15][20] + P[15][17] * SH_MAG[0] + P[15][18] * SH_MAG[3] + P[15][0] * SK_MY[2] - P[15][2] *
|
||||
SK_MY[1] - P[15][16] * SK_MY[3]);
|
||||
Kfusion[16] = SK_MY[0] * (P[16][20] + P[16][17] * SH_MAG[0] + P[16][18] * SH_MAG[3] + P[16][0] * SK_MY[2] - P[16][2] *
|
||||
SK_MY[1] - P[16][16] * SK_MY[3]);
|
||||
Kfusion[17] = SK_MY[0] * (P[17][20] + P[17][17] * SH_MAG[0] + P[17][18] * SH_MAG[3] + P[17][0] * SK_MY[2] - P[17][2] *
|
||||
SK_MY[1] - P[17][16] * SK_MY[3]);
|
||||
Kfusion[18] = SK_MY[0] * (P[18][20] + P[18][17] * SH_MAG[0] + P[18][18] * SH_MAG[3] + P[18][0] * SK_MY[2] - P[18][2] *
|
||||
SK_MY[1] - P[18][16] * SK_MY[3]);
|
||||
Kfusion[19] = SK_MY[0] * (P[19][20] + P[19][17] * SH_MAG[0] + P[19][18] * SH_MAG[3] + P[19][0] * SK_MY[2] - P[19][2] *
|
||||
SK_MY[1] - P[19][16] * SK_MY[3]);
|
||||
Kfusion[20] = SK_MY[0] * (P[20][20] + P[20][17] * SH_MAG[0] + P[20][18] * SH_MAG[3] + P[20][0] * SK_MY[2] - P[20][2] *
|
||||
SK_MY[1] - P[20][16] * SK_MY[3]);
|
||||
Kfusion[21] = SK_MY[0] * (P[21][20] + P[21][17] * SH_MAG[0] + P[21][18] * SH_MAG[3] + P[21][0] * SK_MY[2] - P[21][2] *
|
||||
SK_MY[1] - P[21][16] * SK_MY[3]);
|
||||
Kfusion[22] = SK_MY[0] * (P[22][20] + P[22][17] * SH_MAG[0] + P[22][18] * SH_MAG[3] + P[22][0] * SK_MY[2] - P[22][2] *
|
||||
SK_MY[1] - P[22][16] * SK_MY[3]);
|
||||
Kfusion[23] = SK_MY[0] * (P[23][20] + P[23][17] * SH_MAG[0] + P[23][18] * SH_MAG[3] + P[23][0] * SK_MY[2] - P[23][2] *
|
||||
SK_MY[1] - P[23][16] * SK_MY[3]);
|
||||
} else if (index == 2) {
|
||||
// Calculate Z axis Kalman gains
|
||||
SK_MZ[0] = 1.0f / _mag_innov_var[2];
|
||||
SK_MZ[1] = magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2);
|
||||
SK_MZ[2] = magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1];
|
||||
SK_MZ[3] = 2 * q0 * q1 - 2 * q2 * q3;
|
||||
if (!_mag_healthy) {
|
||||
return;
|
||||
}
|
||||
|
||||
Kfusion[0] = SK_MZ[0] * (P[0][21] + P[0][18] * SH_MAG[2] + P[0][16] * SH_MAG[5] - P[0][0] * SK_MZ[1] + P[0][1] *
|
||||
SK_MZ[2] - P[0][17] * SK_MZ[3]);
|
||||
Kfusion[1] = SK_MZ[0] * (P[1][21] + P[1][18] * SH_MAG[2] + P[1][16] * SH_MAG[5] - P[1][0] * SK_MZ[1] + P[1][1] *
|
||||
SK_MZ[2] - P[1][17] * SK_MZ[3]);
|
||||
Kfusion[2] = SK_MZ[0] * (P[2][21] + P[2][18] * SH_MAG[2] + P[2][16] * SH_MAG[5] - P[2][0] * SK_MZ[1] + P[2][1] *
|
||||
SK_MZ[2] - P[2][17] * SK_MZ[3]);
|
||||
Kfusion[3] = SK_MZ[0] * (P[3][21] + P[3][18] * SH_MAG[2] + P[3][16] * SH_MAG[5] - P[3][0] * SK_MZ[1] + P[3][1] *
|
||||
SK_MZ[2] - P[3][17] * SK_MZ[3]);
|
||||
Kfusion[4] = SK_MZ[0] * (P[4][21] + P[4][18] * SH_MAG[2] + P[4][16] * SH_MAG[5] - P[4][0] * SK_MZ[1] + P[4][1] *
|
||||
SK_MZ[2] - P[4][17] * SK_MZ[3]);
|
||||
Kfusion[5] = SK_MZ[0] * (P[5][21] + P[5][18] * SH_MAG[2] + P[5][16] * SH_MAG[5] - P[5][0] * SK_MZ[1] + P[5][1] *
|
||||
SK_MZ[2] - P[5][17] * SK_MZ[3]);
|
||||
Kfusion[6] = SK_MZ[0] * (P[6][21] + P[6][18] * SH_MAG[2] + P[6][16] * SH_MAG[5] - P[6][0] * SK_MZ[1] + P[6][1] *
|
||||
SK_MZ[2] - P[6][17] * SK_MZ[3]);
|
||||
Kfusion[7] = SK_MZ[0] * (P[7][21] + P[7][18] * SH_MAG[2] + P[7][16] * SH_MAG[5] - P[7][0] * SK_MZ[1] + P[7][1] *
|
||||
SK_MZ[2] - P[7][17] * SK_MZ[3]);
|
||||
Kfusion[8] = SK_MZ[0] * (P[8][21] + P[8][18] * SH_MAG[2] + P[8][16] * SH_MAG[5] - P[8][0] * SK_MZ[1] + P[8][1] *
|
||||
SK_MZ[2] - P[8][17] * SK_MZ[3]);
|
||||
Kfusion[9] = SK_MZ[0] * (P[9][21] + P[9][18] * SH_MAG[2] + P[9][16] * SH_MAG[5] - P[9][0] * SK_MZ[1] + P[9][1] *
|
||||
SK_MZ[2] - P[9][17] * SK_MZ[3]);
|
||||
Kfusion[10] = SK_MZ[0] * (P[10][21] + P[10][18] * SH_MAG[2] + P[10][16] * SH_MAG[5] - P[10][0] * SK_MZ[1] + P[10][1] *
|
||||
SK_MZ[2] - P[10][17] * SK_MZ[3]);
|
||||
Kfusion[11] = SK_MZ[0] * (P[11][21] + P[11][18] * SH_MAG[2] + P[11][16] * SH_MAG[5] - P[11][0] * SK_MZ[1] + P[11][1] *
|
||||
SK_MZ[2] - P[11][17] * SK_MZ[3]);
|
||||
Kfusion[12] = SK_MZ[0] * (P[12][21] + P[12][18] * SH_MAG[2] + P[12][16] * SH_MAG[5] - P[12][0] * SK_MZ[1] + P[12][1] *
|
||||
SK_MZ[2] - P[12][17] * SK_MZ[3]);
|
||||
Kfusion[13] = SK_MZ[0] * (P[13][21] + P[13][18] * SH_MAG[2] + P[13][16] * SH_MAG[5] - P[13][0] * SK_MZ[1] + P[13][1] *
|
||||
SK_MZ[2] - P[13][17] * SK_MZ[3]);
|
||||
Kfusion[14] = SK_MZ[0] * (P[14][21] + P[14][18] * SH_MAG[2] + P[14][16] * SH_MAG[5] - P[14][0] * SK_MZ[1] + P[14][1] *
|
||||
SK_MZ[2] - P[14][17] * SK_MZ[3]);
|
||||
Kfusion[15] = SK_MZ[0] * (P[15][21] + P[15][18] * SH_MAG[2] + P[15][16] * SH_MAG[5] - P[15][0] * SK_MZ[1] + P[15][1] *
|
||||
SK_MZ[2] - P[15][17] * SK_MZ[3]);
|
||||
Kfusion[16] = SK_MZ[0] * (P[16][21] + P[16][18] * SH_MAG[2] + P[16][16] * SH_MAG[5] - P[16][0] * SK_MZ[1] + P[16][1] *
|
||||
SK_MZ[2] - P[16][17] * SK_MZ[3]);
|
||||
Kfusion[17] = SK_MZ[0] * (P[17][21] + P[17][18] * SH_MAG[2] + P[17][16] * SH_MAG[5] - P[17][0] * SK_MZ[1] + P[17][1] *
|
||||
SK_MZ[2] - P[17][17] * SK_MZ[3]);
|
||||
Kfusion[18] = SK_MZ[0] * (P[18][21] + P[18][18] * SH_MAG[2] + P[18][16] * SH_MAG[5] - P[18][0] * SK_MZ[1] + P[18][1] *
|
||||
SK_MZ[2] - P[18][17] * SK_MZ[3]);
|
||||
Kfusion[19] = SK_MZ[0] * (P[19][21] + P[19][18] * SH_MAG[2] + P[19][16] * SH_MAG[5] - P[19][0] * SK_MZ[1] + P[19][1] *
|
||||
SK_MZ[2] - P[19][17] * SK_MZ[3]);
|
||||
Kfusion[20] = SK_MZ[0] * (P[20][21] + P[20][18] * SH_MAG[2] + P[20][16] * SH_MAG[5] - P[20][0] * SK_MZ[1] + P[20][1] *
|
||||
SK_MZ[2] - P[20][17] * SK_MZ[3]);
|
||||
Kfusion[21] = SK_MZ[0] * (P[21][21] + P[21][18] * SH_MAG[2] + P[21][16] * SH_MAG[5] - P[21][0] * SK_MZ[1] + P[21][1] *
|
||||
SK_MZ[2] - P[21][17] * SK_MZ[3]);
|
||||
Kfusion[22] = SK_MZ[0] * (P[22][21] + P[22][18] * SH_MAG[2] + P[22][16] * SH_MAG[5] - P[22][0] * SK_MZ[1] + P[22][1] *
|
||||
SK_MZ[2] - P[22][17] * SK_MZ[3]);
|
||||
Kfusion[23] = SK_MZ[0] * (P[23][21] + P[23][18] * SH_MAG[2] + P[23][16] * SH_MAG[5] - P[23][0] * SK_MZ[1] + P[23][1] *
|
||||
SK_MZ[2] - P[23][17] * SK_MZ[3]);
|
||||
} else {
|
||||
return;
|
||||
}
|
||||
// update the states and covariance usinng sequential fusion of the magnetometer components
|
||||
for (uint8_t index = 0; index <= 2; index++) {
|
||||
// Calculate Kalman gains
|
||||
if (index == 0) {
|
||||
// Calculate X axis Kalman gains
|
||||
SK_MX[0] = 1.0f / _mag_innov_var[0];
|
||||
SK_MX[1] = magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2);
|
||||
SK_MX[2] = magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5];
|
||||
SK_MX[3] = SH_MAG[7];
|
||||
|
||||
// by definition our error state is zero at the time of fusion
|
||||
_state.ang_error.setZero();
|
||||
Kfusion[0] = SK_MX[0] * (P[0][19] + P[0][16] * SH_MAG[1] + P[0][17] * SH_MAG[4] - P[0][1] * SK_MX[2] + P[0][2] *
|
||||
SK_MX[1] + P[0][18] * SK_MX[3]);
|
||||
Kfusion[1] = SK_MX[0] * (P[1][19] + P[1][16] * SH_MAG[1] + P[1][17] * SH_MAG[4] - P[1][1] * SK_MX[2] + P[1][2] *
|
||||
SK_MX[1] + P[1][18] * SK_MX[3]);
|
||||
Kfusion[2] = SK_MX[0] * (P[2][19] + P[2][16] * SH_MAG[1] + P[2][17] * SH_MAG[4] - P[2][1] * SK_MX[2] + P[2][2] *
|
||||
SK_MX[1] + P[2][18] * SK_MX[3]);
|
||||
Kfusion[3] = SK_MX[0] * (P[3][19] + P[3][16] * SH_MAG[1] + P[3][17] * SH_MAG[4] - P[3][1] * SK_MX[2] + P[3][2] *
|
||||
SK_MX[1] + P[3][18] * SK_MX[3]);
|
||||
Kfusion[4] = SK_MX[0] * (P[4][19] + P[4][16] * SH_MAG[1] + P[4][17] * SH_MAG[4] - P[4][1] * SK_MX[2] + P[4][2] *
|
||||
SK_MX[1] + P[4][18] * SK_MX[3]);
|
||||
Kfusion[5] = SK_MX[0] * (P[5][19] + P[5][16] * SH_MAG[1] + P[5][17] * SH_MAG[4] - P[5][1] * SK_MX[2] + P[5][2] *
|
||||
SK_MX[1] + P[5][18] * SK_MX[3]);
|
||||
Kfusion[6] = SK_MX[0] * (P[6][19] + P[6][16] * SH_MAG[1] + P[6][17] * SH_MAG[4] - P[6][1] * SK_MX[2] + P[6][2] *
|
||||
SK_MX[1] + P[6][18] * SK_MX[3]);
|
||||
Kfusion[7] = SK_MX[0] * (P[7][19] + P[7][16] * SH_MAG[1] + P[7][17] * SH_MAG[4] - P[7][1] * SK_MX[2] + P[7][2] *
|
||||
SK_MX[1] + P[7][18] * SK_MX[3]);
|
||||
Kfusion[8] = SK_MX[0] * (P[8][19] + P[8][16] * SH_MAG[1] + P[8][17] * SH_MAG[4] - P[8][1] * SK_MX[2] + P[8][2] *
|
||||
SK_MX[1] + P[8][18] * SK_MX[3]);
|
||||
Kfusion[9] = SK_MX[0] * (P[9][19] + P[9][16] * SH_MAG[1] + P[9][17] * SH_MAG[4] - P[9][1] * SK_MX[2] + P[9][2] *
|
||||
SK_MX[1] + P[9][18] * SK_MX[3]);
|
||||
Kfusion[10] = SK_MX[0] * (P[10][19] + P[10][16] * SH_MAG[1] + P[10][17] * SH_MAG[4] - P[10][1] * SK_MX[2] + P[10][2] *
|
||||
SK_MX[1] + P[10][18] * SK_MX[3]);
|
||||
Kfusion[11] = SK_MX[0] * (P[11][19] + P[11][16] * SH_MAG[1] + P[11][17] * SH_MAG[4] - P[11][1] * SK_MX[2] + P[11][2] *
|
||||
SK_MX[1] + P[11][18] * SK_MX[3]);
|
||||
Kfusion[12] = SK_MX[0] * (P[12][19] + P[12][16] * SH_MAG[1] + P[12][17] * SH_MAG[4] - P[12][1] * SK_MX[2] + P[12][2] *
|
||||
SK_MX[1] + P[12][18] * SK_MX[3]);
|
||||
Kfusion[13] = SK_MX[0] * (P[13][19] + P[13][16] * SH_MAG[1] + P[13][17] * SH_MAG[4] - P[13][1] * SK_MX[2] + P[13][2] *
|
||||
SK_MX[1] + P[13][18] * SK_MX[3]);
|
||||
Kfusion[14] = SK_MX[0] * (P[14][19] + P[14][16] * SH_MAG[1] + P[14][17] * SH_MAG[4] - P[14][1] * SK_MX[2] + P[14][2] *
|
||||
SK_MX[1] + P[14][18] * SK_MX[3]);
|
||||
Kfusion[15] = SK_MX[0] * (P[15][19] + P[15][16] * SH_MAG[1] + P[15][17] * SH_MAG[4] - P[15][1] * SK_MX[2] + P[15][2] *
|
||||
SK_MX[1] + P[15][18] * SK_MX[3]);
|
||||
Kfusion[16] = SK_MX[0] * (P[16][19] + P[16][16] * SH_MAG[1] + P[16][17] * SH_MAG[4] - P[16][1] * SK_MX[2] + P[16][2] *
|
||||
SK_MX[1] + P[16][18] * SK_MX[3]);
|
||||
Kfusion[17] = SK_MX[0] * (P[17][19] + P[17][16] * SH_MAG[1] + P[17][17] * SH_MAG[4] - P[17][1] * SK_MX[2] + P[17][2] *
|
||||
SK_MX[1] + P[17][18] * SK_MX[3]);
|
||||
Kfusion[18] = SK_MX[0] * (P[18][19] + P[18][16] * SH_MAG[1] + P[18][17] * SH_MAG[4] - P[18][1] * SK_MX[2] + P[18][2] *
|
||||
SK_MX[1] + P[18][18] * SK_MX[3]);
|
||||
Kfusion[19] = SK_MX[0] * (P[19][19] + P[19][16] * SH_MAG[1] + P[19][17] * SH_MAG[4] - P[19][1] * SK_MX[2] + P[19][2] *
|
||||
SK_MX[1] + P[19][18] * SK_MX[3]);
|
||||
Kfusion[20] = SK_MX[0] * (P[20][19] + P[20][16] * SH_MAG[1] + P[20][17] * SH_MAG[4] - P[20][1] * SK_MX[2] + P[20][2] *
|
||||
SK_MX[1] + P[20][18] * SK_MX[3]);
|
||||
Kfusion[21] = SK_MX[0] * (P[21][19] + P[21][16] * SH_MAG[1] + P[21][17] * SH_MAG[4] - P[21][1] * SK_MX[2] + P[21][2] *
|
||||
SK_MX[1] + P[21][18] * SK_MX[3]);
|
||||
Kfusion[22] = SK_MX[0] * (P[22][19] + P[22][16] * SH_MAG[1] + P[22][17] * SH_MAG[4] - P[22][1] * SK_MX[2] + P[22][2] *
|
||||
SK_MX[1] + P[22][18] * SK_MX[3]);
|
||||
Kfusion[23] = SK_MX[0] * (P[23][19] + P[23][16] * SH_MAG[1] + P[23][17] * SH_MAG[4] - P[23][1] * SK_MX[2] + P[23][2] *
|
||||
SK_MX[1] + P[23][18] * SK_MX[3]);
|
||||
|
||||
fuse(Kfusion, _mag_innov[index]);
|
||||
} else if (index == 1) {
|
||||
// Calculate Y axis Kalman gains
|
||||
SK_MY[0] = 1.0f / _mag_innov_var[1];
|
||||
SK_MY[1] = magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1];
|
||||
SK_MY[2] = magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5];
|
||||
SK_MY[3] = SH_MAG[8] - 2 * q1 * q2;
|
||||
|
||||
Quaternion q_correction;
|
||||
q_correction.from_axis_angle(_state.ang_error);
|
||||
_state.quat_nominal = q_correction * _state.quat_nominal;
|
||||
_state.quat_nominal.normalize();
|
||||
_state.ang_error.setZero();
|
||||
Kfusion[0] = SK_MY[0] * (P[0][20] + P[0][17] * SH_MAG[0] + P[0][18] * SH_MAG[3] + P[0][0] * SK_MY[2] - P[0][2] *
|
||||
SK_MY[1] - P[0][16] * SK_MY[3]);
|
||||
Kfusion[1] = SK_MY[0] * (P[1][20] + P[1][17] * SH_MAG[0] + P[1][18] * SH_MAG[3] + P[1][0] * SK_MY[2] - P[1][2] *
|
||||
SK_MY[1] - P[1][16] * SK_MY[3]);
|
||||
Kfusion[2] = SK_MY[0] * (P[2][20] + P[2][17] * SH_MAG[0] + P[2][18] * SH_MAG[3] + P[2][0] * SK_MY[2] - P[2][2] *
|
||||
SK_MY[1] - P[2][16] * SK_MY[3]);
|
||||
Kfusion[3] = SK_MY[0] * (P[3][20] + P[3][17] * SH_MAG[0] + P[3][18] * SH_MAG[3] + P[3][0] * SK_MY[2] - P[3][2] *
|
||||
SK_MY[1] - P[3][16] * SK_MY[3]);
|
||||
Kfusion[4] = SK_MY[0] * (P[4][20] + P[4][17] * SH_MAG[0] + P[4][18] * SH_MAG[3] + P[4][0] * SK_MY[2] - P[4][2] *
|
||||
SK_MY[1] - P[4][16] * SK_MY[3]);
|
||||
Kfusion[5] = SK_MY[0] * (P[5][20] + P[5][17] * SH_MAG[0] + P[5][18] * SH_MAG[3] + P[5][0] * SK_MY[2] - P[5][2] *
|
||||
SK_MY[1] - P[5][16] * SK_MY[3]);
|
||||
Kfusion[6] = SK_MY[0] * (P[6][20] + P[6][17] * SH_MAG[0] + P[6][18] * SH_MAG[3] + P[6][0] * SK_MY[2] - P[6][2] *
|
||||
SK_MY[1] - P[6][16] * SK_MY[3]);
|
||||
Kfusion[7] = SK_MY[0] * (P[7][20] + P[7][17] * SH_MAG[0] + P[7][18] * SH_MAG[3] + P[7][0] * SK_MY[2] - P[7][2] *
|
||||
SK_MY[1] - P[7][16] * SK_MY[3]);
|
||||
Kfusion[8] = SK_MY[0] * (P[8][20] + P[8][17] * SH_MAG[0] + P[8][18] * SH_MAG[3] + P[8][0] * SK_MY[2] - P[8][2] *
|
||||
SK_MY[1] - P[8][16] * SK_MY[3]);
|
||||
Kfusion[9] = SK_MY[0] * (P[9][20] + P[9][17] * SH_MAG[0] + P[9][18] * SH_MAG[3] + P[9][0] * SK_MY[2] - P[9][2] *
|
||||
SK_MY[1] - P[9][16] * SK_MY[3]);
|
||||
Kfusion[10] = SK_MY[0] * (P[10][20] + P[10][17] * SH_MAG[0] + P[10][18] * SH_MAG[3] + P[10][0] * SK_MY[2] - P[10][2] *
|
||||
SK_MY[1] - P[10][16] * SK_MY[3]);
|
||||
Kfusion[11] = SK_MY[0] * (P[11][20] + P[11][17] * SH_MAG[0] + P[11][18] * SH_MAG[3] + P[11][0] * SK_MY[2] - P[11][2] *
|
||||
SK_MY[1] - P[11][16] * SK_MY[3]);
|
||||
Kfusion[12] = SK_MY[0] * (P[12][20] + P[12][17] * SH_MAG[0] + P[12][18] * SH_MAG[3] + P[12][0] * SK_MY[2] - P[12][2] *
|
||||
SK_MY[1] - P[12][16] * SK_MY[3]);
|
||||
Kfusion[13] = SK_MY[0] * (P[13][20] + P[13][17] * SH_MAG[0] + P[13][18] * SH_MAG[3] + P[13][0] * SK_MY[2] - P[13][2] *
|
||||
SK_MY[1] - P[13][16] * SK_MY[3]);
|
||||
Kfusion[14] = SK_MY[0] * (P[14][20] + P[14][17] * SH_MAG[0] + P[14][18] * SH_MAG[3] + P[14][0] * SK_MY[2] - P[14][2] *
|
||||
SK_MY[1] - P[14][16] * SK_MY[3]);
|
||||
Kfusion[15] = SK_MY[0] * (P[15][20] + P[15][17] * SH_MAG[0] + P[15][18] * SH_MAG[3] + P[15][0] * SK_MY[2] - P[15][2] *
|
||||
SK_MY[1] - P[15][16] * SK_MY[3]);
|
||||
Kfusion[16] = SK_MY[0] * (P[16][20] + P[16][17] * SH_MAG[0] + P[16][18] * SH_MAG[3] + P[16][0] * SK_MY[2] - P[16][2] *
|
||||
SK_MY[1] - P[16][16] * SK_MY[3]);
|
||||
Kfusion[17] = SK_MY[0] * (P[17][20] + P[17][17] * SH_MAG[0] + P[17][18] * SH_MAG[3] + P[17][0] * SK_MY[2] - P[17][2] *
|
||||
SK_MY[1] - P[17][16] * SK_MY[3]);
|
||||
Kfusion[18] = SK_MY[0] * (P[18][20] + P[18][17] * SH_MAG[0] + P[18][18] * SH_MAG[3] + P[18][0] * SK_MY[2] - P[18][2] *
|
||||
SK_MY[1] - P[18][16] * SK_MY[3]);
|
||||
Kfusion[19] = SK_MY[0] * (P[19][20] + P[19][17] * SH_MAG[0] + P[19][18] * SH_MAG[3] + P[19][0] * SK_MY[2] - P[19][2] *
|
||||
SK_MY[1] - P[19][16] * SK_MY[3]);
|
||||
Kfusion[20] = SK_MY[0] * (P[20][20] + P[20][17] * SH_MAG[0] + P[20][18] * SH_MAG[3] + P[20][0] * SK_MY[2] - P[20][2] *
|
||||
SK_MY[1] - P[20][16] * SK_MY[3]);
|
||||
Kfusion[21] = SK_MY[0] * (P[21][20] + P[21][17] * SH_MAG[0] + P[21][18] * SH_MAG[3] + P[21][0] * SK_MY[2] - P[21][2] *
|
||||
SK_MY[1] - P[21][16] * SK_MY[3]);
|
||||
Kfusion[22] = SK_MY[0] * (P[22][20] + P[22][17] * SH_MAG[0] + P[22][18] * SH_MAG[3] + P[22][0] * SK_MY[2] - P[22][2] *
|
||||
SK_MY[1] - P[22][16] * SK_MY[3]);
|
||||
Kfusion[23] = SK_MY[0] * (P[23][20] + P[23][17] * SH_MAG[0] + P[23][18] * SH_MAG[3] + P[23][0] * SK_MY[2] - P[23][2] *
|
||||
SK_MY[1] - P[23][16] * SK_MY[3]);
|
||||
|
||||
// apply covariance correction via P_new = (I -K*H)*P
|
||||
// first calculate expression for KHP
|
||||
// then calculate P - KHP
|
||||
float KH[_k_num_states][_k_num_states] = {};
|
||||
} else if (index == 2) {
|
||||
// Calculate Z axis Kalman gains
|
||||
SK_MZ[0] = 1.0f / _mag_innov_var[2];
|
||||
SK_MZ[1] = magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2);
|
||||
SK_MZ[2] = magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1];
|
||||
SK_MZ[3] = 2 * q0 * q1 - 2 * q2 * q3;
|
||||
|
||||
for (unsigned row = 0; row < _k_num_states; row++) {
|
||||
for (unsigned column = 0; column < 3; column++) {
|
||||
KH[row][column] = Kfusion[row] * H_MAG[index][column];
|
||||
}
|
||||
Kfusion[0] = SK_MZ[0] * (P[0][21] + P[0][18] * SH_MAG[2] + P[0][16] * SH_MAG[5] - P[0][0] * SK_MZ[1] + P[0][1] *
|
||||
SK_MZ[2] - P[0][17] * SK_MZ[3]);
|
||||
Kfusion[1] = SK_MZ[0] * (P[1][21] + P[1][18] * SH_MAG[2] + P[1][16] * SH_MAG[5] - P[1][0] * SK_MZ[1] + P[1][1] *
|
||||
SK_MZ[2] - P[1][17] * SK_MZ[3]);
|
||||
Kfusion[2] = SK_MZ[0] * (P[2][21] + P[2][18] * SH_MAG[2] + P[2][16] * SH_MAG[5] - P[2][0] * SK_MZ[1] + P[2][1] *
|
||||
SK_MZ[2] - P[2][17] * SK_MZ[3]);
|
||||
Kfusion[3] = SK_MZ[0] * (P[3][21] + P[3][18] * SH_MAG[2] + P[3][16] * SH_MAG[5] - P[3][0] * SK_MZ[1] + P[3][1] *
|
||||
SK_MZ[2] - P[3][17] * SK_MZ[3]);
|
||||
Kfusion[4] = SK_MZ[0] * (P[4][21] + P[4][18] * SH_MAG[2] + P[4][16] * SH_MAG[5] - P[4][0] * SK_MZ[1] + P[4][1] *
|
||||
SK_MZ[2] - P[4][17] * SK_MZ[3]);
|
||||
Kfusion[5] = SK_MZ[0] * (P[5][21] + P[5][18] * SH_MAG[2] + P[5][16] * SH_MAG[5] - P[5][0] * SK_MZ[1] + P[5][1] *
|
||||
SK_MZ[2] - P[5][17] * SK_MZ[3]);
|
||||
Kfusion[6] = SK_MZ[0] * (P[6][21] + P[6][18] * SH_MAG[2] + P[6][16] * SH_MAG[5] - P[6][0] * SK_MZ[1] + P[6][1] *
|
||||
SK_MZ[2] - P[6][17] * SK_MZ[3]);
|
||||
Kfusion[7] = SK_MZ[0] * (P[7][21] + P[7][18] * SH_MAG[2] + P[7][16] * SH_MAG[5] - P[7][0] * SK_MZ[1] + P[7][1] *
|
||||
SK_MZ[2] - P[7][17] * SK_MZ[3]);
|
||||
Kfusion[8] = SK_MZ[0] * (P[8][21] + P[8][18] * SH_MAG[2] + P[8][16] * SH_MAG[5] - P[8][0] * SK_MZ[1] + P[8][1] *
|
||||
SK_MZ[2] - P[8][17] * SK_MZ[3]);
|
||||
Kfusion[9] = SK_MZ[0] * (P[9][21] + P[9][18] * SH_MAG[2] + P[9][16] * SH_MAG[5] - P[9][0] * SK_MZ[1] + P[9][1] *
|
||||
SK_MZ[2] - P[9][17] * SK_MZ[3]);
|
||||
Kfusion[10] = SK_MZ[0] * (P[10][21] + P[10][18] * SH_MAG[2] + P[10][16] * SH_MAG[5] - P[10][0] * SK_MZ[1] + P[10][1] *
|
||||
SK_MZ[2] - P[10][17] * SK_MZ[3]);
|
||||
Kfusion[11] = SK_MZ[0] * (P[11][21] + P[11][18] * SH_MAG[2] + P[11][16] * SH_MAG[5] - P[11][0] * SK_MZ[1] + P[11][1] *
|
||||
SK_MZ[2] - P[11][17] * SK_MZ[3]);
|
||||
Kfusion[12] = SK_MZ[0] * (P[12][21] + P[12][18] * SH_MAG[2] + P[12][16] * SH_MAG[5] - P[12][0] * SK_MZ[1] + P[12][1] *
|
||||
SK_MZ[2] - P[12][17] * SK_MZ[3]);
|
||||
Kfusion[13] = SK_MZ[0] * (P[13][21] + P[13][18] * SH_MAG[2] + P[13][16] * SH_MAG[5] - P[13][0] * SK_MZ[1] + P[13][1] *
|
||||
SK_MZ[2] - P[13][17] * SK_MZ[3]);
|
||||
Kfusion[14] = SK_MZ[0] * (P[14][21] + P[14][18] * SH_MAG[2] + P[14][16] * SH_MAG[5] - P[14][0] * SK_MZ[1] + P[14][1] *
|
||||
SK_MZ[2] - P[14][17] * SK_MZ[3]);
|
||||
Kfusion[15] = SK_MZ[0] * (P[15][21] + P[15][18] * SH_MAG[2] + P[15][16] * SH_MAG[5] - P[15][0] * SK_MZ[1] + P[15][1] *
|
||||
SK_MZ[2] - P[15][17] * SK_MZ[3]);
|
||||
Kfusion[16] = SK_MZ[0] * (P[16][21] + P[16][18] * SH_MAG[2] + P[16][16] * SH_MAG[5] - P[16][0] * SK_MZ[1] + P[16][1] *
|
||||
SK_MZ[2] - P[16][17] * SK_MZ[3]);
|
||||
Kfusion[17] = SK_MZ[0] * (P[17][21] + P[17][18] * SH_MAG[2] + P[17][16] * SH_MAG[5] - P[17][0] * SK_MZ[1] + P[17][1] *
|
||||
SK_MZ[2] - P[17][17] * SK_MZ[3]);
|
||||
Kfusion[18] = SK_MZ[0] * (P[18][21] + P[18][18] * SH_MAG[2] + P[18][16] * SH_MAG[5] - P[18][0] * SK_MZ[1] + P[18][1] *
|
||||
SK_MZ[2] - P[18][17] * SK_MZ[3]);
|
||||
Kfusion[19] = SK_MZ[0] * (P[19][21] + P[19][18] * SH_MAG[2] + P[19][16] * SH_MAG[5] - P[19][0] * SK_MZ[1] + P[19][1] *
|
||||
SK_MZ[2] - P[19][17] * SK_MZ[3]);
|
||||
Kfusion[20] = SK_MZ[0] * (P[20][21] + P[20][18] * SH_MAG[2] + P[20][16] * SH_MAG[5] - P[20][0] * SK_MZ[1] + P[20][1] *
|
||||
SK_MZ[2] - P[20][17] * SK_MZ[3]);
|
||||
Kfusion[21] = SK_MZ[0] * (P[21][21] + P[21][18] * SH_MAG[2] + P[21][16] * SH_MAG[5] - P[21][0] * SK_MZ[1] + P[21][1] *
|
||||
SK_MZ[2] - P[21][17] * SK_MZ[3]);
|
||||
Kfusion[22] = SK_MZ[0] * (P[22][21] + P[22][18] * SH_MAG[2] + P[22][16] * SH_MAG[5] - P[22][0] * SK_MZ[1] + P[22][1] *
|
||||
SK_MZ[2] - P[22][17] * SK_MZ[3]);
|
||||
Kfusion[23] = SK_MZ[0] * (P[23][21] + P[23][18] * SH_MAG[2] + P[23][16] * SH_MAG[5] - P[23][0] * SK_MZ[1] + P[23][1] *
|
||||
SK_MZ[2] - P[23][17] * SK_MZ[3]);
|
||||
|
||||
for (unsigned column = 16; column < 22; column++) {
|
||||
KH[row][column] = Kfusion[row] * H_MAG[index][column];
|
||||
}
|
||||
} else {
|
||||
return;
|
||||
}
|
||||
|
||||
}
|
||||
// by definition our error state is zero at the time of fusion
|
||||
_state.ang_error.setZero();
|
||||
|
||||
float KHP[_k_num_states][_k_num_states] = {};
|
||||
fuse(Kfusion, _mag_innov[index]);
|
||||
|
||||
for (unsigned row = 0; row < _k_num_states; row++) {
|
||||
for (unsigned column = 0; column < _k_num_states; column++) {
|
||||
float tmp = KH[row][0] * P[0][column];
|
||||
tmp += KH[row][1] * P[1][column];
|
||||
tmp += KH[row][2] * P[2][column];
|
||||
tmp += KH[row][16] * P[16][column];
|
||||
tmp += KH[row][17] * P[17][column];
|
||||
tmp += KH[row][18] * P[18][column];
|
||||
tmp += KH[row][19] * P[19][column];
|
||||
tmp += KH[row][20] * P[20][column];
|
||||
tmp += KH[row][21] * P[21][column];
|
||||
KHP[row][column] = tmp;
|
||||
}
|
||||
}
|
||||
Quaternion q_correction;
|
||||
q_correction.from_axis_angle(_state.ang_error);
|
||||
_state.quat_nominal = q_correction * _state.quat_nominal;
|
||||
_state.quat_nominal.normalize();
|
||||
_state.ang_error.setZero();
|
||||
|
||||
for (unsigned row = 0; row < _k_num_states; row++) {
|
||||
for (unsigned column = 0; column < _k_num_states; column++) {
|
||||
P[row][column] -= KHP[row][column];
|
||||
}
|
||||
}
|
||||
// apply covariance correction via P_new = (I -K*H)*P
|
||||
// first calculate expression for KHP
|
||||
// then calculate P - KHP
|
||||
float KH[_k_num_states][_k_num_states] = {};
|
||||
|
||||
makeSymmetrical();
|
||||
limitCov();
|
||||
}
|
||||
for (unsigned row = 0; row < _k_num_states; row++) {
|
||||
for (unsigned column = 0; column < 3; column++) {
|
||||
KH[row][column] = Kfusion[row] * H_MAG[index][column];
|
||||
}
|
||||
|
||||
for (unsigned column = 16; column < 22; column++) {
|
||||
KH[row][column] = Kfusion[row] * H_MAG[index][column];
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
float KHP[_k_num_states][_k_num_states] = {};
|
||||
|
||||
for (unsigned row = 0; row < _k_num_states; row++) {
|
||||
for (unsigned column = 0; column < _k_num_states; column++) {
|
||||
float tmp = KH[row][0] * P[0][column];
|
||||
tmp += KH[row][1] * P[1][column];
|
||||
tmp += KH[row][2] * P[2][column];
|
||||
tmp += KH[row][16] * P[16][column];
|
||||
tmp += KH[row][17] * P[17][column];
|
||||
tmp += KH[row][18] * P[18][column];
|
||||
tmp += KH[row][19] * P[19][column];
|
||||
tmp += KH[row][20] * P[20][column];
|
||||
tmp += KH[row][21] * P[21][column];
|
||||
KHP[row][column] = tmp;
|
||||
}
|
||||
}
|
||||
|
||||
for (unsigned row = 0; row < _k_num_states; row++) {
|
||||
for (unsigned column = 0; column < _k_num_states; column++) {
|
||||
P[row][column] -= KHP[row][column];
|
||||
}
|
||||
}
|
||||
|
||||
makeSymmetrical();
|
||||
limitCov();
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::fuseHeading()
|
||||
@@ -522,14 +533,14 @@ void Ekf::fuseHeading()
|
||||
}
|
||||
|
||||
if (innovation_var >= R_mag) {
|
||||
// the innovation variance contribution from the state covariances is not negative, no fault
|
||||
// the innovation variance contribution from the state covariances is not negative, no fault
|
||||
_fault_status.bad_mag_x = false;
|
||||
_fault_status.bad_mag_y = false;
|
||||
_fault_status.bad_mag_z = false;
|
||||
|
||||
} else {
|
||||
// the innovation variance contribution from the state covariances is negtive which means the covariance matrix is badly conditioned
|
||||
_fault_status.bad_mag_x = true;
|
||||
// the innovation variance contribution from the state covariances is negtive which means the covariance matrix is badly conditioned
|
||||
_fault_status.bad_mag_x = true;
|
||||
_fault_status.bad_mag_y = true;
|
||||
_fault_status.bad_mag_z = true;
|
||||
|
||||
@@ -552,16 +563,16 @@ void Ekf::fuseHeading()
|
||||
}
|
||||
|
||||
// innovation test ratio
|
||||
_yaw_test_ratio = sq(innovation) / (sq(math::max(_params.heading_innov_gate, 1.0f)) * innovation_var);
|
||||
_yaw_test_ratio = sq(innovation) / (sq(math::max(_params.heading_innov_gate, 1.0f)) * innovation_var);
|
||||
|
||||
// set the magnetometer unhealthy if the test fails
|
||||
if (_yaw_test_ratio > 1.0f) {
|
||||
if (_yaw_test_ratio > 1.0f) {
|
||||
_mag_healthy = false;
|
||||
|
||||
// if we are in air we don't want to fuse the measurement
|
||||
// we allow to use it when on the ground because the large innovation could be caused
|
||||
// by interference or a large initial gyro bias
|
||||
if (_control_status.flags.in_air) {
|
||||
if (_control_status.flags.in_air) {
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user