fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-16 12:40:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #74 from PX4/pr-optflow-reworked

Browse Source 
Add fusion of optical flow LOS rate measurements
This commit is contained in:
Paul Riseborough
2016-03-11 11:13:03 +11:00
parent 829e0f1e65 109e0e6dfc
commit 2882c53246
20 changed files with 1813 additions and 728 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
+3 -1
View File
@@ -54,8 +54,10 @@ px4_add_module(
EKF/covariance.cpp
EKF/vel_pos_fusion.cpp
EKF/mag_fusion.cpp
EKF/gps_checks.cpp
EKF/gps_checks.cpp
EKF/optflow_fusion.cpp
EKF/control.cpp
EKF/terrain_estimator.cpp
DEPENDS
platforms__common
)
EKF/common.h
+121 -53
View File
@@ -64,6 +64,13 @@ typedef matrix::Vector<float, 3> Vector3f;
typedef matrix::Quaternion<float> Quaternion;
typedef matrix::Matrix<float, 3, 3> Matrix3f;
struct flow_message {
uint8_t quality; // Quality of Flow data
Vector2f flowdata; // Flow data received
Vector2f gyrodata; // Gyro data from flow sensor
uint32_t dt; // integration time of flow samples
};
struct outputSample {
Quaternion quat_nominal; // nominal quaternion describing vehicle attitude
Vector3f vel; // NED velocity estimate in earth frame in m/s
@@ -107,43 +114,89 @@ struct airspeedSample {
};
struct flowSample {
Vector2f flowRadXY;
Vector2f flowRadXYcomp;
uint64_t time_us;
uint8_t quality; // quality indicator between 0 and 255
Vector2f flowRadXY; // measured delta angle of the image about the X and Y body axes (rad), RH rotaton is positive
Vector2f flowRadXYcomp; // measured delta angle of the image about the X and Y body axes after removal of body rotation (rad), RH rotation is positive
Vector2f gyroXY; // measured delta angle of the inertial frame about the X and Y body axes obtained from rate gyro measurements (rad), RH rotation is positive
float dt; // amount of integration time (sec)
uint64_t time_us; // timestamp in microseconds of the integration period mid-point
};
// Integer definitions for vdist_sensor_type
#define VDIST_SENSOR_BARO 0 // Use baro height
#define VDIST_SENSOR_GPS 1 // Use GPS height
#define VDIST_SENSOR_RANGE 2 // Use range finder height
// Bit locations for mag_declination_source
#define MASK_USE_GEO_DECL (1<<0) // set to true to use the declination from the geo library when the GPS position becomes available, set to false to always use the EKF2_MAG_DECL value
#define MASK_SAVE_GEO_DECL (1<<1) // set to true to set the EKF2_MAG_DECL parameter to the value returned by the geo library
#define MASK_FUSE_DECL (1<<2) // set to true if the declination is always fused as an observation to constrain drift when 3-axis fusion is performed
// Bit locations for fusion_mode
#define MASK_USE_GPS (1<<0) // set to true to use GPS data
#define MASK_USE_OF (1<<1) // set to true to use optical flow data
// Integer definitions for mag_fusion_type
#define MAG_FUSE_TYPE_AUTO 0 // The selection of either heading or 3D magnetometer fusion will be automatic
#define MAG_FUSE_TYPE_HEADING 1 // Simple yaw angle fusion will always be used. This is less accurate, but less affected by earth field distortions. It should not be used for pitch angles outside the range from -60 to +60 deg
#define MAG_FUSE_TYPE_3D 2 // Magnetometer 3-axis fusion will always be used. This is more accurate, but more affected by localised earth field distortions
#define MAG_FUSE_TYPE_2D 3 // A 2D fusion that uses the horizontal projection of the magnetic fields measurement will alays be used. This is less accurate, but less affected by earth field distortions.
struct parameters {
float mag_delay_ms; // magnetometer measurement delay relative to the IMU
float baro_delay_ms; // barometer height measurement delay relative to the IMU
float gps_delay_ms; // GPS measurement delay relative to the IMU
float airspeed_delay_ms; // airspeed measurement delay relative to the IMU
// measurement source control
int fusion_mode; // bitmasked integer that selects which of the GPS and optical flow aiding sources will be used
int vdist_sensor_type; // selects the primary source for height data
// measurement time delays
float mag_delay_ms; // magnetometer measurement delay relative to the IMU (msec)
float baro_delay_ms; // barometer height measurement delay relative to the IMU (msec)
float gps_delay_ms; // GPS measurement delay relative to the IMU (msec)
float airspeed_delay_ms; // airspeed measurement delay relative to the IMU (msec)
float flow_delay_ms; // optical flow measurement delay relative to the IMU (msec) - this is to the middle of the optical flow integration interval
float range_delay_ms; // range finder measurement delay relative to the IMU (msec)
// input noise
float gyro_noise; // IMU angular rate noise used for covariance prediction
float accel_noise; // IMU acceleration noise use for covariance prediction
float gyro_noise; // IMU angular rate noise used for covariance prediction (rad/sec)
float accel_noise; // IMU acceleration noise use for covariance prediction (m/sec/sec)
// process noise
float gyro_bias_p_noise; // process noise for IMU delta angle bias prediction
float accel_bias_p_noise; // process noise for IMU delta velocity bias prediction
float gyro_scale_p_noise; // process noise for gyro scale factor prediction
float mag_p_noise; // process noise for magnetic field prediction
float wind_vel_p_noise; // process noise for wind velocity prediction
float gyro_bias_p_noise; // process noise for IMU delta angle bias prediction (rad/sec)
float accel_bias_p_noise; // process noise for IMU delta velocity bias prediction (m/sec/sec)
float gyro_scale_p_noise; // process noise for gyro scale factor prediction (N/A)
float mag_p_noise; // process noise for magnetic field prediction (Guass/sec)
float wind_vel_p_noise; // process noise for wind velocity prediction (m/sec/sec)
float terrain_p_noise; // process noise for terrain offset (m/sec)
float terrain_gradient; // gradient of terrain used to estimate process noise due to changing position (m/m)
float gps_vel_noise; // observation noise for gps velocity fusion
float gps_pos_noise; // observation noise for gps position fusion
float pos_noaid_noise; // observation noise for non-aiding position fusion
float baro_noise; // observation noise for barometric height fusion
float baro_innov_gate; // barometric height innovation consistency gate size in standard deviations
float posNE_innov_gate; // GPS horizontal position innovation consistency gate size in standard deviations
float vel_innov_gate; // GPS velocity innovation consistency gate size in standard deviations
// position and velocity fusion
float gps_vel_noise; // observation noise for gps velocity fusion (m/sec)
float gps_pos_noise; // observation noise for gps position fusion (m)
float pos_noaid_noise; // observation noise for non-aiding position fusion (m)
float baro_noise; // observation noise for barometric height fusion (m)
float baro_innov_gate; // barometric height innovation consistency gate size (STD)
float posNE_innov_gate; // GPS horizontal position innovation consistency gate size (STD)
float vel_innov_gate; // GPS velocity innovation consistency gate size (STD)
float mag_heading_noise; // measurement noise used for simple heading fusion
float mag_noise; // measurement noise used for 3-axis magnetoemeter fusion
float mag_declination_deg; // magnetic declination in degrees
float heading_innov_gate; // heading fusion innovation consistency gate size in standard deviations
float mag_innov_gate; // magnetometer fusion innovation consistency gate size in standard deviations
int mag_declination_source; // bitmask used to control the handling of declination data
int mag_fusion_type; // integer used to specify the type of magnetometer fusion used
// magnetometer fusion
float mag_heading_noise; // measurement noise used for simple heading fusion (rad)
float mag_noise; // measurement noise used for 3-axis magnetoemeter fusion (Gauss)
float mag_declination_deg; // magnetic declination (degrees)
float heading_innov_gate; // heading fusion innovation consistency gate size (STD)
float mag_innov_gate; // magnetometer fusion innovation consistency gate size (STD)
int mag_declination_source; // bitmask used to control the handling of declination data
int mag_fusion_type; // integer used to specify the type of magnetometer fusion used
// range finder fusion
float range_noise; // observation noise for range finder measurements (m)
float range_innov_gate; // range finder fusion innovation consistency gate size (STD)
float rng_gnd_clearance; // minimum valid value for range when on ground (m)
// optical flow fusion
float flow_noise; // observation noise for optical flow LOS rate measurements (rad/sec)
float flow_noise_qual_min; // observation noise for optical flow LOS rate measurements when flow sensor quality is at the minimum useable (rad/sec)
int flow_qual_min; // minimum acceptable quality integer from the flow sensor
float flow_innov_gate; // optical flow fusion innovation consistency gate size (STD)
float flow_rate_max; // maximum valid optical flow rate (rad/sec)
// 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
@@ -159,10 +212,17 @@ struct parameters {
// Initialize parameter values. Initialization must be accomplished in the constructor to allow C99 compiler compatibility.
parameters()
{
// measurement source control
fusion_mode = MASK_USE_GPS;
vdist_sensor_type = VDIST_SENSOR_BARO;
// measurement time delays
mag_delay_ms = 0.0f;
baro_delay_ms = 0.0f;
gps_delay_ms = 200.0f;
airspeed_delay_ms = 200.0f;
flow_delay_ms = 60.0f;
range_delay_ms = 200.0f;
// input noise
gyro_noise = 1.0e-3f;
@@ -174,7 +234,10 @@ struct parameters {
gyro_scale_p_noise = 3.0e-3f;
mag_p_noise = 2.5e-2f;
wind_vel_p_noise = 1.0e-1f;
terrain_p_noise = 5.0f;
terrain_gradient = 0.5f;
// position and velocity fusion
gps_vel_noise = 5.0e-1f;
gps_pos_noise = 1.0f;
pos_noaid_noise = 10.0f;
@@ -183,15 +246,28 @@ struct parameters {
posNE_innov_gate = 3.0f;
vel_innov_gate = 3.0f;
mag_heading_noise = 5.0e-1f;
// magnetometer fusion
mag_heading_noise = 1.7e-1f;
mag_noise = 5.0e-2f;
mag_declination_deg = 0.0f;
heading_innov_gate = 3.0f;
mag_innov_gate = 3.0f;
mag_declination_source = 7;
mag_declination_source = 3;
mag_fusion_type = 0;
// range finder fusion
range_noise = 0.1f;
range_innov_gate = 5.0f;
rng_gnd_clearance = 0.1f;
// optical flow fusion
flow_noise = 0.15f;
flow_noise_qual_min = 0.5f;
flow_qual_min = 1;
flow_innov_gate = 3.0f;
flow_rate_max = 2.5f;
// GPS quality checks
gps_check_mask = 21;
req_hacc = 5.0f;
req_vacc = 8.0f;
@@ -203,17 +279,6 @@ struct parameters {
}
};
// Bit locations for mag_declination_source
#define MASK_USE_GEO_DECL (1<<0) // set to true to use the declination from the geo library when the GPS position becomes available, set to false to always use the EKF2_MAG_DECL value
#define MASK_SAVE_GEO_DECL (1<<1) // set to true to set the EKF2_MAG_DECL parameter to the value returned by the geo library
#define MASK_FUSE_DECL (1<<2) // set to true if the declination is always fused as an observation to constrain drift when 3-axis fusion is performed
// Integer definitions for mag_fusion_type
#define MAG_FUSE_TYPE_AUTO 0 // The selection of either heading or 3D magnetometer fusion will be automatic
#define MAG_FUSE_TYPE_HEADING 1 // Simple yaw angle fusion will always be used. This is less accurate, but less affected by earth field distortions. It should not be used for pitch angles outside the range from -60 to +60 deg
#define MAG_FUSE_TYPE_3D 2 // Magnetometer 3-axis fusion will always be used. This is more accurate, but more affected by localised earth field distortions
#define MAG_FUSE_TYPE_2D 3 // A 2D fusion that uses the horizontal projection of the magnetic fields measurement will alays be used. This is less accurate, but less affected by earth field distortions.
struct stateSample {
Vector3f ang_error; // attitude axis angle error (error state formulation)
Vector3f vel; // NED velocity in earth frame in m/s
@@ -259,17 +324,20 @@ union gps_check_fail_status_u {
// bitmask containing filter control status
union filter_control_status_u {
struct {
uint8_t tilt_align : 1; // 0 - true if the filter tilt alignment is complete
uint8_t yaw_align : 1; // 1 - true if the filter yaw alignment is complete
uint8_t gps : 1; // 2 - true if GPS measurements are being fused
uint8_t opt_flow : 1; // 3 - true if optical flow measurements are being fused
uint8_t mag_hdg : 1; // 4 - true if a simple magnetic yaw heading is being fused
uint8_t mag_2D : 1; // 5 - true if the horizontal projection of magnetometer data is being fused
uint8_t mag_3D : 1; // 6 - true if 3-axis magnetometer measurement are being fused
uint8_t mag_dec : 1; // 7 - true if synthetic magnetic declination measurements are being fused
uint8_t in_air : 1; // 8 - true when the vehicle is airborne
uint8_t armed : 1; // 9 - true when the vehicle motors are armed
uint8_t wind : 1; // 10 - true when wind velocity is being estimated
uint16_t tilt_align : 1; // 0 - true if the filter tilt alignment is complete
uint16_t yaw_align : 1; // 1 - true if the filter yaw alignment is complete
uint16_t gps : 1; // 2 - true if GPS measurements are being fused
uint16_t opt_flow : 1; // 3 - true if optical flow measurements are being fused
uint16_t mag_hdg : 1; // 4 - true if a simple magnetic yaw heading is being fused
uint16_t mag_2D : 1; // 5 - true if the horizontal projection of magnetometer data is being fused
uint16_t mag_3D : 1; // 6 - true if 3-axis magnetometer measurement are being fused
uint16_t mag_dec : 1; // 7 - true if synthetic magnetic declination measurements are being fused
uint16_t in_air : 1; // 8 - true when the vehicle is airborne
uint16_t armed : 1; // 9 - true when the vehicle motors are armed
uint16_t wind : 1; // 10 - true when wind velocity is being estimated
uint16_t baro_hgt : 1; // 11 - true when baro height is being fused as a primary height reference
uint16_t rng_hgt : 1; // 12 - true when range finder height is being fused as a primary height reference
uint16_t gps_hgt : 1; // 15 - true when range finder height is being fused as a primary height reference
} flags;
uint16_t value;
};
EKF/control.cpp
+87 -11
View File
@@ -49,12 +49,71 @@ void Ekf::controlFusionModes()
// Get the magnetic declination
calcMagDeclination();
// Check for tilt convergence during initial alignment
// filter the tilt error vector using a 1 sec time constant LPF
float filt_coef = 1.0f * _imu_sample_delayed.delta_ang_dt;
_tilt_err_length_filt = filt_coef * _tilt_err_vec.norm() + (1.0f - filt_coef) * _tilt_err_length_filt;
// Once the tilt error has reduced sufficiently, initialise the yaw and magnetic field states
if (_tilt_err_length_filt < 0.005f && !_control_status.flags.tilt_align) {
_control_status.flags.tilt_align = true;
_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
}
// optical flow fusion mode selection logic
_control_status.flags.opt_flow = false;
// to start using optical flow data we need angular alignment complete, and fresh optical flow and height above terrain data
if ((_params.fusion_mode & MASK_USE_OF) && !_control_status.flags.opt_flow && _control_status.flags.tilt_align
&& (_time_last_imu - _time_last_optflow) < 5e5 && (_time_last_imu - _time_last_hagl_fuse) < 5e5) {
// If the heading is not aligned, reset the yaw and magnetic field states
if (!_control_status.flags.yaw_align) {
_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
}
// If the heading is valid, start using optical flow aiding
if (_control_status.flags.yaw_align) {
// set the flag and reset the fusion timeout
_control_status.flags.opt_flow = true;
_time_last_of_fuse = _time_last_imu;
// if we are not using GPS and are in air, then we need to reset the velocity to be consistent with the optical flow reading
if (!_control_status.flags.gps) {
// calculate the rotation matrix from body to earth frame
matrix::Dcm<float> body_to_earth(_state.quat_nominal);
// constrain height above ground to be above minimum possible
float heightAboveGndEst = fmaxf((_terrain_vpos - _state.pos(2)), _params.rng_gnd_clearance);
// calculate absolute distance from focal point to centre of frame assuming a flat earth
float range = heightAboveGndEst / body_to_earth(2, 2);
if (_in_air && (range - _params.rng_gnd_clearance) > 0.3f && _flow_sample_delayed.dt > 0.05f) {
// calculate X and Y body relative velocities from OF measurements
Vector3f vel_optflow_body;
vel_optflow_body(0) = - range * _flow_sample_delayed.flowRadXYcomp(1) / _flow_sample_delayed.dt;
vel_optflow_body(1) = range * _flow_sample_delayed.flowRadXYcomp(0) / _flow_sample_delayed.dt;
vel_optflow_body(2) = 0.0f;
// rotate from body to earth frame
Vector3f vel_optflow_earth;
vel_optflow_earth = body_to_earth * vel_optflow_body;
// take x and Y components
_state.vel(0) = vel_optflow_earth(0);
_state.vel(1) = vel_optflow_earth(1);
} else {
_state.vel.setZero();
}
}
}
} else if (!(_params.fusion_mode & MASK_USE_OF)) {
_control_status.flags.opt_flow = false;
}
// GPS fusion mode selection logic
// To start using GPS we need tilt and yaw alignment completed, the local NED origin set and fresh GPS data
if (!_control_status.flags.gps) {
// To start use GPS we need angular alignment completed, the local NED origin set and fresh GPS data
if ((_params.fusion_mode & MASK_USE_GPS) && !_control_status.flags.gps) {
if (_control_status.flags.tilt_align && (_time_last_imu - _time_last_gps) < 5e5 && _NED_origin_initialised
&& (_time_last_imu - _last_gps_fail_us > 5e6)) {
// If the heading is not aligned, reset the yaw and magnetic field states
@@ -62,18 +121,20 @@ void Ekf::controlFusionModes()
_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
}
// If the heading is valid, reset the positon and velocity and start using gps aiding
// If the heading is valid start using gps aiding
if (_control_status.flags.yaw_align) {
resetPosition();
resetVelocity();
_control_status.flags.gps = true;
_time_last_gps = _time_last_imu;
// if we are not already aiding with optical flow, then we need to reset the position and velocity
if (!_control_status.flags.opt_flow) {
_control_status.flags.gps = resetPosition();
_control_status.flags.gps = resetVelocity();
}
}
}
}
// decide when to start using optical flow data
if (!_control_status.flags.opt_flow) {
// TODO optical flow start logic
} else if (!(_params.fusion_mode & MASK_USE_GPS)) {
_control_status.flags.gps = false;
}
// handle the case when we are relying on GPS fusion and lose it
@@ -106,7 +167,15 @@ void Ekf::controlFusionModes()
// 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
// We are relying on flow aiding to constrain attitude drift so after 5s without aiding we need to do something
if ((_time_last_imu - _time_last_of_fuse > 5e6)) {
// Switch to the non-aiding mode, zero the veloity states
// and set the synthetic position to the current estimate
_control_status.flags.opt_flow = false;
_last_known_posNE(0) = _state.pos(0);
_last_known_posNE(1) = _state.pos(1);
_state.vel.setZero();
}
}
// Determine if we should use simple magnetic heading fusion which works better when there are large external disturbances
@@ -177,6 +246,12 @@ void Ekf::controlFusionModes()
_control_status.flags.mag_dec = false;
}
// Control the soure of height measurements for the main filter
_control_status.flags.baro_hgt = true;
_control_status.flags.rng_hgt = false;
_control_status.flags.gps_hgt = false;
// Placeholder for control of wind velocity states estimation
// TODO add methods for true airspeed and/or sidelsip fusion or some type of drag force measurement
if (false) {
@@ -199,6 +274,7 @@ void Ekf::calculateVehicleStatus()
// Transition to in-air occurs when armed and when altitude has increased sufficiently from the altitude at arming
bool in_air = _control_status.flags.armed && (_state.pos(2) - _last_disarmed_posD) < -1.0f;
if (!_control_status.flags.in_air && in_air) {
_control_status.flags.in_air = true;
}
EKF/covariance.cpp
+14
View File
@@ -103,6 +103,20 @@ void Ekf::initialiseCovariance()
}
void Ekf::get_pos_var(Vector3f &pos_var)
{
pos_var(0) = P[6][6];
pos_var(1) = P[7][7];
pos_var(2) = P[8][8];
}
void Ekf::get_vel_var(Vector3f &vel_var)
{
vel_var(0) = P[3][3];
vel_var(1) = P[4][4];
vel_var(2) = P[5][5];
}
void Ekf::predictCovariance()
{
// assign intermediate state variables
EKF/ekf.cpp
+158 -91
View File
@@ -36,7 +36,7 @@
* Core functions for ekf attitude and position estimator.
*
* @author Roman Bast <bapstroman@gmail.com>
*
* @author Paul Riseborough <p_riseborough@live.com.au>
*/
#include "ekf.h"
@@ -48,6 +48,8 @@ Ekf::Ekf():
_fuse_pos(false),
_fuse_hor_vel(false),
_fuse_vert_vel(false),
_fuse_flow(false),
_fuse_hagl_data(false),
_time_last_fake_gps(0),
_time_last_pos_fuse(0),
_time_last_vel_fuse(0),
@@ -56,6 +58,7 @@ Ekf::Ekf():
_last_disarmed_posD(0.0f),
_heading_innov(0.0f),
_heading_innov_var(0.0f),
_delta_time_of(0.0f),
_mag_declination(0.0f),
_gpsDriftVelN(0.0f),
_gpsDriftVelE(0.0f),
@@ -66,10 +69,14 @@ Ekf::Ekf():
_last_gps_fail_us(0),
_last_gps_origin_time_us(0),
_gps_alt_ref(0.0f),
_baro_counter(0),
_baro_sum(0.0f),
_hgt_counter(0),
_hgt_sum(0.0f),
_mag_counter(0),
_baro_at_alignment(0.0f)
_hgt_at_alignment(0.0f),
_terrain_vpos(0.0f),
_hagl_innov(0.0f),
_hagl_innov_var(0.0f),
_time_last_hagl_fuse(0)
{
_control_status = {};
_control_status_prev = {};
@@ -79,8 +86,10 @@ Ekf::Ekf():
_R_prev = matrix::Dcm<float>();
memset(_vel_pos_innov, 0, sizeof(_vel_pos_innov));
memset(_mag_innov, 0, sizeof(_mag_innov));
memset(_flow_innov, 0, sizeof(_flow_innov));
memset(_vel_pos_innov_var, 0, sizeof(_vel_pos_innov_var));
memset(_mag_innov_var, 0, sizeof(_mag_innov_var));
memset(_flow_innov_var, 0, sizeof(_flow_innov_var));
_delta_angle_corr.setZero();
_delta_vel_corr.setZero();
_vel_corr.setZero();
@@ -89,6 +98,8 @@ Ekf::Ekf():
_q_down_sampled.setZero();
_mag_sum = {};
_delVel_sum = {};
_flow_gyro_bias = {};
_imu_del_ang_of = {};
}
Ekf::~Ekf()
@@ -137,93 +148,140 @@ bool Ekf::init(uint64_t timestamp)
_mag_healthy = false;
_filter_initialised = false;
_terrain_initialised = false;
return ret;
}
bool Ekf::update()
{
bool ret = false; // indicates if there has been an update
if (!_filter_initialised) {
_filter_initialised = initialiseFilter();
if (!_filter_initialised) {
return false;
}
}
//printStates();
//printStatesFast();
// prediction
// Only run the filter if IMU data in the buffer has been updated
if (_imu_updated) {
ret = true;
if (!_filter_initialised) {
_filter_initialised = initialiseFilter();
if (!_filter_initialised) {
return false;
}
}
// perform state and covariance prediction for the main filter
predictState();
predictCovariance();
}
// control logic
controlFusionModes();
// measurement updates
// Fuse magnetometer data using the selected fusion 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.yaw_align) {
fuseMag();
if (_control_status.flags.mag_dec) {
fuseDeclination();
}
} else if (_control_status.flags.mag_2D && _control_status.flags.yaw_align) {
fuseMag2D();
} else if (_control_status.flags.mag_hdg && _control_status.flags.yaw_align) {
// fusion of a Euler yaw angle from either a 321 or 312 rotation sequence
fuseHeading();
// perform state and variance prediction for the terrain estimator
if (!_terrain_initialised) {
_terrain_initialised = initHagl();
} else {
// do no fusion at all
predictHagl();
}
// control logic
controlFusionModes();
// measurement updates
// 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.yaw_align) {
fuseMag();
if (_control_status.flags.mag_dec) {
fuseDeclination();
}
} else if (_control_status.flags.mag_2D && _control_status.flags.yaw_align) {
fuseMag2D();
} else if (_control_status.flags.mag_hdg && _control_status.flags.yaw_align) {
// fusion of a Euler yaw angle from either a 321 or 312 rotation sequence
fuseHeading();
} else {
// do no fusion at all
}
}
// determine if range finder data has fallen behind the fusin time horizon fuse it if we are
// not tilted too much to use it
if (_range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed)
&& (_R_prev(2, 2) > 0.7071f)) {
// if we have range data we always try to estimate terrain height
_fuse_hagl_data = true;
// only use range finder as a height observation in the main filter if specifically enabled
if (_params.vdist_sensor_type == VDIST_SENSOR_RANGE) {
_fuse_height = true;
}
}
// determine if baro data has fallen behind the fuson time horizon and fuse it in the main filter if enabled
if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)
&& _params.vdist_sensor_type == VDIST_SENSOR_BARO) {
_fuse_height = true;
}
// If we are using GPS aiding and data has fallen behind the fusion time horizon then fuse it
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;
}
// If we are using optical flow aiding and data has fallen behind the fusion time horizon, then fuse it
if (_flow_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_flow_sample_delayed)
&& _control_status.flags.opt_flow && (_time_last_imu - _time_last_optflow) < 2e5
&& (_R_prev(2, 2) > 0.7071f)) {
_fuse_flow = true;
}
// 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 (!_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;
}
// fuse available range finder data into a terrain height estimator if it has been initialised
if (_fuse_hagl_data && _terrain_initialised) {
fuseHagl();
_fuse_hagl_data = false;
}
// Fuse available NED velocity and position data into the main filter
if (_fuse_height || _fuse_pos || _fuse_hor_vel || _fuse_vert_vel) {
fuseVelPosHeight();
_fuse_hor_vel = _fuse_vert_vel = _fuse_pos = _fuse_height = false;
}
// Update optical flow bias estimates
calcOptFlowBias();
// Fuse optical flow LOS rate observations into the main filter
if (_fuse_flow) {
fuseOptFlow();
_last_known_posNE(0) = _state.pos(0);
_last_known_posNE(1) = _state.pos(1);
_fuse_flow = false;
}
}
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) {
_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;
}
if (_fuse_height || _fuse_pos || _fuse_hor_vel || _fuse_vert_vel) {
fuseVelPosHeight();
_fuse_hor_vel = _fuse_vert_vel = _fuse_pos = _fuse_height = false;
}
if (_range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed)) {
fuseRange();
}
if (_airspeed_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_airspeed_sample_delayed)) {
fuseAirspeed();
}
// the output observer always runs
calculateOutputStates();
return ret;
// We don't have valid data to output until tilt and yaw alignment is complete
if (_control_status.flags.tilt_align && _control_status.flags.yaw_align) {
return true;
} else {
return false;
}
}
bool Ekf::initialiseFilter(void)
@@ -231,7 +289,8 @@ bool Ekf::initialiseFilter(void)
// Keep accumulating measurements until we have a minimum of 10 samples for the baro and magnetoemter
// Sum the IMU delta angle measurements
_delVel_sum += _imu_down_sampled.delta_vel;
imuSample imu_init = _imu_buffer.get_newest();
_delVel_sum += imu_init.delta_vel;
// Sum the magnetometer measurements
magSample mag_init = _mag_buffer.get_newest();
@@ -241,17 +300,26 @@ bool Ekf::initialiseFilter(void)
_mag_sum += mag_init.mag;
}
// Sum the barometer measurements
// initialize vertical position with newest baro measurement
baroSample baro_init = _baro_buffer.get_newest();
if (_params.vdist_sensor_type == VDIST_SENSOR_RANGE) {
rangeSample range_init = _range_buffer.get_newest();
if (baro_init.time_us != 0) {
_baro_counter ++;
_baro_sum += baro_init.hgt;
if (range_init.time_us != 0) {
_hgt_counter ++;
_hgt_sum += range_init.rng;
}
} else {
// initialize vertical position with newest baro measurement
baroSample baro_init = _baro_buffer.get_newest();
if (baro_init.time_us != 0) {
_hgt_counter ++;
_hgt_sum += baro_init.hgt;
}
}
// check to see if we have enough measurements and return false if not
if (_baro_counter < 10 || _mag_counter < 10) {
// check to see if we have enough measruements and return false if not
if (_hgt_counter < 10 || _mag_counter < 10) {
return false;
} else {
@@ -283,16 +351,18 @@ bool Ekf::initialiseFilter(void)
matrix::Euler<float> euler_init(roll, pitch, 0.0f);
_state.quat_nominal = Quaternion(euler_init);
_output_new.quat_nominal = _state.quat_nominal;
_control_status.flags.tilt_align = true;
// initialise the filtered alignment error estimate to a larger starting value
_tilt_err_length_filt = 1.0f;
// calculate the averaged magnetometer reading
Vector3f mag_init = _mag_sum * (1.0f / (float(_mag_counter)));
// calculate the initial magnetic field and yaw alignment
_control_status.flags.yaw_align = resetMagHeading(mag_init);
resetMagHeading(mag_init);
// calculate the averaged barometer reading
_baro_at_alignment = _baro_sum / (float)_baro_counter;
_hgt_at_alignment = _hgt_sum / (float)_hgt_counter;
// set the velocity to the GPS measurement (by definition, the initial position and height is at the origin)
resetVelocity();
@@ -300,6 +370,9 @@ bool Ekf::initialiseFilter(void)
// initialise the state covariance matrix
initialiseCovariance();
// initialise the terrain estimator
initHagl();
return true;
}
}
@@ -354,7 +427,6 @@ bool Ekf::collect_imu(imuSample &imu)
_imu_down_sampled.delta_ang_dt += imu.delta_ang_dt;
_imu_down_sampled.delta_vel_dt += imu.delta_vel_dt;
Quaternion delta_q;
delta_q.rotate(imu.delta_ang);
_q_down_sampled = _q_down_sampled * delta_q;
@@ -453,8 +525,3 @@ void Ekf::fuseAirspeed()
{
}
void Ekf::fuseRange()
{
}
EKF/ekf.h
+87 -30
View File
@@ -76,6 +76,18 @@ public:
// gets the innovation variance of the heading measurement
void get_heading_innov_var(float *heading_innov_var);
// gets the innovation variance of the flow measurement
void get_flow_innov_var(float flow_innov_var[2]);
// gets the innovation of the flow measurement
void get_flow_innov(float flow_innov[2]);
// gets the innovation variance of the HAGL measurement
void get_hagl_innov_var(float *hagl_innov_var);
// gets the innovation of the HAGL measurement
void get_hagl_innov(float *hagl_innov);
// get the state vector at the delayed time horizon
void get_state_delayed(float *state);
@@ -98,6 +110,17 @@ public:
// get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position
void get_ekf_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_reckoning);
void get_vel_var(Vector3f &vel_var);
void get_pos_var(Vector3f &pos_var);
// return true if the global position estimate is valid
bool global_position_is_valid();
// return true if the etimate is valid
// return the estimated terrain vertical position relative to the NED origin
bool get_terrain_vert_pos(float *ret);
private:
static const uint8_t _k_num_states = 24;
@@ -105,13 +128,15 @@ private:
stateSample _state; // state struct of the ekf running at the delayed time horizon
bool _filter_initialised;
bool _earth_rate_initialised;
bool _filter_initialised; // true when the EKF sttes and covariances been initialised
bool _earth_rate_initialised; // true when we know the earth rotatin rate (requires GPS)
bool _fuse_height; // baro height data should be fused
bool _fuse_pos; // gps position data should be fused
bool _fuse_height; // baro height data should be fused
bool _fuse_pos; // gps position data should be fused
bool _fuse_hor_vel; // gps horizontal velocity measurement should be fused
bool _fuse_vert_vel; // gps vertical velocity measurement should be fused
bool _fuse_vert_vel; // gps vertical velocity measurement should be fused
bool _fuse_flow; // flow measurement should be fused
bool _fuse_hagl_data; // if true then range data will be fused to estimate terrain height
uint64_t _time_last_fake_gps; // last time in us at which we have faked gps measurement for static mode
@@ -131,45 +156,64 @@ private:
float KHP[_k_num_states][_k_num_states]; // intermediate variable for the covariance update
float _vel_pos_innov[6]; // innovations: 0-2 vel, 3-5 pos
float _vel_pos_innov_var[6]; // innovation variances: 0-2 vel, 3-5 pos
float _mag_innov[3]; // earth magnetic field innovations
float _mag_innov_var[3]; // earth magnetic field innovation variance
float _heading_innov; // heading measurement innovation
float _heading_innov_var; // heading measurement innovation variance
float _vel_pos_innov_var[6]; // innovation variances: 0-2 vel, 3-5 pos
float _mag_innov_var[3]; // earth magnetic field innovation variance
float _heading_innov_var; // heading measurement innovation variance
Vector3f _tilt_err_vec; // Vector of the most recent attitude error correction from velocity and position fusion
float _tilt_err_length_filt; // filtered length of _tilt_err_vec
float _mag_declination; // magnetic declination used by reset and fusion functions (rad)
// optical flow processing
float _flow_innov[2]; // flow measurement innovation
float _flow_innov_var[2]; // flow innovation variance
Vector2f _flow_gyro_bias; // bias errors in optical flow sensor rate gyro outputs
Vector2f _imu_del_ang_of; // bias corrected XY delta angle measurements accumulated across the same time frame as the optical flow rates
float _delta_time_of; // time in sec that _imu_del_ang_of was accumulated over
float _mag_declination; // magnetic declination used by reset and fusion functions (rad)
// complementary filter states
Vector3f _delta_angle_corr; // delta angle correction vector
Vector3f _delta_vel_corr; // delta velocity correction vector
Vector3f _vel_corr; // velocity correction vector
Vector3f _vel_corr; // velocity correction vector
imuSample _imu_down_sampled; // down sampled imu data (sensor rate -> filter update rate)
Quaternion _q_down_sampled; // down sampled quaternion (tracking delta angles between ekf update steps)
Quaternion _q_down_sampled; // down sampled quaternion (tracking delta angles between ekf update steps)
// variables used for the GPS quality checks
float _gpsDriftVelN; // GPS north position derivative (m/s)
float _gpsDriftVelE; // GPS east position derivative (m/s)
float _gps_drift_velD; // GPS down position derivative (m/s)
float _gps_velD_diff_filt; // GPS filtered Down velocity (m/s)
float _gps_velN_filt; // GPS filtered North velocity (m/s)
float _gps_velE_filt; // GPS filtered East velocity (m/s)
uint64_t _last_gps_fail_us; // last system time in usec that the GPS failed it's checks
float _gpsDriftVelN; // GPS north position derivative (m/s)
float _gpsDriftVelE; // GPS east position derivative (m/s)
float _gps_drift_velD; // GPS down position derivative (m/s)
float _gps_velD_diff_filt; // GPS filtered Down velocity (m/s)
float _gps_velN_filt; // GPS filtered North velocity (m/s)
float _gps_velE_filt; // GPS filtered East velocity (m/s)
uint64_t _last_gps_fail_us; // 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; // time the origin was last set (uSec)
float _gps_alt_ref; // WGS-84 height (m)
float _gps_alt_ref; // WGS-84 height (m)
// Variables used to initialise the filter states
uint8_t _baro_counter; // number of baro samples averaged
float _baro_sum; // summed baro measurement
uint8_t _mag_counter; // number of magnetometer samples averaged
Vector3f _mag_sum; // summed magnetometer measurement
Vector3f _delVel_sum; // summed delta velocity
float _baro_at_alignment; // baro offset relative to alignment position
uint8_t _hgt_counter; // number of baro samples averaged
float _hgt_sum; // summed baro measurement
uint8_t _mag_counter; // number of magnetometer samples averaged
Vector3f _mag_sum; // summed magnetometer measurement
Vector3f _delVel_sum; // summed delta velocity
float _hgt_at_alignment; // baro offset relative to alignment position
gps_check_fail_status_u _gps_check_fail_status;
// Terrain height state estimation
float _terrain_vpos; // estimated vertical position of the terrain underneath the vehicle in local NED frame (m)
float _terrain_var; // variance of terrain position estimate (m^2)
float _hagl_innov; // innovation of the last height above terrain measurement (m)
float _hagl_innov_var; // innovation variance for the last height above terrain measurement (m^2)
uint64_t _time_last_hagl_fuse; // last system time in usec that the hagl measurement failed it's checks
bool _terrain_initialised; // true when the terrain estimator has been intialised
// update the real time complementary filter states. This includes the prediction
// and the correction step
void calculateOutputStates();
@@ -201,14 +245,27 @@ private:
// fuse airspeed measurement
void fuseAirspeed();
// fuse range measurements
void fuseRange();
// fuse velocity and position measurements (also barometer height)
void fuseVelPosHeight();
// reset velocity states of the ekf
void resetVelocity();
bool resetVelocity();
// fuse optical flow line of sight rate measurements
void fuseOptFlow();
// calculate optical flow bias errors
void calcOptFlowBias();
// initialise the terrain vertical position estimator
// return true if the initialisation is successful
bool initHagl();
// predict the terrain vertical position state and variance
void predictHagl();
// update the terrain vertical position estimate using a height above ground measurement from the range finder
void fuseHagl();
// reset the heading and magnetic field states using the declination and magnetometer measurements
// return true if successful
@@ -218,7 +275,7 @@ private:
void calcMagDeclination();
// reset position states of the ekf (only vertical position)
void resetPosition();
bool resetPosition();
// reset height state of the ekf
void resetHeight();
EKF/ekf_helper.cpp
+29 -22
View File
@@ -49,22 +49,24 @@
// Reset the velocity states. If we have a recent and valid
// gps measurement then use for velocity initialisation
void Ekf::resetVelocity()
bool Ekf::resetVelocity()
{
// if we have a valid GPS measurement use it to initialise velocity states
gpsSample gps_newest = _gps_buffer.get_newest();
if (_time_last_imu - gps_newest.time_us < 400000) {
_state.vel = gps_newest.vel;
return true;
} else {
_state.vel.setZero();
// XXX use the value of the last known velocity
return false;
}
}
// Reset position states. If we have a recent and valid
// gps measurement then use for position initialisation
void Ekf::resetPosition()
bool Ekf::resetPosition()
{
// if we have a fresh GPS measurement, use it to initialise position states and correct the position for the measurement delay
gpsSample gps_newest = _gps_buffer.get_newest();
@@ -74,38 +76,36 @@ void Ekf::resetPosition()
if (time_delay < 0.4f) {
_state.pos(0) = gps_newest.pos(0) + gps_newest.vel(0) * time_delay;
_state.pos(1) = gps_newest.pos(1) + gps_newest.vel(1) * time_delay;
return true;
} else {
// XXX use the value of the last known position
return false;
}
}
// Reset height state using the last baro measurement
// Reset height state using the last height measurement
void Ekf::resetHeight()
{
// if we have a valid GPS measurement use it to initialise the vertical velocity state
gpsSample gps_newest = _gps_buffer.get_newest();
if (_params.vdist_sensor_type == VDIST_SENSOR_RANGE) {
rangeSample range_newest = _range_buffer.get_newest();
if (_time_last_imu - gps_newest.time_us < 400000) {
_state.vel(2) = gps_newest.vel(2);
if (_time_last_imu - range_newest.time_us < 200000) {
_state.pos(2) = _hgt_at_alignment - range_newest.rng;
} else {
// TODO: reset to last known range based estimate
}
} else {
_state.vel(2) = 0.0f;
}
// initialize vertical position with newest baro measurement
baroSample baro_newest = _baro_buffer.get_newest();
// if we have a valid height measurement, use it to initialise the vertical position state
baroSample baro_newest = _baro_buffer.get_newest();
if (_time_last_imu - baro_newest.time_us < 200000) {
_state.pos(2) = _hgt_at_alignment - baro_newest.hgt;
if (_time_last_imu - baro_newest.time_us < 200000) {
// use baro as the default
_state.pos(2) = _baro_at_alignment - baro_newest.hgt;
} else if (_time_last_imu - gps_newest.time_us < 400000) {
// use GPS as a backup
_state.pos(2) = _gps_alt_ref - gps_newest.hgt;
} else {
// Do not modify the state as there are no measurements to use
} else {
// TODO: reset to last known baro based estimate
}
}
}
@@ -397,3 +397,10 @@ void Ekf::zeroCols(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first
memset(&cov_mat[row][first], 0, sizeof(cov_mat[0][0]) * (1 + last - first));
}
}
bool Ekf::global_position_is_valid()
{
// return true if the position estimate is valid
// TODO implement proper check based on published GPS accuracy, innovation consistency checks and timeout status
return (_NED_origin_initialised && ((_time_last_imu - _time_last_gps) < 5e6) && _control_status.flags.gps);
}
EKF/estimator_interface.cpp
+56 -6
View File
@@ -213,21 +213,71 @@ void EstimatorInterface::setAirspeedData(uint64_t time_usec, float *data)
_airspeed_buffer.push(airspeed_sample_new);
}
}
static float rng;
// set range data
void EstimatorInterface::setRangeData(uint64_t time_usec, float *data)
{
if (!collect_range(time_usec, data) || !_initialised) {
return;
}
if (time_usec > _time_last_range) {
rangeSample range_sample_new;
range_sample_new.rng = *data;
rng = *data;
range_sample_new.time_us -= _params.range_delay_ms * 1000;
range_sample_new.time_us = time_usec;
_time_last_range = time_usec;
_range_buffer.push(range_sample_new);
}
}
// set optical flow data
void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, float *data)
void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *flow)
{
if (!collect_opticalflow(time_usec, data) || !_initialised) {
if (!collect_opticalflow(time_usec, flow) || !_initialised) {
return;
}
// if data passes checks, push to buffer
if (time_usec > _time_last_optflow) {
// check if enough integration time
float delta_time = 1e-6f * (float)flow->dt;
bool delta_time_good = (delta_time >= 0.05f);
// check magnitude is within sensor limits
float flow_rate_magnitude;
bool flow_magnitude_good = false;
if (delta_time_good) {
flow_rate_magnitude = flow->flowdata.norm() / delta_time;
flow_magnitude_good = (flow_rate_magnitude <= _params.flow_rate_max);
}
// check quality metric
bool flow_quality_good = (flow->quality >= _params.flow_qual_min);
if (delta_time_good && flow_magnitude_good && flow_quality_good) {
flowSample optflow_sample_new;
// calculate the system time-stamp for the mid point of the integration period
optflow_sample_new.time_us = time_usec - _params.flow_delay_ms * 1000 - flow->dt / 2;
// copy the quality metric returned by the PX4Flow sensor
optflow_sample_new.quality = flow->quality;
// NOTE: the EKF uses the reverse sign convention to the flow sensor. EKF assumes positive LOS rate is produced by a RH rotation of the image about the sensor axis.
// copy the optical and gyro measured delta angles
optflow_sample_new.flowRadXY = - flow->flowdata;
optflow_sample_new.gyroXY = - flow->gyrodata;
// compensate for body motion to give a LOS rate
optflow_sample_new.flowRadXYcomp = optflow_sample_new.flowRadXY - optflow_sample_new.gyroXY;
// convert integraton interval to seconds
optflow_sample_new.dt = 1e-6f * (float)flow->dt;
_time_last_optflow = time_usec;
// push to buffer
_flow_buffer.push(optflow_sample_new);
}
}
}
bool EstimatorInterface::initialise_interface(uint64_t timestamp)
@@ -265,6 +315,7 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
_time_last_baro = 0;
_time_last_range = 0;
_time_last_airspeed = 0;
_time_last_optflow = 0;
memset(&_fault_status, 0, sizeof(_fault_status));
return true;
@@ -283,9 +334,8 @@ void EstimatorInterface::unallocate_buffers()
}
bool EstimatorInterface::position_is_valid()
bool EstimatorInterface::local_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 ((_time_last_imu - _time_last_optflow) < 5e6) || global_position_is_valid();
}
EKF/estimator_interface.h
+35 -10
View File
@@ -81,6 +81,22 @@ public:
virtual void get_covariances(float *covariances) = 0;
// get the ekf WGS-84 origin position and height and the system time it was last set
virtual void get_vel_var(Vector3f &vel_var) = 0;
virtual void get_pos_var(Vector3f &pos_var) = 0;
// gets the innovation variance of the flow measurement
virtual void get_flow_innov_var(float flow_innov_var[2]) = 0;
// gets the innovation of the flow measurement
virtual void get_flow_innov(float flow_innov[2]) = 0;
// gets the innovation variance of the HAGL measurement
virtual void get_hagl_innov_var(float *flow_innov_var) = 0;
// gets the innovation of the HAGL measurement
virtual void get_hagl_innov(float *flow_innov_var) = 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 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position
@@ -99,7 +115,7 @@ public:
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, flow_message *flow) { 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);
@@ -121,7 +137,7 @@ public:
void setRangeData(uint64_t time_usec, float *data);
// set optical flow data
void setOpticalFlowData(uint64_t time_usec, float *data);
void setOpticalFlowData(uint64_t time_usec, flow_message *flow);
// return a address to the parameters struct
// in order to give access to the application
@@ -133,7 +149,15 @@ public:
// set vehicle landed status data
void set_in_air_status(bool in_air) {_in_air = in_air;}
bool position_is_valid();
// return true if the global position estimate is valid
virtual bool global_position_is_valid() = 0;
// return true if the estimate is valid
// return the estimated terrain vertical position relative to the NED origin
virtual bool get_terrain_vert_pos(float *ret) = 0;
// return true if the local position estimate is valid
bool local_position_is_valid();
void copy_quaternion(float *quat)
@@ -196,13 +220,13 @@ protected:
bool _vehicle_armed; // vehicle arm status used to turn off functionality used on the ground
bool _in_air; // we assume vehicle is in the air, set by the given landing detector
bool _NED_origin_initialised;
bool _gps_speed_valid;
float _gps_speed_accuracy; // GPS receiver reported speed accuracy (m/s)
struct map_projection_reference_s _pos_ref; // Contains WGS-84 position latitude and longitude (radians)
float _gps_hpos_accuracy; // GPS receiver reported 1-sigma horizontal accuracy (m)
float _gps_origin_eph; // horizontal position uncertainty of the GPS origin
float _gps_origin_epv; // vertical position uncertainty of the GPS origin
bool _NED_origin_initialised = false;
bool _gps_speed_valid = false;
float _gps_speed_accuracy = 0.0f; // GPS receiver reported 1-sigma speed accuracy (m/s)
float _gps_hpos_accuracy = 0.0f; // GPS receiver reported 1-sigma horizontal accuracy (m)
float _gps_origin_eph = 0.0f; // horizontal position uncertainty of the GPS origin
float _gps_origin_epv = 0.0f; // vertical position uncertainty of the GPS origin
struct map_projection_reference_s _pos_ref = {}; // Contains WGS-84 position latitude and longitude (radians)
bool _mag_healthy; // computed by mag innovation test
float _yaw_test_ratio; // yaw innovation consistency check ratio
@@ -226,6 +250,7 @@ protected:
uint64_t _time_last_baro; // timestamp of last barometer measurement in microseconds
uint64_t _time_last_range; // timestamp of last range measurement in microseconds
uint64_t _time_last_airspeed; // timestamp of last airspeed measurement in microseconds
uint64_t _time_last_optflow;
fault_status_t _fault_status;
EKF/gps_checks.cpp
+10 -4
View File
@@ -55,15 +55,21 @@
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 we have defined the WGS-84 position of the NED origin, run gps quality checks until they pass, then define the origins WGS-84 position using the last GPS fix
if (!_NED_origin_initialised ) {
// we have good GPS data so can now set the origin's WGS-84 position
if (gps_is_good(gps) && !_NED_origin_initialised) {
printf("gps is good - setting EKF origin\n");
// Initialise projection
// Set the origin's WGS-84 position to the last gps fix
double lat = gps->lat / 1.0e7;
double lon = gps->lon / 1.0e7;
map_projection_init_timestamped(&_pos_ref, lat, lon, _time_last_imu);
// if we are already doing aiding, corect for the change in posiiton since the EKF started navigating
if (_control_status.flags.opt_flow || _control_status.flags.gps) {
double est_lat, est_lon;
map_projection_reproject(&_pos_ref, -_state.pos(0), -_state.pos(1), &est_lat, &est_lon);
map_projection_init_timestamped(&_pos_ref, est_lat, est_lon, _time_last_imu);
}
// Take the current GPS height and subtract the filter height above origin to estimate the GPS height of the origin
_gps_alt_ref = 1e-3f * (float)gps->alt + _state.pos(2);
_NED_origin_initialised = true;
EKF/optflow_fusion.cpp
+538
View File
@@ -0,0 +1,538 @@
/****************************************************************************
*
* Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name ECL nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file vel_pos_fusion.cpp
* Function for fusing gps and baro measurements/
*
* @author Paul Riseborough <p_riseborough@live.com.au>
* @author Siddharth Bharat Purohit <siddharthbharatpurohit@gmail.com>
*
*/
#include "ekf.h"
#include "mathlib.h"
void Ekf::fuseOptFlow()
{
float gndclearance = fmaxf(_params.rng_gnd_clearance, 0.1f);
float optflow_test_ratio[2] = {0};
// get latest estimated orientation
float q0 = _state.quat_nominal(0);
float q1 = _state.quat_nominal(1);
float q2 = _state.quat_nominal(2);
float q3 = _state.quat_nominal(3);
// get latest velocity in earth frame
float vn = _state.vel(0);
float ve = _state.vel(1);
float vd = _state.vel(2);
// calculate the observation noise variance - scaling noise linearly across flow quality range
float R_LOS_best = fmaxf(_params.flow_noise, 0.05f);
float R_LOS_worst = fmaxf(_params.flow_noise_qual_min, 0.05f);
// calculate a weighting that varies between 1 when flow quality is best and 0 when flow quality is worst
float weighting = (255.0f - (float)_params.flow_qual_min);
if (weighting >= 1.0f) {
weighting = math::constrain(((float)_flow_sample_delayed.quality - (float)_params.flow_qual_min) / weighting, 0.0f,
1.0f);
} else {
weighting = 0.0f;
}
// take the weighted average of the observation noie for the best and wort flow quality
float R_LOS = sq(R_LOS_best * weighting + R_LOS_worst * (1.0f - weighting));
float H_LOS[2][24] = {}; // Optical flow observation Jacobians
float Kfusion[24][2] = {}; // Optical flow Kalman gains
// constrain height above ground to be above minimum height when sitting on ground
float heightAboveGndEst = math::max((_terrain_vpos - _state.pos(2)), gndclearance);
// get rotation nmatrix from earth to body
matrix::Dcm<float> earth_to_body(_state.quat_nominal);
earth_to_body = earth_to_body.transpose();
// rotate earth velocities into body frame
Vector3f vel_body = earth_to_body * _state.vel;
// calculate range from focal point to centre of image
float range = heightAboveGndEst / earth_to_body(2, 2); // absolute distance to the frame region in view
// calculate optical LOS rates using optical flow rates that have had the body angular rate contribution removed
// correct for gyro bias errors in the data used to do the motion compensation
// Note the sign convention used: A positive LOS rate is a RH rotaton of the scene about that axis.
Vector2f opt_flow_rate;
opt_flow_rate(0) = _flow_sample_delayed.flowRadXYcomp(0) / _flow_sample_delayed.dt + _flow_gyro_bias(0);
opt_flow_rate(1) = _flow_sample_delayed.flowRadXYcomp(1) / _flow_sample_delayed.dt + _flow_gyro_bias(1);
if (opt_flow_rate.norm() < _params.flow_rate_max) {
_flow_innov[0] = vel_body(1) / range - opt_flow_rate(0); // flow around the X axis
_flow_innov[1] = -vel_body(0) / range - opt_flow_rate(1); // flow around the Y axis
} else {
return;
}
// Fuse X and Y axis measurements sequentially assuming observation errors are uncorrelated
// Calculate Obser ation Jacobians and Kalman gans for each measurement axis
for (uint8_t obs_index = 0; obs_index <= 1; obs_index++) {
if (obs_index == 0) {
// calculate X axis observation Jacobian
float t2 = 1.0f / range;
float t3 = q0 * q0;
float t4 = q1 * q1;
float t5 = q2 * q2;
float t6 = q3 * q3;
float t7 = q0 * q2 * 2.0f;
float t8 = q1 * q3 * 2.0f;
float t9 = q0 * q3 * 2.0f;
float t10 = q1 * q2 * 2.0f;
float t11 = q0 * q1 * 2.0f;
H_LOS[0][0] = t2 * (vn * (t7 + t8) + vd * (t3 - t4 - t5 + t6) - ve * (t11 - q2 * q3 * 2.0f));
H_LOS[0][2] = -t2 * (ve * (t9 + t10) - vd * (t7 - t8) + vn * (t3 + t4 - t5 - t6));
H_LOS[0][3] = -t2 * (t9 - t10);
H_LOS[0][4] = t2 * (t3 - t4 + t5 - t6);
H_LOS[0][5] = t2 * (t11 + q2 * q3 * 2.0f);
// calculate intermediate variables for the X observaton innovatoin variance and klmna gains
t2 = 1.0f / range;
t3 = q0 * q1 * 2.0f;
t4 = q2 * q3 * 2.0f;
t5 = q0 * q0;
t6 = q1 * q1;
t7 = q2 * q2;
t8 = q3 * q3;
t9 = q0 * q2 * 2.0f;
t10 = q1 * q3 * 2.0f;
t11 = q0 * q3 * 2.0f;
float t12 = q1 * q2 * 2.0f;
float t13 = t11 - t12;
float t14 = t3 + t4;
float t15 = t5 - t6 - t7 + t8;
float t16 = t15 * vd;
float t17 = t3 - t4;
float t18 = t9 + t10;
float t19 = t18 * vn;
float t28 = t17 * ve;
float t20 = t16 + t19 - t28;
float t21 = t5 + t6 - t7 - t8;
float t22 = t21 * vn;
float t23 = t9 - t10;
float t24 = t11 + t12;
float t25 = t24 * ve;
float t29 = t23 * vd;
float t26 = t22 + t25 - t29;
float t27 = t5 - t6 + t7 - t8;
float t30 = P[0][0] * t2 * t20;
float t31 = P[5][3] * t2 * t14;
float t32 = P[0][3] * t2 * t20;
float t33 = P[4][3] * t2 * t27;
float t56 = P[3][3] * t2 * t13;
float t57 = P[2][3] * t2 * t26;
float t34 = t31 + t32 + t33 - t56 - t57;
float t35 = P[5][5] * t2 * t14;
float t36 = P[0][5] * t2 * t20;
float t37 = P[4][5] * t2 * t27;
float t59 = P[3][5] * t2 * t13;
float t60 = P[2][5] * t2 * t26;
float t38 = t35 + t36 + t37 - t59 - t60;
float t39 = t2 * t14 * t38;
float t40 = P[5][0] * t2 * t14;
float t41 = P[4][0] * t2 * t27;
float t61 = P[3][0] * t2 * t13;
float t62 = P[2][0] * t2 * t26;
float t42 = t30 + t40 + t41 - t61 - t62;
float t43 = t2 * t20 * t42;
float t44 = P[5][2] * t2 * t14;
float t45 = P[0][2] * t2 * t20;
float t46 = P[4][2] * t2 * t27;
float t55 = P[2][2] * t2 * t26;
float t63 = P[3][2] * t2 * t13;
float t47 = t44 + t45 + t46 - t55 - t63;
float t48 = P[5][4] * t2 * t14;
float t49 = P[0][4] * t2 * t20;
float t50 = P[4][4] * t2 * t27;
float t65 = P[3][4] * t2 * t13;
float t66 = P[2][4] * t2 * t26;
float t51 = t48 + t49 + t50 - t65 - t66;
float t52 = t2 * t27 * t51;
float t58 = t2 * t13 * t34;
float t64 = t2 * t26 * t47;
float t53 = R_LOS + t39 + t43 + t52 - t58 - t64;
float t54;
// calculate innovation variance for X axis observation and protect against a badly conditioned calculation
if (t53 >= R_LOS) {
t54 = 1.0f / t53;
_flow_innov_var[0] = t53;
} else {
// we need to reinitialise the covariance matrix and abort this fusion step
initialiseCovariance();
return;
}
// calculate Kalman gains for X-axis observation
Kfusion[0][0] = t54 * (t30 - P[0][3] * t2 * (t11 - q1 * q2 * 2.0f) + P[0][5] * t2 * t14 - P[0][2] * t2 * t26 + P[0][4] *
t2 * t27);
Kfusion[1][0] = t54 * (-P[1][3] * t2 * t13 + P[1][5] * t2 * t14 + P[1][0] * t2 * t20 - P[1][2] * t2 * t26 + P[1][4] * t2
* t27);
Kfusion[2][0] = t54 * (-t55 - P[2][3] * t2 * t13 + P[2][5] * t2 * t14 + P[2][0] * t2 * t20 + P[2][4] * t2 * t27);
Kfusion[3][0] = t54 * (-t56 + P[3][5] * t2 * t14 + P[3][0] * t2 * t20 - P[3][2] * t2 * t26 + P[3][4] * t2 * t27);
Kfusion[4][0] = t54 * (t50 - P[4][3] * t2 * t13 + P[4][5] * t2 * t14 + P[4][0] * t2 * t20 - P[4][2] * t2 * t26);
Kfusion[5][0] = t54 * (t35 - P[5][3] * t2 * t13 + P[5][0] * t2 * t20 - P[5][2] * t2 * t26 + P[5][4] * t2 * t27);
Kfusion[6][0] = t54 * (-P[6][3] * t2 * t13 + P[6][5] * t2 * t14 + P[6][0] * t2 * t20 - P[6][2] * t2 * t26 + P[6][4] * t2
* t27);
Kfusion[7][0] = t54 * (-P[7][3] * t2 * t13 + P[7][5] * t2 * t14 + P[7][0] * t2 * t20 - P[7][2] * t2 * t26 + P[7][4] * t2
* t27);
Kfusion[8][0] = t54 * (-P[8][3] * t2 * t13 + P[8][5] * t2 * t14 + P[8][0] * t2 * t20 - P[8][2] * t2 * t26 + P[8][4] * t2
* t27);
Kfusion[9][0] = t54 * (-P[9][3] * t2 * t13 + P[9][5] * t2 * t14 + P[9][0] * t2 * t20 - P[9][2] * t2 * t26 + P[9][4] * t2
* t27);
Kfusion[10][0] = t54 * (-P[10][3] * t2 * t13 + P[10][5] * t2 * t14 + P[10][0] * t2 * t20 - P[10][2] * t2 * t26 +
P[10][4] * t2 * t27);
Kfusion[11][0] = t54 * (-P[11][3] * t2 * t13 + P[11][5] * t2 * t14 + P[11][0] * t2 * t20 - P[11][2] * t2 * t26 +
P[11][4] * t2 * t27);
Kfusion[12][0] = t54 * (-P[12][3] * t2 * t13 + P[12][5] * t2 * t14 + P[12][0] * t2 * t20 - P[12][2] * t2 * t26 +
P[12][4] * t2 * t27);
Kfusion[13][0] = t54 * (-P[13][3] * t2 * t13 + P[13][5] * t2 * t14 + P[13][0] * t2 * t20 - P[13][2] * t2 * t26 +
P[13][4] * t2 * t27);
Kfusion[14][0] = t54 * (-P[14][3] * t2 * t13 + P[14][5] * t2 * t14 + P[14][0] * t2 * t20 - P[14][2] * t2 * t26 +
P[14][4] * t2 * t27);
Kfusion[15][0] = t54 * (-P[15][3] * t2 * t13 + P[15][5] * t2 * t14 + P[15][0] * t2 * t20 - P[15][2] * t2 * t26 +
P[15][4] * t2 * t27);
if (_control_status.flags.mag_3D) {
Kfusion[16][0] = t54 * (-P[16][3] * t2 * t13 + P[16][5] * t2 * t14 + P[16][0] * t2 * t20 - P[16][2] * t2 * t26 +
P[16][4] * t2 * t27);
Kfusion[17][0] = t54 * (-P[17][3] * t2 * t13 + P[17][5] * t2 * t14 + P[17][0] * t2 * t20 - P[17][2] * t2 * t26 +
P[17][4] * t2 * t27);
Kfusion[18][0] = t54 * (-P[18][3] * t2 * t13 + P[18][5] * t2 * t14 + P[18][0] * t2 * t20 - P[18][2] * t2 * t26 +
P[18][4] * t2 * t27);
Kfusion[19][0] = t54 * (-P[19][3] * t2 * t13 + P[19][5] * t2 * t14 + P[19][0] * t2 * t20 - P[19][2] * t2 * t26 +
P[19][4] * t2 * t27);
Kfusion[20][0] = t54 * (-P[20][3] * t2 * t13 + P[20][5] * t2 * t14 + P[20][0] * t2 * t20 - P[20][2] * t2 * t26 +
P[20][4] * t2 * t27);
Kfusion[21][0] = t54 * (-P[21][3] * t2 * t13 + P[21][5] * t2 * t14 + P[21][0] * t2 * t20 - P[21][2] * t2 * t26 +
P[21][4] * t2 * t27);
}
if (_control_status.flags.wind) {
Kfusion[22][0] = t54 * (-P[22][3] * t2 * t13 + P[22][5] * t2 * t14 + P[22][0] * t2 * t20 - P[22][2] * t2 * t26 +
P[22][4] * t2 * t27);
Kfusion[23][0] = t54 * (-P[23][3] * t2 * t13 + P[23][5] * t2 * t14 + P[23][0] * t2 * t20 - P[23][2] * t2 * t26 +
P[23][4] * t2 * t27);
}
// run innovation consistency checks
optflow_test_ratio[0] = sq(_flow_innov[0]) / (sq(math::max(_params.flow_innov_gate, 1.0f)) * _flow_innov_var[0]);
} else if (obs_index == 1) {
// calculate Y axis observation Jacobian
float t2 = 1.0f / range;
float t3 = q0 * q0;
float t4 = q1 * q1;
float t5 = q2 * q2;
float t6 = q3 * q3;
float t7 = q0 * q1 * 2.0f;
float t8 = q0 * q3 * 2.0f;
float t9 = q0 * q2 * 2.0f;
float t10 = q1 * q3 * 2.0f;
H_LOS[1][1] = t2 * (vn * (t9 + t10) + vd * (t3 - t4 - t5 + t6) - ve * (t7 - q2 * q3 * 2.0f));
H_LOS[1][2] = -t2 * (ve * (t3 - t4 + t5 - t6) + vd * (t7 + q2 * q3 * 2.0f) - vn * (t8 - q1 * q2 * 2.0f));
H_LOS[1][3] = -t2 * (t3 + t4 - t5 - t6);
H_LOS[1][4] = -t2 * (t8 + q1 * q2 * 2.0f);
H_LOS[1][5] = t2 * (t9 - t10);
// calculate intermediate variables for the X observaton innovatoin variance and klmna gains
t2 = 1.0f / range;
t3 = q0 * q2 * 2.0f;
t4 = q0 * q0;
t5 = q1 * q1;
t6 = q2 * q2;
t7 = q3 * q3;
t8 = q0 * q1 * 2.0f;
t9 = q0 * q3 * 2.0f;
t10 = q1 * q2 * 2.0f;
float t11 = t9 + t10;
float t12 = q1 * q3 * 2.0f;
float t13 = t4 - t5 - t6 + t7;
float t14 = t13 * vd;
float t15 = q2 * q3 * 2.0f;
float t16 = t3 + t12;
float t17 = t16 * vn;
float t18 = t4 - t5 + t6 - t7;
float t19 = t18 * ve;
float t20 = t8 + t15;
float t21 = t20 * vd;
float t22 = t9 - t10;
float t28 = t22 * vn;
float t23 = t19 + t21 - t28;
float t24 = t4 + t5 - t6 - t7;
float t25 = t3 - t12;
float t26 = t8 - t15;
float t29 = t26 * ve;
float t27 = t14 + t17 - t29;
float t30 = P[4][4] * t2 * t11;
float t31 = P[2][4] * t2 * t23;
float t32 = P[3][4] * t2 * t24;
float t56 = P[5][4] * t2 * t25;
float t57 = P[1][4] * t2 * t27;
float t33 = t30 + t31 + t32 - t56 - t57;
float t34 = t2 * t11 * t33;
float t35 = P[4][5] * t2 * t11;
float t36 = P[2][5] * t2 * t23;
float t37 = P[3][5] * t2 * t24;
float t58 = P[5][5] * t2 * t25;
float t59 = P[1][5] * t2 * t27;
float t38 = t35 + t36 + t37 - t58 - t59;
float t39 = P[4][1] * t2 * t11;
float t40 = P[2][1] * t2 * t23;
float t41 = P[3][1] * t2 * t24;
float t55 = P[1][1] * t2 * t27;
float t61 = P[5][1] * t2 * t25;
float t42 = t39 + t40 + t41 - t55 - t61;
float t43 = P[4][2] * t2 * t11;
float t44 = P[2][2] * t2 * t23;
float t45 = P[3][2] * t2 * t24;
float t63 = P[5][2] * t2 * t25;
float t64 = P[1][2] * t2 * t27;
float t46 = t43 + t44 + t45 - t63 - t64;
float t47 = t2 * t23 * t46;
float t48 = P[4][3] * t2 * t11;
float t49 = P[2][3] * t2 * t23;
float t50 = P[3][3] * t2 * t24;
float t65 = P[5][3] * t2 * t25;
float t66 = P[1][3] * t2 * t27;
float t51 = t48 + t49 + t50 - t65 - t66;
float t52 = t2 * t24 * t51;
float t60 = t2 * t25 * t38;
float t62 = t2 * t27 * t42;
float t53 = R_LOS + t34 + t47 + t52 - t60 - t62;
float t54;
// calculate innovation variance for X axis observation and protect against a badly conditioned calculation
if (t53 >= R_LOS) {
t54 = 1.0f / t53;
_flow_innov_var[1] = t53;
} else {
// we need to reinitialise the covariance matrix and abort this fusion step
initialiseCovariance();
return;
}
// calculate Kalman gains for X-axis observation
Kfusion[0][1] = -t54 * (P[0][4] * t2 * t11 + P[0][2] * t2 * t23 + P[0][3] * t2 * t24 - P[0][1] * t2 * t27 - P[0][5] * t2
* t25);
Kfusion[1][1] = -t54 * (-t55 + P[1][4] * t2 * t11 + P[1][2] * t2 * t23 + P[1][3] * t2 * t24 - P[1][5] * t2 * t25);
Kfusion[2][1] = -t54 * (t44 + P[2][4] * t2 * t11 + P[2][3] * t2 * t24 - P[2][1] * t2 * t27 - P[2][5] * t2 * t25);
Kfusion[3][1] = -t54 * (t50 + P[3][4] * t2 * t11 + P[3][2] * t2 * t23 - P[3][1] * t2 * t27 - P[3][5] * t2 * t25);
Kfusion[4][1] = -t54 * (t30 + P[4][2] * t2 * t23 + P[4][3] * t2 * t24 - P[4][1] * t2 * t27 - P[4][5] * t2 * t25);
Kfusion[5][1] = -t54 * (-t58 + P[5][4] * t2 * t11 + P[5][2] * t2 * t23 + P[5][3] * t2 * t24 - P[5][1] * t2 * t27);
Kfusion[6][1] = -t54 * (P[6][4] * t2 * t11 + P[6][2] * t2 * t23 + P[6][3] * t2 * t24 - P[6][1] * t2 * t27 - P[6][5] * t2
* t25);
Kfusion[7][1] = -t54 * (P[7][4] * t2 * t11 + P[7][2] * t2 * t23 + P[7][3] * t2 * t24 - P[7][1] * t2 * t27 - P[7][5] * t2
* t25);
Kfusion[8][1] = -t54 * (P[8][4] * t2 * t11 + P[8][2] * t2 * t23 + P[8][3] * t2 * t24 - P[8][1] * t2 * t27 - P[8][5] * t2
* t25);
Kfusion[9][1] = -t54 * (P[9][4] * t2 * t11 + P[9][2] * t2 * t23 + P[9][3] * t2 * t24 - P[9][1] * t2 * t27 - P[9][5] * t2
* t25);
Kfusion[10][1] = -t54 * (P[10][4] * t2 * t11 + P[10][2] * t2 * t23 + P[10][3] * t2 * t24 - P[10][1] * t2 * t27 -
P[10][5] * t2 * t25);
Kfusion[11][1] = -t54 * (P[11][4] * t2 * t11 + P[11][2] * t2 * t23 + P[11][3] * t2 * t24 - P[11][1] * t2 * t27 -
P[11][5] * t2 * t25);
Kfusion[12][1] = -t54 * (P[12][4] * t2 * t11 + P[12][2] * t2 * t23 + P[12][3] * t2 * t24 - P[12][1] * t2 * t27 -
P[12][5] * t2 * t25);
Kfusion[13][1] = -t54 * (P[13][4] * t2 * t11 + P[13][2] * t2 * t23 + P[13][3] * t2 * t24 - P[13][1] * t2 * t27 -
P[13][5] * t2 * t25);
Kfusion[14][1] = -t54 * (P[14][4] * t2 * t11 + P[14][2] * t2 * t23 + P[14][3] * t2 * t24 - P[14][1] * t2 * t27 -
P[14][5] * t2 * t25);
Kfusion[15][1] = -t54 * (P[15][4] * t2 * t11 + P[15][2] * t2 * t23 + P[15][3] * t2 * t24 - P[15][1] * t2 * t27 -
P[15][5] * t2 * t25);
if (_control_status.flags.mag_3D) {
Kfusion[16][1] = -t54 * (P[16][4] * t2 * t11 + P[16][2] * t2 * t23 + P[16][3] * t2 * t24 - P[16][1] * t2 * t27 -
P[16][5] * t2 * t25);
Kfusion[17][1] = -t54 * (P[17][4] * t2 * t11 + P[17][2] * t2 * t23 + P[17][3] * t2 * t24 - P[17][1] * t2 * t27 -
P[17][5] * t2 * t25);
Kfusion[18][1] = -t54 * (P[18][4] * t2 * t11 + P[18][2] * t2 * t23 + P[18][3] * t2 * t24 - P[18][1] * t2 * t27 -
P[18][5] * t2 * t25);
Kfusion[19][1] = -t54 * (P[19][4] * t2 * t11 + P[19][2] * t2 * t23 + P[19][3] * t2 * t24 - P[19][1] * t2 * t27 -
P[19][5] * t2 * t25);
Kfusion[20][1] = -t54 * (P[20][4] * t2 * t11 + P[20][2] * t2 * t23 + P[20][3] * t2 * t24 - P[20][1] * t2 * t27 -
P[20][5] * t2 * t25);
Kfusion[21][1] = -t54 * (P[21][4] * t2 * t11 + P[21][2] * t2 * t23 + P[21][3] * t2 * t24 - P[21][1] * t2 * t27 -
P[21][5] * t2 * t25);
}
if (_control_status.flags.wind) {
Kfusion[22][1] = -t54 * (P[22][4] * t2 * t11 + P[22][2] * t2 * t23 + P[22][3] * t2 * t24 - P[22][1] * t2 * t27 -
P[22][5] * t2 * t25);
Kfusion[23][1] = -t54 * (P[23][4] * t2 * t11 + P[23][2] * t2 * t23 + P[23][3] * t2 * t24 - P[23][1] * t2 * t27 -
P[23][5] * t2 * t25);
}
// run innovation consistency check
optflow_test_ratio[1] = sq(_flow_innov[1]) / (sq(math::max(_params.flow_innov_gate, 1.0f)) * _flow_innov_var[1]);
} else {
return;
}
}
// if either axis fails, we fail the sensor
if (optflow_test_ratio[0] > 1.0f || optflow_test_ratio[1] > 1.0f) {
return;
}
for (uint8_t obs_index = 0; obs_index <= 1; obs_index++) {
// by definition our error state is zero at the time of fusion
_state.ang_error.setZero();
// copy the Kalman gain vector for the axis we are fusing
float gain[24];
for (unsigned row = 0; row <= 23; row++) {
gain[row] = Kfusion[row][obs_index];
}
// Update the state vector
fuse(gain, _flow_innov[obs_index]);
// correct the quaternion using the attitude error state
Quaternion q_correction;
q_correction.from_axis_angle(_state.ang_error);
_state.quat_nominal = q_correction * _state.quat_nominal;
_state.quat_nominal.normalize();
// reset attitude error to zero after the correction has been applied
_state.ang_error.setZero();
// apply covariance correction via P_new = (I -K*H)*P
// first calculate expression for KHP
// then calculate P - KHP
for (unsigned row = 0; row < _k_num_states; row++) {
for (unsigned column = 0; column <= 5; column++) {
KH[row][column] = gain[row] * H_LOS[obs_index][column];
}
}
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][3] * P[3][column];
tmp += KH[row][4] * P[4][column];
tmp += KH[row][5] * P[5][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];
}
}
_time_last_of_fuse = _time_last_imu;
_gps_check_fail_status.value = 0;
makeSymmetrical();
limitCov();
}
}
void Ekf::get_flow_innov(float flow_innov[2])
{
memcpy(flow_innov, _flow_innov, sizeof(_flow_innov));
}
void Ekf::get_flow_innov_var(float flow_innov_var[2])
{
memcpy(flow_innov_var, _flow_innov_var, sizeof(_flow_innov_var));
}
// calculate optical flow gyro bias errors
void Ekf::calcOptFlowBias()
{
// accumulate the bias corrected delta angles from the navigation sensor and lapsed time
_imu_del_ang_of(0) += _imu_sample_delayed.delta_ang(0);
_imu_del_ang_of(1) += _imu_sample_delayed.delta_ang(1);
_delta_time_of += _imu_sample_delayed.delta_ang_dt;
// reset the accumulators if the time interval is too large
if (_delta_time_of > 1.0f) {
_imu_del_ang_of.setZero();
_delta_time_of = 0.0f;
}
// if accumulation time differences are not excessive and accumulation time is adequate
// compare the optical flow and and navigation rate data and calculate a bias error
if (_fuse_flow) {
if ((fabsf(_delta_time_of - _flow_sample_delayed.dt) < 0.05f) && (_delta_time_of > 0.01f)
&& (_flow_sample_delayed.dt > 0.01f)) {
// calculate a reference angular rate
Vector2f reference_body_rate;
reference_body_rate(0) = _imu_del_ang_of(0) / _delta_time_of;
reference_body_rate(1) = _imu_del_ang_of(1) / _delta_time_of;
// calculate the optical flow sensor measured body rate
Vector2f of_body_rate;
of_body_rate(0) = _flow_sample_delayed.gyroXY(0) / _flow_sample_delayed.dt;
of_body_rate(1) = _flow_sample_delayed.gyroXY(1) / _flow_sample_delayed.dt;
// calculate the bias estimate using a combined LPF and spike filter
_flow_gyro_bias(0) = 0.99f * _flow_gyro_bias(0) + 0.01f * math::constrain((of_body_rate(0) - reference_body_rate(0)),
-0.1f, 0.1f);
_flow_gyro_bias(1) = 0.99f * _flow_gyro_bias(1) + 0.01f * math::constrain((of_body_rate(1) - reference_body_rate(1)),
-0.1f, 0.1f);
}
// reset the accumulators
_imu_del_ang_of.setZero();
_delta_time_of = 0.0f;
}
}
EKF/terrain_estimator.cpp
+153
View File
@@ -0,0 +1,153 @@
/****************************************************************************
*
* Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name ECL nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file terrain_estimator.cpp
* Function for fusing rangefinder measurements to estimate terrain vertical position/
*
* @author Paul Riseborough <p_riseborough@live.com.au>
*
*/
#include "ekf.h"
#include "mathlib.h"
bool Ekf::initHagl()
{
// get most recent range measurement from buffer
rangeSample latest_measurement = _range_buffer.get_newest();
if ((_time_last_imu - latest_measurement.time_us) < 2e5) {
// if we have a fresh measurement, use it to initialise the terrain estimator
_terrain_vpos = _state.pos(2) + latest_measurement.rng;
// initialise state variance to variance of measurement
_terrain_var = sq(_params.range_noise);
// success
return true;
} else if (!_in_air) {
// if on ground we assume a ground clearance
_terrain_vpos = _state.pos(2) + _params.rng_gnd_clearance;
// Use the ground clearance value as our uncertainty
_terrain_var = sq(_params.rng_gnd_clearance);
// ths is a guess
return false;
} else {
// no information - cannot initialise
return false;
}
}
void Ekf::predictHagl()
{
// predict the state variance growth
// the state is the vertical position of the terrain underneath the vehicle
// process noise due to errors in vehicle height estimate
_terrain_var += sq(_imu_sample_delayed.delta_vel_dt * _params.terrain_p_noise);
// process noise due to terrain gradient
_terrain_var += sq(_imu_sample_delayed.delta_vel_dt * _params.terrain_gradient) * (sq(_state.vel(0)) + sq(_state.vel(1)));
// limit the variance to prevent it becoming badly conditioned
_terrain_var = math::constrain(_terrain_var, 0.0f, 1e4f);
}
void Ekf::fuseHagl()
{
// If the vehicle is excessively tilted, do not try to fuse range finder observations
if (_R_prev(2, 2) > 0.7071f) {
// get a height above ground measurement from the range finder assuming a flat earth
float meas_hagl = _range_sample_delayed.rng * _R_prev(2, 2);
// predict the hagl from the vehicle position and terrain height
float pred_hagl = _terrain_vpos - _state.pos(2);
// calculate the innovation
_hagl_innov = pred_hagl - meas_hagl;
// calculate the observation variance adding the variance of the vehicles own height uncertainty and factoring in the effect of tilt on measurement error
float obs_variance = fmaxf(P[8][8], 0.0f) + sq(_params.range_noise / _R_prev(2, 2));
// calculate the innovation variance - limiting it to prevent a badly conditioned fusion
_hagl_innov_var = fmaxf(_terrain_var + obs_variance, obs_variance);
// perform an innovation consistency check and only fuse data if it passes
float gate_size = fmaxf(_params.range_innov_gate, 1.0f);
float test_ratio = sq(_hagl_innov) / (sq(gate_size) * _hagl_innov_var);
if (test_ratio <= 1.0f) {
// calculate the Kalman gain
float gain = _terrain_var / _hagl_innov_var;
// correct the state
_terrain_vpos -= gain * _hagl_innov;
// correct the variance
_terrain_var = fmaxf(_terrain_var * (1.0f - gain), 0.0f);
// record last successful fusion time
_time_last_hagl_fuse = _time_last_imu;
}
} else {
return;
}
}
// return true if the estimate is fresh
// return the estimated vertical position of the terrain relative to the NED origin
bool Ekf::get_terrain_vert_pos(float *ret)
{
memcpy(ret, &_terrain_vpos, sizeof(float));
// The height is useful if the uncertainty in terrain height is significantly smaller than than the estimated height above terrain
bool accuracy_useful = (sqrtf(_terrain_var) < 0.2f * fmaxf((_terrain_vpos - _state.pos(2)), _params.rng_gnd_clearance));
if (_time_last_imu - _time_last_hagl_fuse < 1e6 || accuracy_useful) {
return true;
} else {
return false;
}
}
void Ekf::get_hagl_innov(float *hagl_innov)
{
memcpy(hagl_innov, &_hagl_innov, sizeof(_hagl_innov));
}
void Ekf::get_hagl_innov_var(float *hagl_innov_var)
{
memcpy(hagl_innov_var, &_hagl_innov_var, sizeof(_hagl_innov_var));
}
EKF/vel_pos_fusion.cpp
+33 -10
View File
@@ -36,6 +36,8 @@
* Function for fusing gps and baro measurements/
*
* @author Roman Bast <bapstroman@gmail.com>
* @author Siddharth Bharat Purohit <siddharthbharatpurohit@gmail.com>
* @author Paul Riseborough <p_riseborough@live.com.au>
*
*/
@@ -105,14 +107,27 @@ void Ekf::fuseVelPosHeight()
}
if (_fuse_height) {
fuse_map[5] = true;
// vertical position innovation - baro measurement has opposite sign to earth z axis
_vel_pos_innov[5] = _state.pos(2) - (_baro_at_alignment - _baro_sample_delayed.hgt);
// observation variance - user parameter defined
R[5] = fmaxf(_params.baro_noise, 0.01f);
R[5] = R[5] * R[5];
// innovation gate size
gate_size[5] = fmaxf(_params.baro_innov_gate, 1.0f);
if (_control_status.flags.baro_hgt) {
fuse_map[5] = true;
// vertical position innovation - baro measurement has opposite sign to earth z axis
_vel_pos_innov[5] = _state.pos(2) - (_hgt_at_alignment - _baro_sample_delayed.hgt);
// observation variance - user parameter defined
R[5] = fmaxf(_params.baro_noise, 0.01f);
R[5] = R[5] * R[5];
// innovation gate size
gate_size[5] = fmaxf(_params.baro_innov_gate, 1.0f);
} else if (_control_status.flags.rng_hgt && (_R_prev(2, 2) > 0.7071f)) {
fuse_map[5] = true;
// use range finder with tilt correction
_vel_pos_innov[5] = _state.pos(2) - (-math::max(_range_sample_delayed.rng *_R_prev(2, 2),
_params.rng_gnd_clearance));
// observation variance - user parameter defined
R[5] = fmaxf(_params.range_noise, 0.01f);
R[5] = R[5] * R[5];
// innovation gate size
gate_size[5] = fmaxf(_params.range_innov_gate, 1.0f);
}
}
// calculate innovation test ratios
@@ -122,7 +137,8 @@ void Ekf::fuseVelPosHeight()
unsigned state_index = obs_index + 3; // we start with vx and this is the 4. state
_vel_pos_innov_var[obs_index] = P[state_index][state_index] + R[obs_index];
// Compute the ratio of innovation to gate size
_vel_pos_test_ratio[obs_index] = sq(_vel_pos_innov[obs_index]) / (sq(gate_size[obs_index]) * _vel_pos_innov_var[obs_index]);
_vel_pos_test_ratio[obs_index] = sq(_vel_pos_innov[obs_index]) / (sq(gate_size[obs_index]) *
_vel_pos_innov_var[obs_index]);
}
}
@@ -141,11 +157,13 @@ void Ekf::fuseVelPosHeight()
// record the successful velocity fusion time
if (vel_check_pass && _fuse_hor_vel) {
_time_last_vel_fuse = _time_last_imu;
_tilt_err_vec.setZero();
}
// record the successful position fusion time
if (pos_check_pass && _fuse_pos) {
_time_last_pos_fuse = _time_last_imu;
_tilt_err_vec.setZero();
}
// record the successful height fusion time
@@ -190,6 +208,12 @@ void Ekf::fuseVelPosHeight()
}
}
// sum the attitude error from velocity and position fusion only
// used as a metric for convergence monitoring
if (obs_index != 5) {
_tilt_err_vec += _state.ang_error;
}
// by definition the angle error state is zero at the fusion time
_state.ang_error.setZero();
@@ -222,4 +246,3 @@ void Ekf::fuseVelPosHeight()
}
}
matlab/generated/Inertial Nav EKF/Optical Flow Fusion Alternative.txt
-281
View File
@@ -1,281 +0,0 @@
// Auto code for fusion of line of sight rate massurements from a optical flow camera aligned with the Z body axis
// Generated using MAtlab in-built symbolic toolbox to c-code function
// Observations are body modtion compensated optica flow rates about the X and Y body axis
// Sequential fusion is used (observation errors about each axis are assumed to be uncorrelated)
// intermediate variable from algebraic optimisation
float SH_LOS[14];
SH_LOS[0] = sq(q0) - sq(q1) - sq(q2) + sq(q3);
SH_LOS[1] = vn*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) - vd*(2*q0*q2 - 2*q1*q3) + ve*(2*q0*q3 + 2*q1*q2);
SH_LOS[2] = ve*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + vd*(2*q0*q1 + 2*q2*q3) - vn*(2*q0*q3 - 2*q1*q2);
SH_LOS[3] = 1/(pd - ptd);
SH_LOS[4] = vd*SH_LOS[0] - ve*(2*q0*q1 - 2*q2*q3) + vn*(2*q0*q2 + 2*q1*q3);
SH_LOS[5] = 2.0f*q0*q2 - 2.0f*q1*q3;
SH_LOS[6] = 2.0f*q0*q1 + 2.0f*q2*q3;
SH_LOS[7] = q0*q0;
SH_LOS[8] = q1*q1;
SH_LOS[9] = q2*q2;
SH_LOS[10] = q3*q3;
SH_LOS[11] = q0*q3*2.0f;
SH_LOS[12] = pd-ptd;
SH_LOS[13] = 1.0f/(SH_LOS[12]*SH_LOS[12]);
// Calculate the observation jacobians for the LOS rate about the X body axis
float H_LOS[24];
H_LOS[0] = SH_LOS[3]*SH_LOS[2]*SH_LOS[6]-SH_LOS[3]*SH_LOS[0]*SH_LOS[4];
H_LOS[1] = SH_LOS[3]*SH_LOS[2]*SH_LOS[5];
H_LOS[2] = SH_LOS[3]*SH_LOS[0]*SH_LOS[1];
H_LOS[3] = SH_LOS[3]*SH_LOS[0]*(SH_LOS[11]-q1*q2*2.0f);
H_LOS[4] = -SH_LOS[3]*SH_LOS[0]*(SH_LOS[7]-SH_LOS[8]+SH_LOS[9]-SH_LOS[10]);
H_LOS[5] = -SH_LOS[3]*SH_LOS[0]*SH_LOS[6];
H_LOS[8] = SH_LOS[2]*SH_LOS[0]*SH_LOS[13];
// Calculate the observation jacobians for the LOS rate about the Y body axis
float H_LOS[24];
H_LOS[0] = -SH_LOS[3]*SH_LOS[6]*SH_LOS[1];
H_LOS[1] = -SH_LOS[3]*SH_LOS[0]*SH_LOS[4]-SH_LOS[3]*SH_LOS[1]*SH_LOS[5];
H_LOS[2] = SH_LOS[3]*SH_LOS[2]*SH_LOS[0];
H_LOS[3] = SH_LOS[3]*SH_LOS[0]*(SH_LOS[7]+SH_LOS[8]-SH_LOS[9]-SH_LOS[10]);
H_LOS[4] = SH_LOS[3]*SH_LOS[0]*(SH_LOS[11]+q1*q2*2.0f);
H_LOS[5] = -SH_LOS[3]*SH_LOS[0]*SH_LOS[5];
H_LOS[8] = -SH_LOS[0]*SH_LOS[1]*SH_LOS[13];
// Intermediate variables used to calculate the Kalman gain matrices for the LOS rate about the X body axis
float t2 = SH_LOS[3];
float t3 = SH_LOS[0];
float t4 = SH_LOS[2];
float t5 = SH_LOS[6];
float t100 = t2 * t3 * t5;
float t6 = SH_LOS[4];
float t7 = t2*t3*t6;
float t9 = t2*t4*t5;
float t8 = t7-t9;
float t10 = q0*q3*2.0f;
float t21 = q1*q2*2.0f;
float t11 = t10-t21;
float t101 = t2 * t3 * t11;
float t12 = pd-ptd;
float t13 = 1.0f/(t12*t12);
float t104 = t3 * t4 * t13;
float t14 = SH_LOS[5];
float t102 = t2 * t4 * t14;
float t15 = SH_LOS[1];
float t103 = t2 * t3 * t15;
float t16 = q0*q0;
float t17 = q1*q1;
float t18 = q2*q2;
float t19 = q3*q3;
float t20 = t16-t17+t18-t19;
float t105 = t2 * t3 * t20;
float t22 = P[1][1]*t102;
float t23 = P[3][0]*t101;
float t24 = P[8][0]*t104;
float t25 = P[1][0]*t102;
float t26 = P[2][0]*t103;
float t63 = P[0][0]*t8;
float t64 = P[5][0]*t100;
float t65 = P[4][0]*t105;
float t27 = t23+t24+t25+t26-t63-t64-t65;
float t28 = P[3][3]*t101;
float t29 = P[8][3]*t104;
float t30 = P[1][3]*t102;
float t31 = P[2][3]*t103;
float t67 = P[0][3]*t8;
float t68 = P[5][3]*t100;
float t69 = P[4][3]*t105;
float t32 = t28+t29+t30+t31-t67-t68-t69;
float t33 = t101*t32;
float t34 = P[3][8]*t101;
float t35 = P[8][8]*t104;
float t36 = P[1][8]*t102;
float t37 = P[2][8]*t103;
float t70 = P[0][8]*t8;
float t71 = P[5][8]*t100;
float t72 = P[4][8]*t105;
float t38 = t34+t35+t36+t37-t70-t71-t72;
float t39 = t104*t38;
float t40 = P[3][1]*t101;
float t41 = P[8][1]*t104;
float t42 = P[2][1]*t103;
float t73 = P[0][1]*t8;
float t74 = P[5][1]*t100;
float t75 = P[4][1]*t105;
float t43 = t22+t40+t41+t42-t73-t74-t75;
float t44 = t102*t43;
float t45 = P[3][2]*t101;
float t46 = P[8][2]*t104;
float t47 = P[1][2]*t102;
float t48 = P[2][2]*t103;
float t76 = P[0][2]*t8;
float t77 = P[5][2]*t100;
float t78 = P[4][2]*t105;
float t49 = t45+t46+t47+t48-t76-t77-t78;
float t50 = t103*t49;
float t51 = P[3][5]*t101;
float t52 = P[8][5]*t104;
float t53 = P[1][5]*t102;
float t54 = P[2][5]*t103;
float t79 = P[0][5]*t8;
float t80 = P[5][5]*t100;
float t81 = P[4][5]*t105;
float t55 = t51+t52+t53+t54-t79-t80-t81;
float t56 = P[3][4]*t101;
float t57 = P[8][4]*t104;
float t58 = P[1][4]*t102;
float t59 = P[2][4]*t103;
float t83 = P[0][4]*t8;
float t84 = P[5][4]*t100;
float t85 = P[4][4]*t105;
float t60 = t56+t57+t58+t59-t83-t84-t85;
float t66 = t8*t27;
float t82 = t100*t55;
float t86 = t105*t60;
float t61 = R_LOS+t33+t39+t44+t50-t66-t82-t86; // innovation variance - should always be >= R_LOS
float t62 = 1.0f/t61;
// Calculate the Kalman gain matrix for the LOS rate about the X body axis
float Kfusion[24];
Kfusion[0] = t62*(-P[0][0]*t8-P[0][5]*t100+P[0][3]*t101+P[0][1]*t102+P[0][2]*t103+P[0][8]*t104-P[0][4]*t105);
Kfusion[1] = t62*(t22-P[1][0]*t8-P[1][5]*t100+P[1][3]*t101+P[1][2]*t103+P[1][8]*t104-P[1][4]*t105);
Kfusion[2] = t62*(t48-P[2][0]*t8-P[2][5]*t100+P[2][3]*t101+P[2][1]*t102+P[2][8]*t104-P[2][4]*t105);
Kfusion[3] = t62*(t28-P[3][0]*t8-P[3][5]*t100+P[3][1]*t102+P[3][2]*t103+P[3][8]*t104-P[3][4]*t105);
Kfusion[4] = t62*(-t85-P[4][0]*t8-P[4][5]*t100+P[4][3]*t101+P[4][1]*t102+P[4][2]*t103+P[4][8]*t104);
Kfusion[5] = t62*(-t80-P[5][0]*t8+P[5][3]*t101+P[5][1]*t102+P[5][2]*t103+P[5][8]*t104-P[5][4]*t105);
Kfusion[6] = t62*(-P[6][0]*t8-P[6][5]*t100+P[6][3]*t101+P[6][1]*t102+P[6][2]*t103+P[6][8]*t104-P[6][4]*t105);
Kfusion[7] = t62*(-P[7][0]*t8-P[7][5]*t100+P[7][3]*t101+P[7][1]*t102+P[7][2]*t103+P[7][8]*t104-P[7][4]*t105);
Kfusion[8] = t62*(t35-P[8][0]*t8-P[8][5]*t100+P[8][3]*t101+P[8][1]*t102+P[8][2]*t103-P[8][4]*t105);
Kfusion[9] = t62*(-P[9][0]*t8-P[9][5]*t100+P[9][3]*t101+P[9][1]*t102+P[9][2]*t103+P[9][8]*t104-P[9][4]*t105);
Kfusion[10] = t62*(-P[10][0]*t8-P[10][5]*t100+P[10][3]*t101+P[10][1]*t102+P[10][2]*t103+P[10][8]*t104-P[10][4]*t105);
Kfusion[11] = t62*(-P[11][0]*t8-P[11][5]*t100+P[11][3]*t101+P[11][1]*t102+P[11][2]*t103+P[11][8]*t104-P[11][4]*t105);
Kfusion[12] = t62*(-P[12][0]*t8-P[12][5]*t100+P[12][3]*t101+P[12][1]*t102+P[12][2]*t103+P[12][8]*t104-P[12][4]*t105);
Kfusion[13] = t62*(-P[13][0]*t8-P[13][5]*t100+P[13][3]*t101+P[13][1]*t102+P[13][2]*t103+P[13][8]*t104-P[13][4]*t105);
Kfusion[14] = t62*(-P[14][0]*t8-P[14][5]*t100+P[14][3]*t101+P[14][1]*t102+P[14][2]*t103+P[14][8]*t104-P[14][4]*t105);
Kfusion[15] = t62*(-P[15][0]*t8-P[15][5]*t100+P[15][3]*t101+P[15][1]*t102+P[15][2]*t103+P[15][8]*t104-P[15][4]*t105);
Kfusion[16] = t62*(-P[16][0]*t8-P[16][5]*t100+P[16][3]*t101+P[16][1]*t102+P[16][2]*t103+P[16][8]*t104-P[16][4]*t105);
Kfusion[17] = t62*(-P[17][0]*t8-P[17][5]*t100+P[17][3]*t101+P[17][1]*t102+P[17][2]*t103+P[17][8]*t104-P[17][4]*t105);
Kfusion[18] = t62*(-P[18][0]*t8-P[18][5]*t100+P[18][3]*t101+P[18][1]*t102+P[18][2]*t103+P[18][8]*t104-P[18][4]*t105);
Kfusion[19] = t62*(-P[19][0]*t8-P[19][5]*t100+P[19][3]*t101+P[19][1]*t102+P[19][2]*t103+P[19][8]*t104-P[19][4]*t105);
Kfusion[20] = t62*(-P[20][0]*t8-P[20][5]*t100+P[20][3]*t101+P[20][1]*t102+P[20][2]*t103+P[20][8]*t104-P[20][4]*t105);
Kfusion[21] = t62*(-P[21][0]*t8-P[21][5]*t100+P[21][3]*t101+P[21][1]*t102+P[21][2]*t103+P[21][8]*t104-P[21][4]*t105);
Kfusion[22] = t62*(-P[22][0]*t8-P[22][5]*t100+P[22][3]*t101+P[22][1]*t102+P[22][2]*t103+P[22][8]*t104-P[22][4]*t105);
Kfusion[23] = t62*(-P[23][0]*t8-P[23][5]*t100+P[23][3]*t101+P[23][1]*t102+P[23][2]*t103+P[23][8]*t104-P[23][4]*t105);
// Intermediate variables used to calculate the Kalman gain matrices for the LOS rate about the Y body axis
float t2 = SH_LOS[3];
float t3 = SH_LOS[0];
float t4 = SH_LOS[1];
float t5 = SH_LOS[5];
float t100 = t2 * t3 * t5;
float t6 = SH_LOS[4];
float t7 = t2*t3*t6;
float t8 = t2*t4*t5;
float t9 = t7+t8;
float t10 = q0*q3*2.0f;
float t11 = q1*q2*2.0f;
float t12 = t10+t11;
float t101 = t2 * t3 * t12;
float t13 = pd-ptd;
float t14 = 1.0f/(t13*t13);
float t104 = t3 * t4 * t14;
float t15 = SH_LOS[6];
float t105 = t2 * t4 * t15;
float t16 = SH_LOS[2];
float t102 = t2 * t3 * t16;
float t17 = q0*q0;
float t18 = q1*q1;
float t19 = q2*q2;
float t20 = q3*q3;
float t21 = t17+t18-t19-t20;
float t103 = t2 * t3 * t21;
float t22 = P[0][0]*t105;
float t23 = P[1][1]*t9;
float t24 = P[8][1]*t104;
float t25 = P[0][1]*t105;
float t26 = P[5][1]*t100;
float t64 = P[4][1]*t101;
float t65 = P[2][1]*t102;
float t66 = P[3][1]*t103;
float t27 = t23+t24+t25+t26-t64-t65-t66;
float t28 = t9*t27;
float t29 = P[1][4]*t9;
float t30 = P[8][4]*t104;
float t31 = P[0][4]*t105;
float t32 = P[5][4]*t100;
float t67 = P[4][4]*t101;
float t68 = P[2][4]*t102;
float t69 = P[3][4]*t103;
float t33 = t29+t30+t31+t32-t67-t68-t69;
float t34 = P[1][8]*t9;
float t35 = P[8][8]*t104;
float t36 = P[0][8]*t105;
float t37 = P[5][8]*t100;
float t71 = P[4][8]*t101;
float t72 = P[2][8]*t102;
float t73 = P[3][8]*t103;
float t38 = t34+t35+t36+t37-t71-t72-t73;
float t39 = t104*t38;
float t40 = P[1][0]*t9;
float t41 = P[8][0]*t104;
float t42 = P[5][0]*t100;
float t74 = P[4][0]*t101;
float t75 = P[2][0]*t102;
float t76 = P[3][0]*t103;
float t43 = t22+t40+t41+t42-t74-t75-t76;
float t44 = t105*t43;
float t45 = P[1][2]*t9;
float t46 = P[8][2]*t104;
float t47 = P[0][2]*t105;
float t48 = P[5][2]*t100;
float t63 = P[2][2]*t102;
float t77 = P[4][2]*t101;
float t78 = P[3][2]*t103;
float t49 = t45+t46+t47+t48-t63-t77-t78;
float t50 = P[1][5]*t9;
float t51 = P[8][5]*t104;
float t52 = P[0][5]*t105;
float t53 = P[5][5]*t100;
float t80 = P[4][5]*t101;
float t81 = P[2][5]*t102;
float t82 = P[3][5]*t103;
float t54 = t50+t51+t52+t53-t80-t81-t82;
float t55 = t100*t54;
float t56 = P[1][3]*t9;
float t57 = P[8][3]*t104;
float t58 = P[0][3]*t105;
float t59 = P[5][3]*t100;
float t83 = P[4][3]*t101;
float t84 = P[2][3]*t102;
float t85 = P[3][3]*t103;
float t60 = t56+t57+t58+t59-t83-t84-t85;
float t70 = t101*t33;
float t79 = t102*t49;
float t86 = t103*t60;
float t61 = R_LOS+t28+t39+t44+t55-t70-t79-t86; // innovation variance - should always be >= R_LOS
float t62 = 1.0f/t61;
// Calculate the Kalman gain matrix for the LOS rate about the Y body axis
float Kfusion[24];
Kfusion[0] = -t62*(t22+P[0][1]*t9+P[0][5]*t100-P[0][4]*t101-P[0][2]*t102-P[0][3]*t103+P[0][8]*t104);
Kfusion[1] = -t62*(t23+P[1][5]*t100+P[1][0]*t105-P[1][4]*t101-P[1][2]*t102-P[1][3]*t103+P[1][8]*t104);
Kfusion[2] = -t62*(-t63+P[2][1]*t9+P[2][5]*t100+P[2][0]*t105-P[2][4]*t101-P[2][3]*t103+P[2][8]*t104);
Kfusion[3] = -t62*(-t85+P[3][1]*t9+P[3][5]*t100+P[3][0]*t105-P[3][4]*t101-P[3][2]*t102+P[3][8]*t104);
Kfusion[4] = -t62*(-t67+P[4][1]*t9+P[4][5]*t100+P[4][0]*t105-P[4][2]*t102-P[4][3]*t103+P[4][8]*t104);
Kfusion[5] = -t62*(t53+P[5][1]*t9+P[5][0]*t105-P[5][4]*t101-P[5][2]*t102-P[5][3]*t103+P[5][8]*t104);
Kfusion[6] = -t62*(P[6][1]*t9+P[6][5]*t100+P[6][0]*t105-P[6][4]*t101-P[6][2]*t102-P[6][3]*t103+P[6][8]*t104);
Kfusion[7] = -t62*(P[7][1]*t9+P[7][5]*t100+P[7][0]*t105-P[7][4]*t101-P[7][2]*t102-P[7][3]*t103+P[7][8]*t104);
Kfusion[8] = -t62*(t35+P[8][1]*t9+P[8][5]*t100+P[8][0]*t105-P[8][4]*t101-P[8][2]*t102-P[8][3]*t103);
Kfusion[9] = -t62*(P[9][1]*t9+P[9][5]*t100+P[9][0]*t105-P[9][4]*t101-P[9][2]*t102-P[9][3]*t103+P[9][8]*t104);
Kfusion[10] = -t62*(P[10][1]*t9+P[10][5]*t100+P[10][0]*t105-P[10][4]*t101-P[10][2]*t102-P[10][3]*t103+P[10][8]*t104);
Kfusion[11] = -t62*(P[11][1]*t9+P[11][5]*t100+P[11][0]*t105-P[11][4]*t101-P[11][2]*t102-P[11][3]*t103+P[11][8]*t104);
Kfusion[12] = -t62*(P[12][1]*t9+P[12][5]*t100+P[12][0]*t105-P[12][4]*t101-P[12][2]*t102-P[12][3]*t103+P[12][8]*t104);
Kfusion[13] = -t62*(P[13][1]*t9+P[13][5]*t100+P[13][0]*t105-P[13][4]*t101-P[13][2]*t102-P[13][3]*t103+P[13][8]*t104);
Kfusion[14] = -t62*(P[14][1]*t9+P[14][5]*t100+P[14][0]*t105-P[14][4]*t101-P[14][2]*t102-P[14][3]*t103+P[14][8]*t104);
Kfusion[15] = -t62*(P[15][1]*t9+P[15][5]*t100+P[15][0]*t105-P[15][4]*t101-P[15][2]*t102-P[15][3]*t103+P[15][8]*t104);
Kfusion[16] = -t62*(P[16][1]*t9+P[16][5]*t100+P[16][0]*t105-P[16][4]*t101-P[16][2]*t102-P[16][3]*t103+P[16][8]*t104);
Kfusion[17] = -t62*(P[17][1]*t9+P[17][5]*t100+P[17][0]*t105-P[17][4]*t101-P[17][2]*t102-P[17][3]*t103+P[17][8]*t104);
Kfusion[18] = -t62*(P[18][1]*t9+P[18][5]*t100+P[18][0]*t105-P[18][4]*t101-P[18][2]*t102-P[18][3]*t103+P[18][8]*t104);
Kfusion[19] = -t62*(P[19][1]*t9+P[19][5]*t100+P[19][0]*t105-P[19][4]*t101-P[19][2]*t102-P[19][3]*t103+P[19][8]*t104);
Kfusion[20] = -t62*(P[20][1]*t9+P[20][5]*t100+P[20][0]*t105-P[20][4]*t101-P[20][2]*t102-P[20][3]*t103+P[20][8]*t104);
Kfusion[21] = -t62*(P[21][1]*t9+P[21][5]*t100+P[21][0]*t105-P[21][4]*t101-P[21][2]*t102-P[21][3]*t103+P[21][8]*t104);
Kfusion[22] = -t62*(P[22][1]*t9+P[22][5]*t100+P[22][0]*t105-P[22][4]*t101-P[22][2]*t102-P[22][3]*t103+P[22][8]*t104);
Kfusion[23] = -t62*(P[23][1]*t9+P[23][5]*t100+P[23][0]*t105-P[23][4]*t101-P[23][2]*t102-P[23][3]*t103+P[23][8]*t104);
matlab/generated/Inertial Nav EKF/Optical Flow Fusion.txt
+212 -107
View File
@@ -3,114 +3,219 @@
// Observations are body modtion compensated optica flow rates about the X and Y body axis
// Sequential fusion is used (observation errors about each axis are assumed to be uncorrelated)
// intermediate variable from algebraic optimisation
float SH_LOS[7];
SH_LOS[0] = sq(q0) - sq(q1) - sq(q2) + sq(q3);
SH_LOS[1] = vn*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) - vd*(2.0f*q0*q2 - 2.0f*q1*q3) + ve*(2.0f*q0*q3 + 2.0f*q1*q2);
SH_LOS[2] = ve*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + vd*(2.0f*q0*q1 + 2.0f*q2*q3) - vn*(2.0f*q0*q3 - 2.0f*q1*q2);
SH_LOS[3] = 1.0f/(pd - ptd);
SH_LOS[4] = vd*SH_LOS[0] - ve*(2.0f*q0*q1 - 2.0f*q2*q3) + vn*(2.0f*q0*q2 + 2.0f*q1*q3);
SH_LOS[5] = 2.0f*q0*q2 - 2.0f*q1*q3;
SH_LOS[6] = 2.0f*q0*q1 + 2.0f*q2*q3;
float H_LOS[2][24];
// Calculate the observation jacobians for the LOS rate about the X body axis
float H_LOS[24];
H_LOS[0] = SH_LOS[2]*SH_LOS[3]*SH_LOS[6] - SH_LOS[0]*SH_LOS[3]*SH_LOS[4];
H_LOS[1] = SH_LOS[2]*SH_LOS[3]*SH_LOS[5];
H_LOS[2] = SH_LOS[0]*SH_LOS[1]*SH_LOS[3];
H_LOS[3] = SH_LOS[0]*SH_LOS[3]*(2.0f*q0*q3 - 2.0f*q1*q2);
H_LOS[4] = -SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3));
H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*SH_LOS[6];
H_LOS[8] = SH_LOS[0]*SH_LOS[2]*sq(SH_LOS[3]);
// calculate X axis observation Jacobian
float t2 = 1.0f / range;
float t3 = q0 * q0;
float t4 = q1 * q1;
float t5 = q2 * q2;
float t6 = q3 * q3;
float t7 = q0 * q2 * 2.0f;
float t8 = q1 * q3 * 2.0f;
float t9 = q0 * q3 * 2.0f;
float t10 = q1 * q2 * 2.0f;
float t11 = q0 * q1 * 2.0f;
H_LOS[0][0] = t2 * (vn * (t7 + t8) + vd * (t3 - t4 - t5 + t6) - ve * (t11 - q2 * q3 * 2.0f));
H_LOS[0][2] = -t2 * (ve * (t9 + t10) - vd * (t7 - t8) + vn * (t3 + t4 - t5 - t6));
H_LOS[0][3] = -t2 * (t9 - t10);
H_LOS[0][4] = t2 * (t3 - t4 + t5 - t6);
H_LOS[0][5] = t2 * (t11 + q2 * q3 * 2.0f);
// Calculate the observation jacobians for the LOS rate about the Y body axis
float H_LOS[24];
H_LOS[0] = -SH_LOS[1]*SH_LOS[3]*SH_LOS[6];
H_LOS[1] = - SH_LOS[0]*SH_LOS[3]*SH_LOS[4] - SH_LOS[1]*SH_LOS[3]*SH_LOS[5];
H_LOS[2] = SH_LOS[0]*SH_LOS[2]*SH_LOS[3];
H_LOS[3] = SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3));
H_LOS[4] = SH_LOS[0]*SH_LOS[3]*(2.0f*q0*q3 + 2.0f*q1*q2);
H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*SH_LOS[5];
H_LOS[8] = -SH_LOS[0]*SH_LOS[1]*sq(SH_LOS[3]);
// calculate X axis Kalman gains
t2 = 1.0/range;
t3 = q0*q1*2.0;
t4 = q2*q3*2.0;
t5 = q0*q0;
t6 = q1*q1;
t7 = q2*q2;
t8 = q3*q3;
t9 = q0*q2*2.0;
t10 = q1*q3*2.0;
t11 = q0*q3*2.0;
float t12 = q1*q2*2.0;
float t13 = t11-t12;
float t14 = t3+t4;
float t15 = t5-t6-t7+t8;
float t16 = t15*vd;
float t17 = t3-t4;
float t18 = t9+t10;
float t19 = t18*vn;
float t28 = t17*ve;
float t20 = t16+t19-t28;
float t21 = t5+t6-t7-t8;
float t22 = t21*vn;
float t23 = t9-t10;
float t24 = t11+t12;
float t25 = t24*ve;
float t29 = t23*vd;
float t26 = t22+t25-t29;
float t27 = t5-t6+t7-t8;
float t30 = P[0][0]*t2*t20;
float t31 = P[5][3]*t2*t14;
float t32 = P[0][3]*t2*t20;
float t33 = P[4][3]*t2*t27;
float t56 = P[3][3]*t2*t13;
float t57 = P[2][3]*t2*t26;
float t34 = t31+t32+t33-t56-t57;
float t35 = P[5][5]*t2*t14;
float t36 = P[0][5]*t2*t20;
float t37 = P[4][5]*t2*t27;
float t59 = P[3][5]*t2*t13;
float t60 = P[2][5]*t2*t26;
float t38 = t35+t36+t37-t59-t60;
float t39 = t2*t14*t38;
float t40 = P[5][0]*t2*t14;
float t41 = P[4][0]*t2*t27;
float t61 = P[3][0]*t2*t13;
float t62 = P[2][0]*t2*t26;
float t42 = t30+t40+t41-t61-t62;
float t43 = t2*t20*t42;
float t44 = P[5][2]*t2*t14;
float t45 = P[0][2]*t2*t20;
float t46 = P[4][2]*t2*t27;
float t55 = P[2][2]*t2*t26;
float t63 = P[3][2]*t2*t13;
float t47 = t44+t45+t46-t55-t63;
float t48 = P[5][4]*t2*t14;
float t49 = P[0][4]*t2*t20;
float t50 = P[4][4]*t2*t27;
float t65 = P[3][4]*t2*t13;
float t66 = P[2][4]*t2*t26;
float t51 = t48+t49+t50-t65-t66;
float t52 = t2*t27*t51;
float t58 = t2*t13*t34;
float t64 = t2*t26*t47;
float t53 = R_LOS+t39+t43+t52-t58-t64;
float t54 = 1.0/t53;
Kfusion[0][0] = t54*(t30-P[0][3]*t2*(t11-q1*q2*2.0)+P[0][5]*t2*t14-P[0][2]*t2*t26+P[0][4]*t2*t27);
Kfusion[1][0] = t54*(-P[1][3]*t2*t13+P[1][5]*t2*t14+P[1][0]*t2*t20-P[1][2]*t2*t26+P[1][4]*t2*t27);
Kfusion[2][0] = t54*(-t55-P[2][3]*t2*t13+P[2][5]*t2*t14+P[2][0]*t2*t20+P[2][4]*t2*t27);
Kfusion[3][0] = t54*(-t56+P[3][5]*t2*t14+P[3][0]*t2*t20-P[3][2]*t2*t26+P[3][4]*t2*t27);
Kfusion[4][0] = t54*(t50-P[4][3]*t2*t13+P[4][5]*t2*t14+P[4][0]*t2*t20-P[4][2]*t2*t26);
Kfusion[5][0] = t54*(t35-P[5][3]*t2*t13+P[5][0]*t2*t20-P[5][2]*t2*t26+P[5][4]*t2*t27);
Kfusion[6][0] = t54*(-P[6][3]*t2*t13+P[6][5]*t2*t14+P[6][0]*t2*t20-P[6][2]*t2*t26+P[6][4]*t2*t27);
Kfusion[7][0] = t54*(-P[7][3]*t2*t13+P[7][5]*t2*t14+P[7][0]*t2*t20-P[7][2]*t2*t26+P[7][4]*t2*t27);
Kfusion[8][0] = t54*(-P[8][3]*t2*t13+P[8][5]*t2*t14+P[8][0]*t2*t20-P[8][2]*t2*t26+P[8][4]*t2*t27);
Kfusion[9][0] = t54*(-P[9][3]*t2*t13+P[9][5]*t2*t14+P[9][0]*t2*t20-P[9][2]*t2*t26+P[9][4]*t2*t27);
Kfusion[10][0] = t54*(-P[10][3]*t2*t13+P[10][5]*t2*t14+P[10][0]*t2*t20-P[10][2]*t2*t26+P[10][4]*t2*t27);
Kfusion[11][0] = t54*(-P[11][3]*t2*t13+P[11][5]*t2*t14+P[11][0]*t2*t20-P[11][2]*t2*t26+P[11][4]*t2*t27);
Kfusion[12][0] = t54*(-P[12][3]*t2*t13+P[12][5]*t2*t14+P[12][0]*t2*t20-P[12][2]*t2*t26+P[12][4]*t2*t27);
Kfusion[13][0] = t54*(-P[13][3]*t2*t13+P[13][5]*t2*t14+P[13][0]*t2*t20-P[13][2]*t2*t26+P[13][4]*t2*t27);
Kfusion[14][0] = t54*(-P[14][3]*t2*t13+P[14][5]*t2*t14+P[14][0]*t2*t20-P[14][2]*t2*t26+P[14][4]*t2*t27);
Kfusion[15][0] = t54*(-P[15][3]*t2*t13+P[15][5]*t2*t14+P[15][0]*t2*t20-P[15][2]*t2*t26+P[15][4]*t2*t27);
Kfusion[16][0] = t54*(-P[16][3]*t2*t13+P[16][5]*t2*t14+P[16][0]*t2*t20-P[16][2]*t2*t26+P[16][4]*t2*t27);
Kfusion[17][0] = t54*(-P[17][3]*t2*t13+P[17][5]*t2*t14+P[17][0]*t2*t20-P[17][2]*t2*t26+P[17][4]*t2*t27);
Kfusion[18][0] = t54*(-P[18][3]*t2*t13+P[18][5]*t2*t14+P[18][0]*t2*t20-P[18][2]*t2*t26+P[18][4]*t2*t27);
Kfusion[19][0] = t54*(-P[19][3]*t2*t13+P[19][5]*t2*t14+P[19][0]*t2*t20-P[19][2]*t2*t26+P[19][4]*t2*t27);
Kfusion[20][0] = t54*(-P[20][3]*t2*t13+P[20][5]*t2*t14+P[20][0]*t2*t20-P[20][2]*t2*t26+P[20][4]*t2*t27);
Kfusion[21][0] = t54*(-P[21][3]*t2*t13+P[21][5]*t2*t14+P[21][0]*t2*t20-P[21][2]*t2*t26+P[21][4]*t2*t27);
Kfusion[22][0] = t54*(-P[22][3]*t2*t13+P[22][5]*t2*t14+P[22][0]*t2*t20-P[22][2]*t2*t26+P[22][4]*t2*t27);
Kfusion[23][0] = t54*(-P[23][3]*t2*t13+P[23][5]*t2*t14+P[23][0]*t2*t20-P[23][2]*t2*t26+P[23][4]*t2*t27);
// calculate Y axis observation jacobian
float t2 = 1.0f/range;
float t3 = q0*q0;
float t4 = q1*q1;
float t5 = q2*q2;
float t6 = q3*q3;
float t7 = q0*q1*2.0f;
float t8 = q0*q3*2.0f;
float t9 = q0*q2*2.0f;
float t10 = q1*q3*2.0f;
H_LOS[1][1] = t2*(vn*(t9+t10)+vd*(t3-t4-t5+t6)-ve*(t7-q2*q3*2.0f));
H_LOS[1][2] = -t2*(ve*(t3-t4+t5-t6)+vd*(t7+q2*q3*2.0f)-vn*(t8-q1*q2*2.0f));
H_LOS[1][3] = -t2*(t3+t4-t5-t6);
H_LOS[1][4] = -t2*(t8+q1*q2*2.0f);
H_LOS[1][5] = t2*(t9-t10);
// Intermediate variables used to calculate the Kalman gain matrices
float SK_LOS[22];
// this is 1/(innovation variance) for the X axis measurement
SK_LOS[0] = 1.0f/(R_LOS - (SH_LOS[0]*SH_LOS[3]*SH_LOS[4] - SH_LOS[2]*SH_LOS[3]*SH_LOS[6])*(P[8][0]*SH_LOS[0]*SH_LOS[2]*sq(SH_LOS[3]) - P[0][0]*(SH_LOS[0]*SH_LOS[3]*SH_LOS[4] - SH_LOS[2]*SH_LOS[3]*SH_LOS[6]) + P[3][0]*SH_LOS[0]*SH_LOS[3]*(2.0f*q0*q3 - 2.0f*q1*q2) + P[1][0]*SH_LOS[2]*SH_LOS[3]*SH_LOS[5] + P[2][0]*SH_LOS[0]*SH_LOS[1]*SH_LOS[3] - P[5][0]*SH_LOS[0]*SH_LOS[3]*SH_LOS[6] - P[4][0]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) + SH_LOS[2]*SH_LOS[3]*SH_LOS[5]*(P[8][1]*SH_LOS[0]*SH_LOS[2]*sq(SH_LOS[3]) - P[0][1]*(SH_LOS[0]*SH_LOS[3]*SH_LOS[4] - SH_LOS[2]*SH_LOS[3]*SH_LOS[6]) + P[3][1]*SH_LOS[0]*SH_LOS[3]*(2.0f*q0*q3 - 2.0f*q1*q2) + P[1][1]*SH_LOS[2]*SH_LOS[3]*SH_LOS[5] + P[2][1]*SH_LOS[0]*SH_LOS[1]*SH_LOS[3] - P[5][1]*SH_LOS[0]*SH_LOS[3]*SH_LOS[6] - P[4][1]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) + SH_LOS[0]*SH_LOS[1]*SH_LOS[3]*(P[8][2]*SH_LOS[0]*SH_LOS[2]*sq(SH_LOS[3]) - P[0][2]*(SH_LOS[0]*SH_LOS[3]*SH_LOS[4] - SH_LOS[2]*SH_LOS[3]*SH_LOS[6]) + P[3][2]*SH_LOS[0]*SH_LOS[3]*(2.0f*q0*q3 - 2.0f*q1*q2) + P[1][2]*SH_LOS[2]*SH_LOS[3]*SH_LOS[5] + P[2][2]*SH_LOS[0]*SH_LOS[1]*SH_LOS[3] - P[5][2]*SH_LOS[0]*SH_LOS[3]*SH_LOS[6] - P[4][2]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) - SH_LOS[0]*SH_LOS[3]*SH_LOS[6]*(P[8][5]*SH_LOS[0]*SH_LOS[2]*sq(SH_LOS[3]) - P[0][5]*(SH_LOS[0]*SH_LOS[3]*SH_LOS[4] - SH_LOS[2]*SH_LOS[3]*SH_LOS[6]) + P[3][5]*SH_LOS[0]*SH_LOS[3]*(2.0f*q0*q3 - 2.0f*q1*q2) + P[1][5]*SH_LOS[2]*SH_LOS[3]*SH_LOS[5] + P[2][5]*SH_LOS[0]*SH_LOS[1]*SH_LOS[3] - P[5][5]*SH_LOS[0]*SH_LOS[3]*SH_LOS[6] - P[4][5]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) - SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))*(P[8][4]*SH_LOS[0]*SH_LOS[2]*sq(SH_LOS[3]) - P[0][4]*(SH_LOS[0]*SH_LOS[3]*SH_LOS[4] - SH_LOS[2]*SH_LOS[3]*SH_LOS[6]) + P[3][4]*SH_LOS[0]*SH_LOS[3]*(2.0f*q0*q3 - 2.0f*q1*q2) + P[1][4]*SH_LOS[2]*SH_LOS[3]*SH_LOS[5] + P[2][4]*SH_LOS[0]*SH_LOS[1]*SH_LOS[3] - P[5][4]*SH_LOS[0]*SH_LOS[3]*SH_LOS[6] - P[4][4]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) + SH_LOS[0]*SH_LOS[2]*sq(SH_LOS[3])*(P[8][8]*SH_LOS[0]*SH_LOS[2]*sq(SH_LOS[3]) - P[0][8]*(SH_LOS[0]*SH_LOS[3]*SH_LOS[4] - SH_LOS[2]*SH_LOS[3]*SH_LOS[6]) + P[3][8]*SH_LOS[0]*SH_LOS[3]*(2.0f*q0*q3 - 2.0f*q1*q2) + P[1][8]*SH_LOS[2]*SH_LOS[3]*SH_LOS[5] + P[2][8]*SH_LOS[0]*SH_LOS[1]*SH_LOS[3] - P[5][8]*SH_LOS[0]*SH_LOS[3]*SH_LOS[6] - P[4][8]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) + SH_LOS[0]*SH_LOS[3]*(2.0f*q0*q3 - 2.0f*q1*q2)*(P[8][3]*SH_LOS[0]*SH_LOS[2]*sq(SH_LOS[3]) - P[0][3]*(SH_LOS[0]*SH_LOS[3]*SH_LOS[4] - SH_LOS[2]*SH_LOS[3]*SH_LOS[6]) + P[3][3]*SH_LOS[0]*SH_LOS[3]*(2.0f*q0*q3 - 2.0f*q1*q2) + P[1][3]*SH_LOS[2]*SH_LOS[3]*SH_LOS[5] + P[2][3]*SH_LOS[0]*SH_LOS[1]*SH_LOS[3] - P[5][3]*SH_LOS[0]*SH_LOS[3]*SH_LOS[6] - P[4][3]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))));
// this is 1/(innovation variance) for the Y axis measurement
SK_LOS[1] = 1.0f/(R_LOS + (SH_LOS[0]*SH_LOS[3]*SH_LOS[4] + SH_LOS[1]*SH_LOS[3]*SH_LOS[5])*(P[1][1]*(SH_LOS[0]*SH_LOS[3]*SH_LOS[4] + SH_LOS[1]*SH_LOS[3]*SH_LOS[5]) + P[8][1]*SH_LOS[0]*SH_LOS[1]*sq(SH_LOS[3]) - P[4][1]*SH_LOS[0]*SH_LOS[3]*(2.0f*q0*q3 + 2.0f*q1*q2) + P[0][1]*SH_LOS[1]*SH_LOS[3]*SH_LOS[6] - P[2][1]*SH_LOS[0]*SH_LOS[2]*SH_LOS[3] + P[5][1]*SH_LOS[0]*SH_LOS[3]*SH_LOS[5] - P[3][1]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) + SH_LOS[1]*SH_LOS[3]*SH_LOS[6]*(P[1][0]*(SH_LOS[0]*SH_LOS[3]*SH_LOS[4] + SH_LOS[1]*SH_LOS[3]*SH_LOS[5]) + P[8][0]*SH_LOS[0]*SH_LOS[1]*sq(SH_LOS[3]) - P[4][0]*SH_LOS[0]*SH_LOS[3]*(2.0f*q0*q3 + 2.0f*q1*q2) + P[0][0]*SH_LOS[1]*SH_LOS[3]*SH_LOS[6] - P[2][0]*SH_LOS[0]*SH_LOS[2]*SH_LOS[3] + P[5][0]*SH_LOS[0]*SH_LOS[3]*SH_LOS[5] - P[3][0]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) - SH_LOS[0]*SH_LOS[2]*SH_LOS[3]*(P[1][2]*(SH_LOS[0]*SH_LOS[3]*SH_LOS[4] + SH_LOS[1]*SH_LOS[3]*SH_LOS[5]) + P[8][2]*SH_LOS[0]*SH_LOS[1]*sq(SH_LOS[3]) - P[4][2]*SH_LOS[0]*SH_LOS[3]*(2.0f*q0*q3 + 2.0f*q1*q2) + P[0][2]*SH_LOS[1]*SH_LOS[3]*SH_LOS[6] - P[2][2]*SH_LOS[0]*SH_LOS[2]*SH_LOS[3] + P[5][2]*SH_LOS[0]*SH_LOS[3]*SH_LOS[5] - P[3][2]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) + SH_LOS[0]*SH_LOS[3]*SH_LOS[5]*(P[1][5]*(SH_LOS[0]*SH_LOS[3]*SH_LOS[4] + SH_LOS[1]*SH_LOS[3]*SH_LOS[5]) + P[8][5]*SH_LOS[0]*SH_LOS[1]*sq(SH_LOS[3]) - P[4][5]*SH_LOS[0]*SH_LOS[3]*(2.0f*q0*q3 + 2.0f*q1*q2) + P[0][5]*SH_LOS[1]*SH_LOS[3]*SH_LOS[6] - P[2][5]*SH_LOS[0]*SH_LOS[2]*SH_LOS[3] + P[5][5]*SH_LOS[0]*SH_LOS[3]*SH_LOS[5] - P[3][5]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) - SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))*(P[1][3]*(SH_LOS[0]*SH_LOS[3]*SH_LOS[4] + SH_LOS[1]*SH_LOS[3]*SH_LOS[5]) + P[8][3]*SH_LOS[0]*SH_LOS[1]*sq(SH_LOS[3]) - P[4][3]*SH_LOS[0]*SH_LOS[3]*(2.0f*q0*q3 + 2.0f*q1*q2) + P[0][3]*SH_LOS[1]*SH_LOS[3]*SH_LOS[6] - P[2][3]*SH_LOS[0]*SH_LOS[2]*SH_LOS[3] + P[5][3]*SH_LOS[0]*SH_LOS[3]*SH_LOS[5] - P[3][3]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) + SH_LOS[0]*SH_LOS[1]*sq(SH_LOS[3])*(P[1][8]*(SH_LOS[0]*SH_LOS[3]*SH_LOS[4] + SH_LOS[1]*SH_LOS[3]*SH_LOS[5]) + P[8][8]*SH_LOS[0]*SH_LOS[1]*sq(SH_LOS[3]) - P[4][8]*SH_LOS[0]*SH_LOS[3]*(2.0f*q0*q3 + 2.0f*q1*q2) + P[0][8]*SH_LOS[1]*SH_LOS[3]*SH_LOS[6] - P[2][8]*SH_LOS[0]*SH_LOS[2]*SH_LOS[3] + P[5][8]*SH_LOS[0]*SH_LOS[3]*SH_LOS[5] - P[3][8]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) - SH_LOS[0]*SH_LOS[3]*(2.0f*q0*q3 + 2.0f*q1*q2)*(P[1][4]*(SH_LOS[0]*SH_LOS[3]*SH_LOS[4] + SH_LOS[1]*SH_LOS[3]*SH_LOS[5]) + P[8][4]*SH_LOS[0]*SH_LOS[1]*sq(SH_LOS[3]) - P[4][4]*SH_LOS[0]*SH_LOS[3]*(2.0f*q0*q3 + 2.0f*q1*q2) + P[0][4]*SH_LOS[1]*SH_LOS[3]*SH_LOS[6] - P[2][4]*SH_LOS[0]*SH_LOS[2]*SH_LOS[3] + P[5][4]*SH_LOS[0]*SH_LOS[3]*SH_LOS[5] - P[3][4]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))));
SK_LOS[2] = sq(q0) + sq(q1) - sq(q2) - sq(q3);
SK_LOS[3] = sq(q0) - sq(q1) + sq(q2) - sq(q3);
SK_LOS[4] = SH_LOS[3];
SK_LOS[5] = SH_LOS[0]*SH_LOS[2]*sq(SK_LOS[4]);
SK_LOS[6] = SH_LOS[0]*SH_LOS[4]*SK_LOS[4];
SK_LOS[7] = SH_LOS[2]*SH_LOS[6]*SK_LOS[4];
SK_LOS[8] = SH_LOS[0]*SK_LOS[4]*(2.0f*q0*q3 - 2.0f*q1*q2);
SK_LOS[9] = SH_LOS[0]*SH_LOS[1]*SK_LOS[4];
SK_LOS[10] = SH_LOS[2]*SH_LOS[5]*SK_LOS[4];
SK_LOS[11] = SH_LOS[0]*SH_LOS[6]*SK_LOS[4];
SK_LOS[12] = SH_LOS[0]*SK_LOS[3]*SK_LOS[4];
SK_LOS[13] = SH_LOS[1]*SH_LOS[5]*SK_LOS[4];
SK_LOS[14] = SH_LOS[0]*SH_LOS[1]*sq(SK_LOS[4]);
SK_LOS[15] = SH_LOS[0]*SK_LOS[4]*(2.0f*q0*q3 + 2.0f*q1*q2);
SK_LOS[16] = SH_LOS[0]*SH_LOS[2]*SK_LOS[4];
SK_LOS[17] = SH_LOS[1]*SH_LOS[6]*SK_LOS[4];
SK_LOS[18] = SH_LOS[0]*SH_LOS[5]*SK_LOS[4];
SK_LOS[19] = SH_LOS[0]*SK_LOS[2]*SK_LOS[4];
SK_LOS[20] = SK_LOS[6] - SK_LOS[7];
SK_LOS[21] = SK_LOS[6] + SK_LOS[13];
// Calculate the Kalman gain matrix for the X axis measurement
float Kfusion[24];
Kfusion[0] = SK_LOS[0]*(P[0][8]*SK_LOS[5] - P[0][0]*SK_LOS[20] + P[0][3]*SK_LOS[8] + P[0][2]*SK_LOS[9] + P[0][1]*SK_LOS[10] - P[0][5]*SK_LOS[11] - P[0][4]*SK_LOS[12]);
Kfusion[1] = SK_LOS[0]*(P[1][8]*SK_LOS[5] - P[1][0]*SK_LOS[20] + P[1][3]*SK_LOS[8] + P[1][2]*SK_LOS[9] + P[1][1]*SK_LOS[10] - P[1][5]*SK_LOS[11] - P[1][4]*SK_LOS[12]);
Kfusion[2] = SK_LOS[0]*(P[2][8]*SK_LOS[5] - P[2][0]*SK_LOS[20] + P[2][3]*SK_LOS[8] + P[2][2]*SK_LOS[9] + P[2][1]*SK_LOS[10] - P[2][5]*SK_LOS[11] - P[2][4]*SK_LOS[12]);
Kfusion[3] = SK_LOS[0]*(P[3][8]*SK_LOS[5] - P[3][0]*SK_LOS[20] + P[3][3]*SK_LOS[8] + P[3][2]*SK_LOS[9] + P[3][1]*SK_LOS[10] - P[3][5]*SK_LOS[11] - P[3][4]*SK_LOS[12]);
Kfusion[4] = SK_LOS[0]*(P[4][8]*SK_LOS[5] - P[4][0]*SK_LOS[20] + P[4][3]*SK_LOS[8] + P[4][2]*SK_LOS[9] + P[4][1]*SK_LOS[10] - P[4][5]*SK_LOS[11] - P[4][4]*SK_LOS[12]);
Kfusion[5] = SK_LOS[0]*(P[5][8]*SK_LOS[5] - P[5][0]*SK_LOS[20] + P[5][3]*SK_LOS[8] + P[5][2]*SK_LOS[9] + P[5][1]*SK_LOS[10] - P[5][5]*SK_LOS[11] - P[5][4]*SK_LOS[12]);
Kfusion[6] = SK_LOS[0]*(P[6][8]*SK_LOS[5] - P[6][0]*SK_LOS[20] + P[6][3]*SK_LOS[8] + P[6][2]*SK_LOS[9] + P[6][1]*SK_LOS[10] - P[6][5]*SK_LOS[11] - P[6][4]*SK_LOS[12]);
Kfusion[7] = SK_LOS[0]*(P[7][8]*SK_LOS[5] - P[7][0]*SK_LOS[20] + P[7][3]*SK_LOS[8] + P[7][2]*SK_LOS[9] + P[7][1]*SK_LOS[10] - P[7][5]*SK_LOS[11] - P[7][4]*SK_LOS[12]);
Kfusion[8] = SK_LOS[0]*(P[8][8]*SK_LOS[5] - P[8][0]*SK_LOS[20] + P[8][3]*SK_LOS[8] + P[8][2]*SK_LOS[9] + P[8][1]*SK_LOS[10] - P[8][5]*SK_LOS[11] - P[8][4]*SK_LOS[12]);
Kfusion[9] = SK_LOS[0]*(P[9][8]*SK_LOS[5] - P[9][0]*SK_LOS[20] + P[9][3]*SK_LOS[8] + P[9][2]*SK_LOS[9] + P[9][1]*SK_LOS[10] - P[9][5]*SK_LOS[11] - P[9][4]*SK_LOS[12]);
Kfusion[10] = SK_LOS[0]*(P[10][8]*SK_LOS[5] - P[10][0]*SK_LOS[20] + P[10][3]*SK_LOS[8] + P[10][2]*SK_LOS[9] + P[10][1]*SK_LOS[10] - P[10][5]*SK_LOS[11] - P[10][4]*SK_LOS[12]);
Kfusion[11] = SK_LOS[0]*(P[11][8]*SK_LOS[5] - P[11][0]*SK_LOS[20] + P[11][3]*SK_LOS[8] + P[11][2]*SK_LOS[9] + P[11][1]*SK_LOS[10] - P[11][5]*SK_LOS[11] - P[11][4]*SK_LOS[12]);
Kfusion[12] = SK_LOS[0]*(P[12][8]*SK_LOS[5] - P[12][0]*SK_LOS[20] + P[12][3]*SK_LOS[8] + P[12][2]*SK_LOS[9] + P[12][1]*SK_LOS[10] - P[12][5]*SK_LOS[11] - P[12][4]*SK_LOS[12]);
Kfusion[13] = SK_LOS[0]*(P[13][8]*SK_LOS[5] - P[13][0]*SK_LOS[20] + P[13][3]*SK_LOS[8] + P[13][2]*SK_LOS[9] + P[13][1]*SK_LOS[10] - P[13][5]*SK_LOS[11] - P[13][4]*SK_LOS[12]);
Kfusion[14] = SK_LOS[0]*(P[14][8]*SK_LOS[5] - P[14][0]*SK_LOS[20] + P[14][3]*SK_LOS[8] + P[14][2]*SK_LOS[9] + P[14][1]*SK_LOS[10] - P[14][5]*SK_LOS[11] - P[14][4]*SK_LOS[12]);
Kfusion[15] = SK_LOS[0]*(P[15][8]*SK_LOS[5] - P[15][0]*SK_LOS[20] + P[15][3]*SK_LOS[8] + P[15][2]*SK_LOS[9] + P[15][1]*SK_LOS[10] - P[15][5]*SK_LOS[11] - P[15][4]*SK_LOS[12]);
Kfusion[16] = SK_LOS[0]*(P[16][8]*SK_LOS[5] - P[16][0]*SK_LOS[20] + P[16][3]*SK_LOS[8] + P[16][2]*SK_LOS[9] + P[16][1]*SK_LOS[10] - P[16][5]*SK_LOS[11] - P[16][4]*SK_LOS[12]);
Kfusion[17] = SK_LOS[0]*(P[17][8]*SK_LOS[5] - P[17][0]*SK_LOS[20] + P[17][3]*SK_LOS[8] + P[17][2]*SK_LOS[9] + P[17][1]*SK_LOS[10] - P[17][5]*SK_LOS[11] - P[17][4]*SK_LOS[12]);
Kfusion[18] = SK_LOS[0]*(P[18][8]*SK_LOS[5] - P[18][0]*SK_LOS[20] + P[18][3]*SK_LOS[8] + P[18][2]*SK_LOS[9] + P[18][1]*SK_LOS[10] - P[18][5]*SK_LOS[11] - P[18][4]*SK_LOS[12]);
Kfusion[19] = SK_LOS[0]*(P[19][8]*SK_LOS[5] - P[19][0]*SK_LOS[20] + P[19][3]*SK_LOS[8] + P[19][2]*SK_LOS[9] + P[19][1]*SK_LOS[10] - P[19][5]*SK_LOS[11] - P[19][4]*SK_LOS[12]);
Kfusion[20] = SK_LOS[0]*(P[20][8]*SK_LOS[5] - P[20][0]*SK_LOS[20] + P[20][3]*SK_LOS[8] + P[20][2]*SK_LOS[9] + P[20][1]*SK_LOS[10] - P[20][5]*SK_LOS[11] - P[20][4]*SK_LOS[12]);
Kfusion[21] = SK_LOS[0]*(P[21][8]*SK_LOS[5] - P[21][0]*SK_LOS[20] + P[21][3]*SK_LOS[8] + P[21][2]*SK_LOS[9] + P[21][1]*SK_LOS[10] - P[21][5]*SK_LOS[11] - P[21][4]*SK_LOS[12]);
Kfusion[22] = SK_LOS[0]*(P[22][8]*SK_LOS[5] - P[22][0]*SK_LOS[20] + P[22][3]*SK_LOS[8] + P[22][2]*SK_LOS[9] + P[22][1]*SK_LOS[10] - P[22][5]*SK_LOS[11] - P[22][4]*SK_LOS[12]);
Kfusion[23] = SK_LOS[0]*(P[23][8]*SK_LOS[5] - P[23][0]*SK_LOS[20] + P[23][3]*SK_LOS[8] + P[23][2]*SK_LOS[9] + P[23][1]*SK_LOS[10] - P[23][5]*SK_LOS[11] - P[23][4]*SK_LOS[12]);
// Calculate the Kalman gain matrix for the Y axis measurement
float Kfusion[24];
Kfusion[0] = -SK_LOS[1]*(P[0][1]*SK_LOS[21] + P[0][8]*SK_LOS[14] - P[0][4]*SK_LOS[15] - P[0][2]*SK_LOS[16] + P[0][0]*SK_LOS[17] + P[0][5]*SK_LOS[18] - P[0][3]*SK_LOS[19]);
Kfusion[1] = -SK_LOS[1]*(P[1][1]*SK_LOS[21] + P[1][8]*SK_LOS[14] - P[1][4]*SK_LOS[15] - P[1][2]*SK_LOS[16] + P[1][0]*SK_LOS[17] + P[1][5]*SK_LOS[18] - P[1][3]*SK_LOS[19]);
Kfusion[2] = -SK_LOS[1]*(P[2][1]*SK_LOS[21] + P[2][8]*SK_LOS[14] - P[2][4]*SK_LOS[15] - P[2][2]*SK_LOS[16] + P[2][0]*SK_LOS[17] + P[2][5]*SK_LOS[18] - P[2][3]*SK_LOS[19]);
Kfusion[3] = -SK_LOS[1]*(P[3][1]*SK_LOS[21] + P[3][8]*SK_LOS[14] - P[3][4]*SK_LOS[15] - P[3][2]*SK_LOS[16] + P[3][0]*SK_LOS[17] + P[3][5]*SK_LOS[18] - P[3][3]*SK_LOS[19]);
Kfusion[4] = -SK_LOS[1]*(P[4][1]*SK_LOS[21] + P[4][8]*SK_LOS[14] - P[4][4]*SK_LOS[15] - P[4][2]*SK_LOS[16] + P[4][0]*SK_LOS[17] + P[4][5]*SK_LOS[18] - P[4][3]*SK_LOS[19]);
Kfusion[5] = -SK_LOS[1]*(P[5][1]*SK_LOS[21] + P[5][8]*SK_LOS[14] - P[5][4]*SK_LOS[15] - P[5][2]*SK_LOS[16] + P[5][0]*SK_LOS[17] + P[5][5]*SK_LOS[18] - P[5][3]*SK_LOS[19]);
Kfusion[6] = -SK_LOS[1]*(P[6][1]*SK_LOS[21] + P[6][8]*SK_LOS[14] - P[6][4]*SK_LOS[15] - P[6][2]*SK_LOS[16] + P[6][0]*SK_LOS[17] + P[6][5]*SK_LOS[18] - P[6][3]*SK_LOS[19]);
Kfusion[7] = -SK_LOS[1]*(P[7][1]*SK_LOS[21] + P[7][8]*SK_LOS[14] - P[7][4]*SK_LOS[15] - P[7][2]*SK_LOS[16] + P[7][0]*SK_LOS[17] + P[7][5]*SK_LOS[18] - P[7][3]*SK_LOS[19]);
Kfusion[8] = -SK_LOS[1]*(P[8][1]*SK_LOS[21] + P[8][8]*SK_LOS[14] - P[8][4]*SK_LOS[15] - P[8][2]*SK_LOS[16] + P[8][0]*SK_LOS[17] + P[8][5]*SK_LOS[18] - P[8][3]*SK_LOS[19]);
Kfusion[9] = -SK_LOS[1]*(P[9][1]*SK_LOS[21] + P[9][8]*SK_LOS[14] - P[9][4]*SK_LOS[15] - P[9][2]*SK_LOS[16] + P[9][0]*SK_LOS[17] + P[9][5]*SK_LOS[18] - P[9][3]*SK_LOS[19]);
Kfusion[10] = -SK_LOS[1]*(P[10][1]*SK_LOS[21] + P[10][8]*SK_LOS[14] - P[10][4]*SK_LOS[15] - P[10][2]*SK_LOS[16] + P[10][0]*SK_LOS[17] + P[10][5]*SK_LOS[18] - P[10][3]*SK_LOS[19]);
Kfusion[11] = -SK_LOS[1]*(P[11][1]*SK_LOS[21] + P[11][8]*SK_LOS[14] - P[11][4]*SK_LOS[15] - P[11][2]*SK_LOS[16] + P[11][0]*SK_LOS[17] + P[11][5]*SK_LOS[18] - P[11][3]*SK_LOS[19]);
Kfusion[12] = -SK_LOS[1]*(P[12][1]*SK_LOS[21] + P[12][8]*SK_LOS[14] - P[12][4]*SK_LOS[15] - P[12][2]*SK_LOS[16] + P[12][0]*SK_LOS[17] + P[12][5]*SK_LOS[18] - P[12][3]*SK_LOS[19]);
Kfusion[13] = -SK_LOS[1]*(P[13][1]*SK_LOS[21] + P[13][8]*SK_LOS[14] - P[13][4]*SK_LOS[15] - P[13][2]*SK_LOS[16] + P[13][0]*SK_LOS[17] + P[13][5]*SK_LOS[18] - P[13][3]*SK_LOS[19]);
Kfusion[14] = -SK_LOS[1]*(P[14][1]*SK_LOS[21] + P[14][8]*SK_LOS[14] - P[14][4]*SK_LOS[15] - P[14][2]*SK_LOS[16] + P[14][0]*SK_LOS[17] + P[14][5]*SK_LOS[18] - P[14][3]*SK_LOS[19]);
Kfusion[15] = -SK_LOS[1]*(P[15][1]*SK_LOS[21] + P[15][8]*SK_LOS[14] - P[15][4]*SK_LOS[15] - P[15][2]*SK_LOS[16] + P[15][0]*SK_LOS[17] + P[15][5]*SK_LOS[18] - P[15][3]*SK_LOS[19]);
Kfusion[16] = -SK_LOS[1]*(P[16][1]*SK_LOS[21] + P[16][8]*SK_LOS[14] - P[16][4]*SK_LOS[15] - P[16][2]*SK_LOS[16] + P[16][0]*SK_LOS[17] + P[16][5]*SK_LOS[18] - P[16][3]*SK_LOS[19]);
Kfusion[17] = -SK_LOS[1]*(P[17][1]*SK_LOS[21] + P[17][8]*SK_LOS[14] - P[17][4]*SK_LOS[15] - P[17][2]*SK_LOS[16] + P[17][0]*SK_LOS[17] + P[17][5]*SK_LOS[18] - P[17][3]*SK_LOS[19]);
Kfusion[18] = -SK_LOS[1]*(P[18][1]*SK_LOS[21] + P[18][8]*SK_LOS[14] - P[18][4]*SK_LOS[15] - P[18][2]*SK_LOS[16] + P[18][0]*SK_LOS[17] + P[18][5]*SK_LOS[18] - P[18][3]*SK_LOS[19]);
Kfusion[19] = -SK_LOS[1]*(P[19][1]*SK_LOS[21] + P[19][8]*SK_LOS[14] - P[19][4]*SK_LOS[15] - P[19][2]*SK_LOS[16] + P[19][0]*SK_LOS[17] + P[19][5]*SK_LOS[18] - P[19][3]*SK_LOS[19]);
Kfusion[20] = -SK_LOS[1]*(P[20][1]*SK_LOS[21] + P[20][8]*SK_LOS[14] - P[20][4]*SK_LOS[15] - P[20][2]*SK_LOS[16] + P[20][0]*SK_LOS[17] + P[20][5]*SK_LOS[18] - P[20][3]*SK_LOS[19]);
Kfusion[21] = -SK_LOS[1]*(P[21][1]*SK_LOS[21] + P[21][8]*SK_LOS[14] - P[21][4]*SK_LOS[15] - P[21][2]*SK_LOS[16] + P[21][0]*SK_LOS[17] + P[21][5]*SK_LOS[18] - P[21][3]*SK_LOS[19]);
Kfusion[22] = -SK_LOS[1]*(P[22][1]*SK_LOS[21] + P[22][8]*SK_LOS[14] - P[22][4]*SK_LOS[15] - P[22][2]*SK_LOS[16] + P[22][0]*SK_LOS[17] + P[22][5]*SK_LOS[18] - P[22][3]*SK_LOS[19]);
Kfusion[23] = -SK_LOS[1]*(P[23][1]*SK_LOS[21] + P[23][8]*SK_LOS[14] - P[23][4]*SK_LOS[15] - P[23][2]*SK_LOS[16] + P[23][0]*SK_LOS[17] + P[23][5]*SK_LOS[18] - P[23][3]*SK_LOS[19]);
// calculate Y axis Kalman gains
t2 = 1.0f/range;
t3 = q0*q2*2.0f;
t4 = q0*q0;
t5 = q1*q1;
t6 = q2*q2;
t7 = q3*q3;
t8 = q0*q1*2.0f;
t9 = q0*q3*2.0f;
t10 = q1*q2*2.0f;
float t11 = t9+t10;
float t12 = q1*q3*2.0f;
float t13 = t4-t5-t6+t7;
float t14 = t13*vd;
float t15 = q2*q3*2.0f;
float t16 = t3+t12;
float t17 = t16*vn;
float t18 = t4-t5+t6-t7;
float t19 = t18*ve;
float t20 = t8+t15;
float t21 = t20*vd;
float t22 = t9-t10;
float t28 = t22*vn;
float t23 = t19+t21-t28;
float t24 = t4+t5-t6-t7;
float t25 = t3-t12;
float t26 = t8-t15;
float t29 = t26*ve;
float t27 = t14+t17-t29;
float t30 = P[4][4]*t2*t11;
float t31 = P[2][4]*t2*t23;
float t32 = P[3][4]*t2*t24;
float t56 = P[5][4]*t2*t25;
float t57 = P[1][4]*t2*t27;
float t33 = t30+t31+t32-t56-t57;
float t34 = t2*t11*t33;
float t35 = P[4][5]*t2*t11;
float t36 = P[2][5]*t2*t23;
float t37 = P[3][5]*t2*t24;
float t58 = P[5][5]*t2*t25;
float t59 = P[1][5]*t2*t27;
float t38 = t35+t36+t37-t58-t59;
float t39 = P[4][1]*t2*t11;
float t40 = P[2][1]*t2*t23;
float t41 = P[3][1]*t2*t24;
float t55 = P[1][1]*t2*t27;
float t61 = P[5][1]*t2*t25;
float t42 = t39+t40+t41-t55-t61;
float t43 = P[4][2]*t2*t11;
float t44 = P[2][2]*t2*t23;
float t45 = P[3][2]*t2*t24;
float t63 = P[5][2]*t2*t25;
float t64 = P[1][2]*t2*t27;
float t46 = t43+t44+t45-t63-t64;
float t47 = t2*t23*t46;
float t48 = P[4][3]*t2*t11;
float t49 = P[2][3]*t2*t23;
float t50 = P[3][3]*t2*t24;
float t65 = P[5][3]*t2*t25;
float t66 = P[1][3]*t2*t27;
float t51 = t48+t49+t50-t65-t66;
float t52 = t2*t24*t51;
float t60 = t2*t25*t38;
float t62 = t2*t27*t42;
float t53 = R_LOS+t34+t47+t52-t60-t62;
float t54 = 1.0f/t53;
Kfusion[0][1] = -t54*(P[0][4]*t2*t11+P[0][2]*t2*t23+P[0][3]*t2*t24-P[0][1]*t2*t27-P[0][5]*t2*t25);
Kfusion[1][1] = -t54*(-t55+P[1][4]*t2*t11+P[1][2]*t2*t23+P[1][3]*t2*t24-P[1][5]*t2*t25);
Kfusion[2][1] = -t54*(t44+P[2][4]*t2*t11+P[2][3]*t2*t24-P[2][1]*t2*t27-P[2][5]*t2*t25);
Kfusion[3][1] = -t54*(t50+P[3][4]*t2*t11+P[3][2]*t2*t23-P[3][1]*t2*t27-P[3][5]*t2*t25);
Kfusion[4][1] = -t54*(t30+P[4][2]*t2*t23+P[4][3]*t2*t24-P[4][1]*t2*t27-P[4][5]*t2*t25);
Kfusion[5][1] = -t54*(-t58+P[5][4]*t2*t11+P[5][2]*t2*t23+P[5][3]*t2*t24-P[5][1]*t2*t27);
Kfusion[6][1] = -t54*(P[6][4]*t2*t11+P[6][2]*t2*t23+P[6][3]*t2*t24-P[6][1]*t2*t27-P[6][5]*t2*t25);
Kfusion[7][1] = -t54*(P[7][4]*t2*t11+P[7][2]*t2*t23+P[7][3]*t2*t24-P[7][1]*t2*t27-P[7][5]*t2*t25);
Kfusion[8][1] = -t54*(P[8][4]*t2*t11+P[8][2]*t2*t23+P[8][3]*t2*t24-P[8][1]*t2*t27-P[8][5]*t2*t25);
Kfusion[9][1] = -t54*(P[9][4]*t2*t11+P[9][2]*t2*t23+P[9][3]*t2*t24-P[9][1]*t2*t27-P[9][5]*t2*t25);
Kfusion[10][1] = -t54*(P[10][4]*t2*t11+P[10][2]*t2*t23+P[10][3]*t2*t24-P[10][1]*t2*t27-P[10][5]*t2*t25);
Kfusion[11][1] = -t54*(P[11][4]*t2*t11+P[11][2]*t2*t23+P[11][3]*t2*t24-P[11][1]*t2*t27-P[11][5]*t2*t25);
Kfusion[12][1] = -t54*(P[12][4]*t2*t11+P[12][2]*t2*t23+P[12][3]*t2*t24-P[12][1]*t2*t27-P[12][5]*t2*t25);
Kfusion[13][1] = -t54*(P[13][4]*t2*t11+P[13][2]*t2*t23+P[13][3]*t2*t24-P[13][1]*t2*t27-P[13][5]*t2*t25);
Kfusion[14][1] = -t54*(P[14][4]*t2*t11+P[14][2]*t2*t23+P[14][3]*t2*t24-P[14][1]*t2*t27-P[14][5]*t2*t25);
Kfusion[15][1] = -t54*(P[15][4]*t2*t11+P[15][2]*t2*t23+P[15][3]*t2*t24-P[15][1]*t2*t27-P[15][5]*t2*t25);
Kfusion[16][1] = -t54*(P[16][4]*t2*t11+P[16][2]*t2*t23+P[16][3]*t2*t24-P[16][1]*t2*t27-P[16][5]*t2*t25);
Kfusion[17][1] = -t54*(P[17][4]*t2*t11+P[17][2]*t2*t23+P[17][3]*t2*t24-P[17][1]*t2*t27-P[17][5]*t2*t25);
Kfusion[18][1] = -t54*(P[18][4]*t2*t11+P[18][2]*t2*t23+P[18][3]*t2*t24-P[18][1]*t2*t27-P[18][5]*t2*t25);
Kfusion[19][1] = -t54*(P[19][4]*t2*t11+P[19][2]*t2*t23+P[19][3]*t2*t24-P[19][1]*t2*t27-P[19][5]*t2*t25);
Kfusion[20][1] = -t54*(P[20][4]*t2*t11+P[20][2]*t2*t23+P[20][3]*t2*t24-P[20][1]*t2*t27-P[20][5]*t2*t25);
Kfusion[21][1] = -t54*(P[21][4]*t2*t11+P[21][2]*t2*t23+P[21][3]*t2*t24-P[21][1]*t2*t27-P[21][5]*t2*t25);
Kfusion[22][1] = -t54*(P[22][4]*t2*t11+P[22][2]*t2*t23+P[22][3]*t2*t24-P[22][1]*t2*t27-P[22][5]*t2*t25);
Kfusion[23][1] = -t54*(P[23][4]*t2*t11+P[23][2]*t2*t23+P[23][3]*t2*t24-P[23][1]*t2*t27-P[23][5]*t2*t25);
matlab/scripts/Inertial Nav EKF/ConvertToC.m
+1 -1
View File
@@ -15,7 +15,7 @@ fileID = fopen(fileName,'r');
% This call is based on the structure of the file used to generate this
% code. If an error occurs for a different file, try regenerating the code
% from the Import Tool.
dataArray = textscan(fileID, formatSpec, 'Delimiter', delimiter, 'ReturnOnError', false, 'Bufsize', 65535);
dataArray = textscan(fileID, formatSpec, 'Delimiter', delimiter, 'ReturnOnError', false);
%% Close the text file.
fclose(fileID);
matlab/scripts/Inertial Nav EKF/ConvertToM.m
+1 -1
View File
@@ -15,7 +15,7 @@ fileID = fopen(fileName,'r');
% This call is based on the structure of the file used to generate this
% code. If an error occurs for a different file, try regenerating the code
% from the Import Tool.
dataArray = textscan(fileID, formatSpec, 'Delimiter', delimiter, 'ReturnOnError', false,'Bufsize',65535);
dataArray = textscan(fileID, formatSpec, 'Delimiter', delimiter, 'ReturnOnError', false);
%% Close the text file.
fclose(fileID);
matlab/scripts/Inertial Nav EKF/GenerateNavFilterEquations.m
+162 -98
View File
@@ -1,13 +1,13 @@
% IMPORTANT - This script requires the Matlab symbolic toolbox and takes ~3 hours to run
% Derivation of Navigation EKF using a local NED earth Tangent Frame and
% Derivation of Navigation EKF using a local NED earth Tangent Frame and
% XYZ body fixed frame
% Sequential fusion of velocity and position measurements
% Fusion of true airspeed
% Sequential fusion of magnetic flux measurements
% 24 state architecture.
% IMU data is assumed to arrive at a constant rate with a time step of dt
% IMU delta angle and velocity data are used as time varying parameters,
% IMU delta angle and velocity data are used as control inputs,
% not observations
% Author: Paul Riseborough
@@ -15,8 +15,8 @@
% Based on use of a rotation vector for attitude estimation as described
% here:
% Mark E. Pittelkau. "Rotation Vector in Attitude Estimation",
% Journal of Guidance, Control, and Dynamics, Vol. 26, No. 6 (2003),
% Mark E. Pittelkau. "Rotation Vector in Attitude Estimation",
% Journal of Guidance, Control, and Dynamics, Vol. 26, No. 6 (2003),
% pp. 855-860.
% State vector:
@@ -52,7 +52,7 @@ syms vn ve vd real % NED velocity - m/sec
syms pn pe pd real % NED position - m
syms dax_b day_b daz_b real % delta angle bias - rad
syms dax_s day_s daz_s real % delta angle scale factor
syms dvz_b real % delta velocity bias - m/sec
syms dvz_b dvy_b dvz_b real % delta velocity bias - m/sec
syms dt real % IMU time step - sec
syms gravity real % gravity - m/sec^2
syms daxNoise dayNoise dazNoise dvxNoise dvyNoise dvzNoise real; % IMU delta angle and delta velocity measurement noise
@@ -72,9 +72,9 @@ syms R_DECL R_YAW real; % variance of declination or yaw angle observation
syms BCXinv BCYinv real % inverse of ballistic coefficient for wind relative movement along the x and y body axes
syms rho real % air density (kg/m^3)
syms R_ACC real % variance of accelerometer measurements (m/s^2)^2
syms Kacc real % ratio of horizontal acceleration to top speed for a multirotor
syms Kaccx Kaccy real % derivative of X and Y body specific forces wrt componenent of true airspeed along each axis (1/s)
%% define the process equations
%% define the state prediction equations
% define the measured Delta angle and delta velocity vectors
dAngMeas = [dax; day; daz];
@@ -101,7 +101,7 @@ truthQuat = QuatMult(estQuat, errQuat);
Tbn = Quat2Tbn(truthQuat);
% define the truth delta angle
% ignore coning compensation as these effects are negligible in terms of
% ignore coning compensation as these effects are negligible in terms of
% covariance growth for our application and grade of sensor
dAngTruth = dAngMeas.*dAngScale - dAngBias - [daxNoise;dayNoise;dazNoise];
@@ -157,7 +157,27 @@ nStates=numel(stateVector);
% Define vector of process equations
newStateVector = [errRotNew;vNew;pNew;dabNew;dasNew;dvbNew;magNnew;magEnew;magDnew;magXnew;magYnew;magZnew;vwnNew;vweNew];
%% derive the covariance prediction equation
% derive the state transition matrix
F = jacobian(newStateVector, stateVector);
% set the rotation error states to zero
F = subs(F, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
[F,SF]=OptimiseAlgebra(F,'SF');
% define a symbolic covariance matrix using strings to represent
% '_l_' to represent '( '
% '_c_' to represent ,
% '_r_' to represent ')'
% these can be substituted later to create executable code
for rowIndex = 1:nStates
for colIndex = 1:nStates
eval(['syms OP_l_',num2str(rowIndex),'_c_',num2str(colIndex), '_r_ real']);
eval(['P(',num2str(rowIndex),',',num2str(colIndex), ') = OP_l_',num2str(rowIndex),'_c_',num2str(colIndex),'_r_;']);
end
end
save 'StatePrediction.mat';
%% derive the covariance prediction equations
% This reduces the number of floating point operations by a factor of 6 or
% more compared to using the standard matrix operations in code
@@ -179,26 +199,8 @@ Q = G*distMatrix*transpose(G);
% remove the disturbance noise from the process equations as it is only
% needed when calculating the disturbance influence matrix
vNew = subs(vNew,{'daxNoise','dayNoise','dazNoise','dvxNoise','dvyNoise','dvzNoise'}, {0,0,0,0,0,0},0);
errRotNew = subs(errRotNew,{'daxNoise','dayNoise','dazNoise','dvxNoise','dvyNoise','dvzNoise'}, {0,0,0,0,0,0},0);
% derive the state transition matrix
F = jacobian(newStateVector, stateVector);
% set the rotation error states to zero
F = subs(F, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
[F,SF]=OptimiseAlgebra(F,'SF');
% define a symbolic covariance matrix using strings to represent
% '_l_' to represent '( '
% '_c_' to represent ,
% '_r_' to represent ')'
% these can be substituted later to create executable code
for rowIndex = 1:nStates
for colIndex = 1:nStates
eval(['syms OP_l_',num2str(rowIndex),'_c_',num2str(colIndex), '_r_ real']);
eval(['P(',num2str(rowIndex),',',num2str(colIndex), ') = OP_l_',num2str(rowIndex),'_c_',num2str(colIndex),'_r_;']);
end
end
vNew = subs(vNew,{'daxNoise','dayNoise','dazNoise','dvxNoise','dvyNoise','dvzNoise'}, {0,0,0,0,0,0});
errRotNew = subs(errRotNew,{'daxNoise','dayNoise','dazNoise','dvxNoise','dvyNoise','dvzNoise'}, {0,0,0,0,0,0});
% Derive the predicted covariance matrix using the standard equation
PP = F*P*transpose(F) + Q;
@@ -206,14 +208,27 @@ PP = F*P*transpose(F) + Q;
% Collect common expressions to optimise processing
[PP,SPP]=OptimiseAlgebra(PP,'SPP');
save('StateAndCovariancePrediction.mat');
clear all;
reset(symengine);
%% derive equations for fusion of true airspeed measurements
load('StatePrediction.mat');
VtasPred = sqrt((vn-vwn)^2 + (ve-vwe)^2 + vd^2); % predicted measurement
H_TAS = jacobian(VtasPred,stateVector); % measurement Jacobian
H_TAS = subs(H_TAS, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
[H_TAS,SH_TAS]=OptimiseAlgebra(H_TAS,'SH_TAS'); % optimise processing
K_TAS = (P*transpose(H_TAS))/(H_TAS*P*transpose(H_TAS) + R_TAS);[K_TAS,SK_TAS]=OptimiseAlgebra(K_TAS,'SK_TAS'); % Kalman gain vector
K_TAS = (P*transpose(H_TAS))/(H_TAS*P*transpose(H_TAS) + R_TAS);
[K_TAS,SK_TAS]=OptimiseAlgebra(K_TAS,'SK_TAS'); % Kalman gain vector
% save equations and reset workspace
save('Airspeed.mat','SH_TAS','H_TAS','SK_TAS','K_TAS');
clear all;
reset(symengine);
%% derive equations for fusion of angle of sideslip measurements
load('StatePrediction.mat');
% calculate wind relative velocities in nav frame and rotate into body frame
Vbw = Tbn'*[(vn-vwn);(ve-vwe);vd];
% calculate predicted angle of sideslip using small angle assumption
@@ -223,7 +238,14 @@ H_BETA = subs(H_BETA, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
[H_BETA,SH_BETA]=OptimiseAlgebra(H_BETA,'SH_BETA'); % optimise processing
K_BETA = (P*transpose(H_BETA))/(H_BETA*P*transpose(H_BETA) + R_BETA);[K_BETA,SK_BETA]=OptimiseAlgebra(K_BETA,'SK_BETA'); % Kalman gain vector
% save equations and reset workspace
save('Sideslip.mat','SH_BETA','H_BETA','SK_BETA','K_BETA');
clear all;
reset(symengine);
%% derive equations for fusion of magnetic field measurement
load('StatePrediction.mat');
magMeas = transpose(Tbn)*[magN;magE;magD] + [magX;magY;magZ]; % predicted measurement
H_MAG = jacobian(magMeas,stateVector); % measurement Jacobian
H_MAG = subs(H_MAG, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
@@ -236,99 +258,128 @@ K_MY = (P*transpose(H_MAG(2,:)))/(H_MAG(2,:)*P*transpose(H_MAG(2,:)) + R_MAG); %
K_MZ = (P*transpose(H_MAG(3,:)))/(H_MAG(3,:)*P*transpose(H_MAG(3,:)) + R_MAG); % Kalman gain vector
[K_MZ,SK_MZ]=OptimiseAlgebra(K_MZ,'SK_MZ');
%% derive equations for sequential fusion of optical flow measurements
% save equations and reset workspace
save('Magnetometer.mat','SH_MAG','H_MAG','SK_MX','K_MX','SK_MY','K_MY','SK_MZ','K_MZ');
clear all;
reset(symengine);
%% derive equations for sequential fusion of optical flow measurements
load('StatePrediction.mat');
% range is defined as distance from camera focal point to centre of sensor fov
syms range real;
% calculate range from plane to centre of sensor fov assuming flat earth
% and camera axes aligned with body axes
range = ((ptd - pd)/Tbn(3,3));
% calculate relative velocity in body frame
relVelBody = transpose(Tbn)*[vn;ve;vd];
% divide by range to get predicted angular LOS rates relative to X and Y
% axes. Note these are body angular rate motion compensated optical flow rates
losRateX = +relVelBody(2)/range;
losRateY = -relVelBody(1)/range;
H_LOS = jacobian([losRateX;losRateY],stateVector); % measurement Jacobian
H_LOS = subs(H_LOS, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
H_LOS = simplify(H_LOS);
[H_LOS,SH_LOS] = OptimiseAlgebra(H_LOS,'SH_LOS');
save('temp1.mat','losRateX','losRateY');
% combine into a single K matrix to enable common expressions to be found
% note this matrix cannot be used in a single step fusion
K_LOSX = (P*transpose(H_LOS(1,:)))/(H_LOS(1,:)*P*transpose(H_LOS(1,:)) + R_LOS); % Kalman gain vector
% calculate the observation Jacobian for the X axis
H_LOSX = jacobian(losRateX,stateVector); % measurement Jacobian
H_LOSX = subs(H_LOSX, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
H_LOSX = simplify(H_LOSX);
save('temp2.mat','H_LOSX');
ccode(H_LOSX,'file','H_LOSX.c');
fix_c_code('H_LOSX.c');
clear all;
reset(symengine);
load('StatePrediction.mat');
load('temp1.mat');
% calculate the observation Jacobian for the Y axis
H_LOSY = jacobian(losRateY,stateVector); % measurement Jacobian
H_LOSY = subs(H_LOSY, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
H_LOSY = simplify(H_LOSY);
save('temp3.mat','H_LOSY');
ccode(H_LOSY,'file','H_LOSY.c');
fix_c_code('H_LOSY.c');
clear all;
reset(symengine);
load('StatePrediction.mat');
load('temp1.mat');
load('temp2.mat');
% calculate Kalman gain vector for the X axis
K_LOSX = (P*transpose(H_LOSX))/(H_LOSX*P*transpose(H_LOSX) + R_LOS); % Kalman gain vector
K_LOSX = subs(K_LOSX, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
K_LOSY = (P*transpose(H_LOS(2,:)))/(H_LOS(2,:)*P*transpose(H_LOS(2,:)) + R_LOS); % Kalman gain vector
K_LOSX = simplify(K_LOSX);
ccode(K_LOSX,'file','K_LOSX.c');
fix_c_code('K_LOSX.c');
clear all;
reset(symengine);
load('StatePrediction.mat');
load('temp1.mat');
load('temp3.mat');
% calculate Kalman gain vector for the Y axis
K_LOSY = (P*transpose(H_LOSY))/(H_LOSY*P*transpose(H_LOSY) + R_LOS); % Kalman gain vector
K_LOSY = subs(K_LOSY, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
K_LOS = [K_LOSX,K_LOSY];
simplify(K_LOS);
[K_LOS,SK_LOS]=OptimiseAlgebra(K_LOS,'SK_LOS');
K_LOSY = simplify(K_LOSY);
ccode(K_LOSY,'file','K_LOSY.c');
fix_c_code('K_LOSY.c');
% Use matlab c code converter for an alternate method of
ccode(H_LOS,'file','H_LOS.txt');
ccode(K_LOSX,'file','K_LOSX.txt');
ccode(K_LOSY,'file','K_LOSY.txt');
%% derive equations for simple fusion of 2-D magnetic heading measurements
% rotate magnetic field into earth axes
magMeasNED = Tbn*[magX;magY;magZ];
% the predicted measurement is the angle wrt true north of the horizontal
% component of the measured field
angMeas = atan(magMeasNED(2)/magMeasNED(1));
simpleStateVector = [errRotVec;vn;ve;vd;pn;pe;pd;dax_b;day_b;daz_b;dax_s;day_s;daz_s;dvz_b];
Psimple = P(1:16,1:16);
H_MAGS = jacobian(angMeas,simpleStateVector); % measurement Jacobian
%H_MAGS = H_MAGS(1:3);
H_MAGS = subs(H_MAGS, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
%H_MAGS = simplify(H_MAGS);
%[H_MAGS,SH_MAGS]=OptimiseAlgebra(H_MAGS,'SH_MAGS');
ccode(H_MAGS,'file','calcH_MAGS.c');
% Calculate Kalman gain vector
K_MAGS = (Psimple*transpose(H_MAGS))/(H_MAGS*Psimple*transpose(H_MAGS) + R_DECL);
%K_MAGS = simplify(K_MAGS);
%[K_MAGS,SK_MAGS]=OptimiseAlgebra(K_MAGS,'SK_MAGS');
ccode(K_MAGS,'file','calcK_MAGS.c');
% reset workspace
clear all;
reset(symengine);
%% derive equations for fusion of 321 sequence yaw measurement
load('StatePrediction.mat');
% Calculate the yaw (first rotation) angle from the 321 rotation sequence
angMeas = atan(Tbn(2,1)/Tbn(1,1));
H_YAW = jacobian(angMeas,stateVector); % measurement Jacobian
H_YAW = subs(H_YAW, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
ccode(H_YAW,'file','calcH_YAW321.c');
% Calculate Kalman gain vector
K_YAW = (P*transpose(H_YAW))/(H_YAW*P*transpose(H_YAW) + R_YAW);
ccode([K_YAW;H_YAW'],'file','calcYAW321.c');
H_YAW321 = jacobian(angMeas,stateVector); % measurement Jacobian
H_YAW321 = subs(H_YAW321, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
H_YAW321 = simplify(H_YAW321);
ccode(H_YAW321,'file','calcH_YAW321.c');
fix_c_code('calcH_YAW321.c');
% reset workspace
clear all;
reset(symengine);
%% derive equations for fusion of 312 sequence yaw measurement
load('StatePrediction.mat');
% Calculate the yaw (first rotation) angle from an Euler 312 sequence
angMeas = atan(-Tbn(1,2)/Tbn(2,2));
H_YAW2 = jacobian(angMeas,stateVector); % measurement Jacobianclea
H_YAW2 = subs(H_YAW2, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
ccode(H_YAW2,'file','calcH_YAW312.c');
% Calculate Kalman gain vector
K_YAW2 = (P*transpose(H_YAW2))/(H_YAW2*P*transpose(H_YAW2) + R_YAW);
ccode([K_YAW2;H_YAW2'],'file','calcYAW312.c');
H_YAW312 = jacobian(angMeas,stateVector); % measurement Jacobianclea
H_YAW312 = subs(H_YAW312, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
H_YAW312 = simplify(H_YAW312);
ccode(H_YAW312,'file','calcH_YAW312.c');
fix_c_code('calcH_YAW312.c');
% reset workspace
clear all;
reset(symengine);
%% derive equations for fusion of declination
load('StatePrediction.mat');
%% derive equations for fusion of synthetic deviation measurement
% used to keep correct heading when operating without absolute position or
% velocity measurements - eg when using optical flow
% rotate magnetic field into earth axes
magMeasNED = [magN;magE;magD];
% the predicted measurement is the angle wrt magnetic north of the horizontal
% component of the measured field
angMeas = atan(magMeasNED(2)/magMeasNED(1));
angMeas = atan(magE/magN);
H_MAGD = jacobian(angMeas,stateVector); % measurement Jacobian
H_MAGD = subs(H_MAGD, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
H_MAGD = simplify(H_MAGD);
%[H_MAGD,SH_MAGD]=OptimiseAlgebra(H_MAGD,'SH_MAGD');
ccode(H_MAGD,'file','calcH_MAGD.c');
% Calculate Kalman gain vector
K_MAGD = (P*transpose(H_MAGD))/(H_MAGD*P*transpose(H_MAGD) + R_DECL);
ccode([H_MAGD',K_MAGD],'file','calcMAGD.c');
K_MAGD = simplify(K_MAGD);
ccode([K_MAGD,H_MAGD'],'file','calcMAGD.c');
fix_c_code('calcMAGD.c');
% reset workspace
clear all;
reset(symengine);
%% derive equations for fusion of lateral body acceleration (multirotors only)
load('StatePrediction.mat');
% use relationship between airspeed along the X and Y body axis and the
% drag to predict the lateral acceleration for a multirotor vehicle type
@@ -344,32 +395,45 @@ vrel = transpose(Tbn)*[(vn-vwn);(ve-vwe);vd]; % predicted wind relative velocity
% accYpred = -0.5*rho*vrel(2)*vrel(2)*BCYinv; % predicted acceleration measured along Y body axis
% Use a simple viscous drag model for the linear estimator equations
% Use the the derivative from speed to acceleration averaged across the
% Use the the derivative from speed to acceleration averaged across the
% speed range
% The nonlinear equation will be used to calculate the predicted
% measurement in implementation
accXpred = -Kacc*vrel(1); % predicted acceleration measured along X body axis
accYpred = -Kacc*vrel(2); % predicted acceleration measured along Y body axis
accXpred = -Kaccx*vrel(1); % predicted acceleration measured along X body axis
accYpred = -Kaccy*vrel(2); % predicted acceleration measured along Y body axis
% Derive observation Jacobian and Kalman gain matrix for X accel fusion
H_ACCX = jacobian(accXpred,stateVector); % measurement Jacobian
H_ACCX = subs(H_ACCX, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
H_ACCX = simplify(H_ACCX);
[H_ACCX,SH_ACCX]=OptimiseAlgebra(H_ACCX,'SH_ACCX'); % optimise processing
K_ACCX = (P*transpose(H_ACCX))/(H_ACCX*P*transpose(H_ACCX) + R_ACC);
ccode([H_ACCX',K_ACCX],'file','calcACCX.c');
[K_ACCX,SK_ACCX]=OptimiseAlgebra(K_ACCX,'SK_ACCX'); % Kalman gain vector
% Derive observation Jacobian and Kalman gain matrix for Y accel fusion
H_ACCY = jacobian(accYpred,stateVector); % measurement Jacobian
H_ACCY = subs(H_ACCY, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
H_ACCY = simplify(H_ACCY);
[H_ACCY,SH_ACCY]=OptimiseAlgebra(H_ACCY,'SH_ACCY'); % optimise processing
K_ACCY = (P*transpose(H_ACCY))/(H_ACCY*P*transpose(H_ACCY) + R_ACC);
ccode([H_ACCY',K_ACCY],'file','calcACCY.c');
[K_ACCY,SK_ACCY]=OptimiseAlgebra(K_ACCY,'SK_ACCY'); % Kalman gain vector
% save equations and reset workspace
save('Drag.mat','SH_ACCX','H_ACCX','SK_ACCX','K_ACCX','SH_ACCY','H_ACCY','SK_ACCY','K_ACCY');
clear all;
reset(symengine);
%% Save output and convert to m and c code fragments
% load equations for predictions and updates
load('StateAndCovariancePrediction.mat');
load('Airspeed.mat');
load('Sideslip.mat');
load('Magnetometer.mat');
load('Drag.mat');
fileName = strcat('SymbolicOutput',int2str(nStates),'.mat');
save(fileName);
SaveScriptCode(nStates);
ConvertToM(nStates);
ConvertToC(nStates);
ConvertToC(nStates);
matlab/scripts/Inertial Nav EKF/SaveScriptCode.m
+21 -2
View File
@@ -409,7 +409,7 @@ if exist('SH_LOS','var')
fprintf(fid,'\n');
fprintf(fid,'Kfusion = zeros(%d,1);\n',nRow,nCol);
for rowIndex = 1:nRow
string = char(K_LOS(rowIndex,1));
string = char(K_LOSX(rowIndex));
% don't write out a zero-assignment
if ~strcmpi(string,'0')
fprintf(fid,'Kfusion(%d) = %s;\n',rowIndex,string);
@@ -421,7 +421,7 @@ if exist('SH_LOS','var')
fprintf(fid,'\n');
fprintf(fid,'Kfusion = zeros(%d,1);\n',nRow,nCol);
for rowIndex = 1:nRow
string = char(K_LOS(rowIndex,2));
string = char(K_LOSY(rowIndex));
% don't write out a zero-assignment
if ~strcmpi(string,'0')
fprintf(fid,'Kfusion(%d) = %s;\n',rowIndex,string);
@@ -547,6 +547,25 @@ if exist('SH_MAGS','var')
end
fprintf(fid,'\n');
fprintf(fid,'\n');
fprintf(fid,'SK_MAGS = zeros(%d,1);\n',numel(SK_MAGS));
for rowIndex = 1:numel(SK_MAGS)
string = char(SK_MAGS(rowIndex,1));
fprintf(fid,'SK_MAGS(%d) = %s;\n',rowIndex,string);
end
fprintf(fid,'\n');
[nRow,nCol] = size(K_MAGS);
fprintf(fid,'\n');
fprintf(fid,'Kfusion = zeros(%d,1);\n',nRow,nCol);
for rowIndex = 1:nRow
string = char(K_MAGS(rowIndex,1));
% don't write out a zero-assignment
if ~strcmpi(string,'0')
fprintf(fid,'Kfusion(%d) = %s;\n',rowIndex,string);
end
end
fprintf(fid,'\n');
end
%% Write equations for X accel fusion
matlab/scripts/Inertial Nav EKF/fix_c_code.m
+92
View File
@@ -0,0 +1,92 @@
function fix_c_code(fileName)
%% Initialize variables
delimiter = '';
%% Format string for each line of text:
% column1: text (%s)
% For more information, see the TEXTSCAN documentation.
formatSpec = '%s%[^\n\r]';
%% Open the text file.
fileID = fopen(fileName,'r');
%% Read columns of data according to format string.
% This call is based on the structure of the file used to generate this
% code. If an error occurs for a different file, try regenerating the code
% from the Import Tool.
dataArray = textscan(fileID, formatSpec, 'Delimiter', delimiter, 'ReturnOnError', false);
%% Close the text file.
fclose(fileID);
%% Create output variable
SymbolicOutput = [dataArray{1:end-1}];
%% Clear temporary variables
clearvars filename delimiter formatSpec fileID dataArray ans;
%% replace brackets and commas
for lineIndex = 1:length(SymbolicOutput)
SymbolicOutput(lineIndex) = regexprep(SymbolicOutput(lineIndex), '_l_', '(');
SymbolicOutput(lineIndex) = regexprep(SymbolicOutput(lineIndex), '_c_', ',');
SymbolicOutput(lineIndex) = regexprep(SymbolicOutput(lineIndex), '_r_', ')');
end
%% Convert indexing and replace brackets
% replace 1-D indexes
for arrayIndex = 1:99
strIndex = int2str(arrayIndex);
strRep = sprintf('[%d]',(arrayIndex-1));
strPat = strcat('\(',strIndex,'\)');
for lineIndex = 1:length(SymbolicOutput)
str = char(SymbolicOutput(lineIndex));
SymbolicOutput(lineIndex) = {regexprep(str, strPat, strRep)};
end
end
% replace 2-D left indexes
for arrayIndex = 1:99
strIndex = int2str(arrayIndex);
strRep = sprintf('[%d,',(arrayIndex-1));
strPat = strcat('\(',strIndex,'\,');
for lineIndex = 1:length(SymbolicOutput)
str = char(SymbolicOutput(lineIndex));
SymbolicOutput(lineIndex) = {regexprep(str, strPat, strRep)};
end
end
% replace 2-D right indexes
for arrayIndex = 1:99
strIndex = int2str(arrayIndex);
strRep = sprintf(',%d]',(arrayIndex-1));
strPat = strcat('\,',strIndex,'\)');
for lineIndex = 1:length(SymbolicOutput)
str = char(SymbolicOutput(lineIndex));
SymbolicOutput(lineIndex) = {regexprep(str, strPat, strRep)};
end
end
% replace commas
for lineIndex = 1:length(SymbolicOutput)
str = char(SymbolicOutput(lineIndex));
SymbolicOutput(lineIndex) = {regexprep(str, '\,', '][')};
end
%% Change covariance matrix variable name to P
for lineIndex = 1:length(SymbolicOutput)
strIn = char(SymbolicOutput(lineIndex));
strIn = regexprep(strIn,'OP\[','P[');
SymbolicOutput(lineIndex) = cellstr(strIn);
end
%% Write to file
fid = fopen(fileName,'wt');
for lineIndex = 1:length(SymbolicOutput)
fprintf(fid,char(SymbolicOutput(lineIndex)));
fprintf(fid,'\n');
end
fclose(fid);
clear all;
end