fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-03 06:51:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Add multi-rotor drag model for wind estimation

Browse Source
This commit is contained in:
Paul Riseborough
2017-04-16 13:07:12 +02:00
committed by Lorenz Meier
parent b07acd8cf3
commit 465b145929
11 changed files with 377 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/CMakeLists.txt
+1
View File
@@ -71,6 +71,7 @@ set(SRCS
sideslip_fusion.cpp
terrain_estimator.cpp
vel_pos_fusion.cpp
drag_fusion.cpp
)
add_definitions(-DPOSIX_SHARED)
EKF/common.h
+3 -2
View File
@@ -157,6 +157,7 @@ struct extVisionSample {
#define MASK_INHIBIT_ACC_BIAS (1<<2) // set to true to inhibit estimation of accelerometer delta velocity bias
#define MASK_USE_EVPOS (1<<3) // set to true to use external vision NED position data
#define MASK_USE_EVYAW (1<<4) // set to true to use exernal vision quaternion data for yaw
#define MASK_USE_DRAG (1<<5) // set to true to use the multi-rotor drag model to estimate wind
// Integer definitions for mag_fusion_type
#define MAG_FUSE_TYPE_AUTO 0 // The selection of either heading or 3D magnetometer fusion will be automatic
@@ -176,7 +177,7 @@ struct extVisionSample {
struct parameters {
// measurement source control
int fusion_mode; // bitmasked integer that selects which of the GPS and optical flow aiding sources will be used
int fusion_mode; // bitmasked integer that selects which aiding sources will be used
int vdist_sensor_type; // selects the primary source for height data
int sensor_interval_min_ms; // minimum time of arrival difference between non IMU sensor updates. Sets the size of the observation buffers.
@@ -447,7 +448,7 @@ union innovation_fault_status_u {
bool reject_sideslip: 1; // 8 - true if the synthetic sideslip observation has been rejected
bool reject_hagl: 1; // 9 - true if the height above ground observation has been rejected
bool reject_optflow_X: 1; // 10 - true if the X optical flow observation has been rejected
bool reject_optflow_Y: 1; // 11 - true if the Y optical flow observation has been rejected
bool reject_optflow_Y: 1; // 11 - true if the Y optical flow observation has been rejected
} flags;
uint16_t value;
EKF/control.cpp
+19 -2
View File
@@ -106,6 +106,7 @@ void Ekf::controlFusionModes()
controlRangeFinderFusion();
controlAirDataFusion();
controlBetaFusion();
controlDragFusion();
// for efficiency, fusion of direct state observations for position and velocity is performed sequentially
// in a single function using sensor data from multiple sources (GPS, external vision, baro, range finder, etc)
@@ -793,7 +794,7 @@ void Ekf::controlAirDataFusion()
// If both airspeed and sideslip fusion have timed out then we no longer have valid wind estimates
bool airspeed_timed_out = _time_last_imu - _time_last_arsp_fuse > 10e6;
bool sideslip_timed_out = _time_last_imu - _time_last_beta_fuse > 10e6;
if (_control_status.flags.wind && airspeed_timed_out && sideslip_timed_out) {
if (_control_status.flags.wind && airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & MASK_USE_DRAG)) {
// if the airspeed or sideslip measurements have timed out for 10 seconds we declare the wind estimate to be invalid
_control_status.flags.wind = false;
@@ -827,7 +828,7 @@ void Ekf::controlBetaFusion()
// If both airspeed and sideslip fusion have timed out then we no longer have valid wind estimates
bool sideslip_timed_out = _time_last_imu - _time_last_beta_fuse > 10e6;
bool airspeed_timed_out = _time_last_imu - _time_last_arsp_fuse > 10e6;
if(_control_status.flags.wind && airspeed_timed_out && sideslip_timed_out){
if(_control_status.flags.wind && airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & MASK_USE_DRAG)){
_control_status.flags.wind = false;
}
@@ -860,6 +861,22 @@ void Ekf::controlBetaFusion()
}
void Ekf::controlDragFusion()
{
if (_params.fusion_mode & MASK_USE_DRAG && _control_status.flags.in_air) {
if (!_control_status.flags.wind) {
_control_status.flags.wind = true;
resetWindStates();
resetWindCovariance();
} else {
fuseDrag();
}
} else {
_control_status.flags.wind = false;
}
}
void Ekf::controlMagFusion()
{
// If we are using external vision data for heading then no magnetometer fusion is used
EKF/covariance.cpp
+2 -2
View File
@@ -864,8 +864,8 @@ void Ekf::resetWindCovariance()
} else {
// without airspeed, start with a small initial uncertainty to improve the initial estimate
P[22][22] = sq(5.0f);
P[23][23] = sq(5.0f);
P[22][22] = sq(1.0f);
P[23][23] = sq(1.0f);
}
EKF/drag_fusion.cpp
+315
View File
@@ -0,0 +1,315 @@
/****************************************************************************
*
* 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 drag_fusion.cpp
* body frame drag fusion methods used for multi-rotor wind estimation.
*
* @author Paul Riseborough <p_riseborough@live.com.au>
*
*/
#include "../ecl.h"
#include "ekf.h"
#include "mathlib.h"
void Ekf::fuseDrag()
{
float SH_ACC[4] = {}; // Variable used to optimise calculations of measurement jacobian
float H_ACC[24] = {}; // Observation Jacobian
float SK_ACC[9] = {}; // Variable used to optimise calculations of the Kalman gain vector
float Kfusion[24] = {}; // Kalman gain vector
float R_ACC = 2.5f; // observation noise in specific force drag (m/sec**2) TODO add tuning parameter
float eas2tas = 1.0f; // conversion factor from EAS to TAS - TODO calculate from barometric pressure
float rho = 1.225f / sqrtf(eas2tas); // air density (kg/m**3)
// measured steady state EAS at specified tilt angle in X and Y directions
// TODO make the speeds tuning parameters
const float max_spd_x = 15.0f;
const float max_spd_y = 15.0f;
const float tilt_ref_angle = math::radians(25.0f);
// calculate inverse of ballistic coefficient from max speed and tilt angle
// used to predict the acceleration for a given predicted state vector
float numerator = 2.0f*_gravity_mss*tanf(tilt_ref_angle);
float BC_inv_x = numerator / (rho * sq(max_spd_x));
float BC_inv_y = numerator / (rho * sq(max_spd_y));
// 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);
// get latest wind velocity in earth frame
float vwn = _state.wind_vel(0);
float vwe = _state.wind_vel(1);
// calculate measured XY specific forces if enough data
if (_imu_sample_delayed.delta_vel_dt < 1e-3f) {
return;
}
// predicted specific forces
// calculate relative wind velocity in earth frame and rotte into body frame
Vector3f rel_wind;
rel_wind(0) = vn - vwn;
rel_wind(1) = ve - vwe;
rel_wind(2) = vd;
matrix::Dcm<float> earth_to_body(_state.quat_nominal);
earth_to_body = earth_to_body.transpose();
rel_wind = earth_to_body * rel_wind;
// perform sequential fusion of XY specific forces
for (uint8_t axis_index = 0; axis_index <2; axis_index++) {
// calculate observation jacobiam and Kalman gain vectors
if (axis_index == 0) {
// Estimate the airspeed from the measured drag force and ballistic coefficient
float mea_acc = (_imu_sample_delayed.delta_vel(axis_index) - _state.accel_bias(axis_index))/_imu_sample_delayed.delta_vel_dt;
float airSpd = sqrtf((2.0f * fabsf(mea_acc)) / (BC_inv_x * rho));
// Estimate the derivative of specific force wrt airspeed along the X axis
// Limit lower value to prevent artithmetic exceptions
float Kacc = fmax(1e-1f,rho * BC_inv_x * airSpd);
SH_ACC[0] = sq(q0) + sq(q1) - sq(q2) - sq(q3);
SH_ACC[1] = vn - vwn;
SH_ACC[2] = ve - vwe;
SH_ACC[3] = 2.0f*q0*q3 + 2.0f*q1*q2;
H_ACC[0] = -Kacc*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd);
H_ACC[1] = -Kacc*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd);
H_ACC[2] = Kacc*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd);
H_ACC[3] = -Kacc*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd);
H_ACC[4] = -Kacc*SH_ACC[0];
H_ACC[5] = -Kacc*SH_ACC[3];
H_ACC[6] = Kacc*(2.0f*q0*q2 - 2.0f*q1*q3);
H_ACC[22] = Kacc*SH_ACC[0];
H_ACC[23] = Kacc*SH_ACC[3];
_drag_innov_var[0] = (R_ACC + Kacc*SH_ACC[0]*(Kacc*P[4][4]*SH_ACC[0] + Kacc*P[5][4]*SH_ACC[3] - Kacc*P[22][4]*SH_ACC[0] - Kacc*P[23][4]*SH_ACC[3] - Kacc*P[6][4]*(2.0f*q0*q2 - 2.0f*q1*q3) + Kacc*P[0][4]*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd) + Kacc*P[1][4]*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P[2][4]*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P[3][4]*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd)) + Kacc*SH_ACC[3]*(Kacc*P[4][5]*SH_ACC[0] + Kacc*P[5][5]*SH_ACC[3] - Kacc*P[22][5]*SH_ACC[0] - Kacc*P[23][5]*SH_ACC[3] - Kacc*P[6][5]*(2.0f*q0*q2 - 2.0f*q1*q3) + Kacc*P[0][5]*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd) + Kacc*P[1][5]*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P[2][5]*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P[3][5]*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd)) - Kacc*SH_ACC[0]*(Kacc*P[4][22]*SH_ACC[0] + Kacc*P[5][22]*SH_ACC[3] - Kacc*P[22][22]*SH_ACC[0] - Kacc*P[23][22]*SH_ACC[3] - Kacc*P[6][22]*(2.0f*q0*q2 - 2.0f*q1*q3) + Kacc*P[0][22]*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd) + Kacc*P[1][22]*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P[2][22]*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P[3][22]*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd)) - Kacc*SH_ACC[3]*(Kacc*P[4][23]*SH_ACC[0] + Kacc*P[5][23]*SH_ACC[3] - Kacc*P[22][23]*SH_ACC[0] - Kacc*P[23][23]*SH_ACC[3] - Kacc*P[6][23]*(2.0f*q0*q2 - 2.0f*q1*q3) + Kacc*P[0][23]*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd) + Kacc*P[1][23]*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P[2][23]*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P[3][23]*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd)) - Kacc*(2.0f*q0*q2 - 2.0f*q1*q3)*(Kacc*P[4][6]*SH_ACC[0] + Kacc*P[5][6]*SH_ACC[3] - Kacc*P[22][6]*SH_ACC[0] - Kacc*P[23][6]*SH_ACC[3] - Kacc*P[6][6]*(2.0f*q0*q2 - 2.0f*q1*q3) + Kacc*P[0][6]*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd) + Kacc*P[1][6]*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P[2][6]*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P[3][6]*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd)) + Kacc*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd)*(Kacc*P[4][0]*SH_ACC[0] + Kacc*P[5][0]*SH_ACC[3] - Kacc*P[22][0]*SH_ACC[0] - Kacc*P[23][0]*SH_ACC[3] - Kacc*P[6][0]*(2.0f*q0*q2 - 2.0f*q1*q3) + Kacc*P[0][0]*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd) + Kacc*P[1][0]*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P[2][0]*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P[3][0]*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd)) + Kacc*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd)*(Kacc*P[4][1]*SH_ACC[0] + Kacc*P[5][1]*SH_ACC[3] - Kacc*P[22][1]*SH_ACC[0] - Kacc*P[23][1]*SH_ACC[3] - Kacc*P[6][1]*(2.0f*q0*q2 - 2.0f*q1*q3) + Kacc*P[0][1]*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd) + Kacc*P[1][1]*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P[2][1]*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P[3][1]*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd)) - Kacc*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd)*(Kacc*P[4][2]*SH_ACC[0] + Kacc*P[5][2]*SH_ACC[3] - Kacc*P[22][2]*SH_ACC[0] - Kacc*P[23][2]*SH_ACC[3] - Kacc*P[6][2]*(2.0f*q0*q2 - 2.0f*q1*q3) + Kacc*P[0][2]*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd) + Kacc*P[1][2]*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P[2][2]*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P[3][2]*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd)) + Kacc*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd)*(Kacc*P[4][3]*SH_ACC[0] + Kacc*P[5][3]*SH_ACC[3] - Kacc*P[22][3]*SH_ACC[0] - Kacc*P[23][3]*SH_ACC[3] - Kacc*P[6][3]*(2.0f*q0*q2 - 2.0f*q1*q3) + Kacc*P[0][3]*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd) + Kacc*P[1][3]*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P[2][3]*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P[3][3]*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd)));
if (_drag_innov_var[0] < R_ACC) {
return;
}
SK_ACC[0] = 1.0f/_drag_innov_var[0];
SK_ACC[1] = 2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd;
SK_ACC[2] = 2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd;
SK_ACC[3] = 2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd;
SK_ACC[4] = 2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd;
SK_ACC[5] = 2.0f*q0*q2 - 2.0f*q1*q3;
SK_ACC[6] = SH_ACC[3];
// Don't allow modification of any states other than wind velocity at this stage of development - we only need a wind estimate.
// Kfusion[0] = -SK_ACC[0]*(Kacc*P[0][4]*SH_ACC[0] - Kacc*P[0][22]*SH_ACC[0] + Kacc*P[0][0]*SK_ACC[3] - Kacc*P[0][2]*SK_ACC[2] + Kacc*P[0][3]*SK_ACC[1] + Kacc*P[0][1]*SK_ACC[4] + Kacc*P[0][5]*SK_ACC[6] - Kacc*P[0][6]*SK_ACC[5] - Kacc*P[0][23]*SK_ACC[6]);
// Kfusion[1] = -SK_ACC[0]*(Kacc*P[1][4]*SH_ACC[0] - Kacc*P[1][22]*SH_ACC[0] + Kacc*P[1][0]*SK_ACC[3] - Kacc*P[1][2]*SK_ACC[2] + Kacc*P[1][3]*SK_ACC[1] + Kacc*P[1][1]*SK_ACC[4] + Kacc*P[1][5]*SK_ACC[6] - Kacc*P[1][6]*SK_ACC[5] - Kacc*P[1][23]*SK_ACC[6]);
// Kfusion[2] = -SK_ACC[0]*(Kacc*P[2][4]*SH_ACC[0] - Kacc*P[2][22]*SH_ACC[0] + Kacc*P[2][0]*SK_ACC[3] - Kacc*P[2][2]*SK_ACC[2] + Kacc*P[2][3]*SK_ACC[1] + Kacc*P[2][1]*SK_ACC[4] + Kacc*P[2][5]*SK_ACC[6] - Kacc*P[2][6]*SK_ACC[5] - Kacc*P[2][23]*SK_ACC[6]);
// Kfusion[3] = -SK_ACC[0]*(Kacc*P[3][4]*SH_ACC[0] - Kacc*P[3][22]*SH_ACC[0] + Kacc*P[3][0]*SK_ACC[3] - Kacc*P[3][2]*SK_ACC[2] + Kacc*P[3][3]*SK_ACC[1] + Kacc*P[3][1]*SK_ACC[4] + Kacc*P[3][5]*SK_ACC[6] - Kacc*P[3][6]*SK_ACC[5] - Kacc*P[3][23]*SK_ACC[6]);
// Kfusion[4] = -SK_ACC[0]*(Kacc*P[4][4]*SH_ACC[0] - Kacc*P[4][22]*SH_ACC[0] + Kacc*P[4][0]*SK_ACC[3] - Kacc*P[4][2]*SK_ACC[2] + Kacc*P[4][3]*SK_ACC[1] + Kacc*P[4][1]*SK_ACC[4] + Kacc*P[4][5]*SK_ACC[6] - Kacc*P[4][6]*SK_ACC[5] - Kacc*P[4][23]*SK_ACC[6]);
// Kfusion[5] = -SK_ACC[0]*(Kacc*P[5][4]*SH_ACC[0] - Kacc*P[5][22]*SH_ACC[0] + Kacc*P[5][0]*SK_ACC[3] - Kacc*P[5][2]*SK_ACC[2] + Kacc*P[5][3]*SK_ACC[1] + Kacc*P[5][1]*SK_ACC[4] + Kacc*P[5][5]*SK_ACC[6] - Kacc*P[5][6]*SK_ACC[5] - Kacc*P[5][23]*SK_ACC[6]);
// Kfusion[6] = -SK_ACC[0]*(Kacc*P[6][4]*SH_ACC[0] - Kacc*P[6][22]*SH_ACC[0] + Kacc*P[6][0]*SK_ACC[3] - Kacc*P[6][2]*SK_ACC[2] + Kacc*P[6][3]*SK_ACC[1] + Kacc*P[6][1]*SK_ACC[4] + Kacc*P[6][5]*SK_ACC[6] - Kacc*P[6][6]*SK_ACC[5] - Kacc*P[6][23]*SK_ACC[6]);
// Kfusion[7] = -SK_ACC[0]*(Kacc*P[7][4]*SH_ACC[0] - Kacc*P[7][22]*SH_ACC[0] + Kacc*P[7][0]*SK_ACC[3] - Kacc*P[7][2]*SK_ACC[2] + Kacc*P[7][3]*SK_ACC[1] + Kacc*P[7][1]*SK_ACC[4] + Kacc*P[7][5]*SK_ACC[6] - Kacc*P[7][6]*SK_ACC[5] - Kacc*P[7][23]*SK_ACC[6]);
// Kfusion[8] = -SK_ACC[0]*(Kacc*P[8][4]*SH_ACC[0] - Kacc*P[8][22]*SH_ACC[0] + Kacc*P[8][0]*SK_ACC[3] - Kacc*P[8][2]*SK_ACC[2] + Kacc*P[8][3]*SK_ACC[1] + Kacc*P[8][1]*SK_ACC[4] + Kacc*P[8][5]*SK_ACC[6] - Kacc*P[8][6]*SK_ACC[5] - Kacc*P[8][23]*SK_ACC[6]);
// Kfusion[9] = -SK_ACC[0]*(Kacc*P[9][4]*SH_ACC[0] - Kacc*P[9][22]*SH_ACC[0] + Kacc*P[9][0]*SK_ACC[3] - Kacc*P[9][2]*SK_ACC[2] + Kacc*P[9][3]*SK_ACC[1] + Kacc*P[9][1]*SK_ACC[4] + Kacc*P[9][5]*SK_ACC[6] - Kacc*P[9][6]*SK_ACC[5] - Kacc*P[9][23]*SK_ACC[6]);
// Kfusion[10] = -SK_ACC[0]*(Kacc*P[10][4]*SH_ACC[0] - Kacc*P[10][22]*SH_ACC[0] + Kacc*P[10][0]*SK_ACC[3] - Kacc*P[10][2]*SK_ACC[2] + Kacc*P[10][3]*SK_ACC[1] + Kacc*P[10][1]*SK_ACC[4] + Kacc*P[10][5]*SK_ACC[6] - Kacc*P[10][6]*SK_ACC[5] - Kacc*P[10][23]*SK_ACC[6]);
// Kfusion[11] = -SK_ACC[0]*(Kacc*P[11][4]*SH_ACC[0] - Kacc*P[11][22]*SH_ACC[0] + Kacc*P[11][0]*SK_ACC[3] - Kacc*P[11][2]*SK_ACC[2] + Kacc*P[11][3]*SK_ACC[1] + Kacc*P[11][1]*SK_ACC[4] + Kacc*P[11][5]*SK_ACC[6] - Kacc*P[11][6]*SK_ACC[5] - Kacc*P[11][23]*SK_ACC[6]);
// Kfusion[12] = -SK_ACC[0]*(Kacc*P[12][4]*SH_ACC[0] - Kacc*P[12][22]*SH_ACC[0] + Kacc*P[12][0]*SK_ACC[3] - Kacc*P[12][2]*SK_ACC[2] + Kacc*P[12][3]*SK_ACC[1] + Kacc*P[12][1]*SK_ACC[4] + Kacc*P[12][5]*SK_ACC[6] - Kacc*P[12][6]*SK_ACC[5] - Kacc*P[12][23]*SK_ACC[6]);
// Kfusion[13] = -SK_ACC[0]*(Kacc*P[13][4]*SH_ACC[0] - Kacc*P[13][22]*SH_ACC[0] + Kacc*P[13][0]*SK_ACC[3] - Kacc*P[13][2]*SK_ACC[2] + Kacc*P[13][3]*SK_ACC[1] + Kacc*P[13][1]*SK_ACC[4] + Kacc*P[13][5]*SK_ACC[6] - Kacc*P[13][6]*SK_ACC[5] - Kacc*P[13][23]*SK_ACC[6]);
// Kfusion[14] = -SK_ACC[0]*(Kacc*P[14][4]*SH_ACC[0] - Kacc*P[14][22]*SH_ACC[0] + Kacc*P[14][0]*SK_ACC[3] - Kacc*P[14][2]*SK_ACC[2] + Kacc*P[14][3]*SK_ACC[1] + Kacc*P[14][1]*SK_ACC[4] + Kacc*P[14][5]*SK_ACC[6] - Kacc*P[14][6]*SK_ACC[5] - Kacc*P[14][23]*SK_ACC[6]);
// Kfusion[15] = -SK_ACC[0]*(Kacc*P[15][4]*SH_ACC[0] - Kacc*P[15][22]*SH_ACC[0] + Kacc*P[15][0]*SK_ACC[3] - Kacc*P[15][2]*SK_ACC[2] + Kacc*P[15][3]*SK_ACC[1] + Kacc*P[15][1]*SK_ACC[4] + Kacc*P[15][5]*SK_ACC[6] - Kacc*P[15][6]*SK_ACC[5] - Kacc*P[15][23]*SK_ACC[6]);
// Kfusion[16] = -SK_ACC[0]*(Kacc*P[16][4]*SH_ACC[0] - Kacc*P[16][22]*SH_ACC[0] + Kacc*P[16][0]*SK_ACC[3] - Kacc*P[16][2]*SK_ACC[2] + Kacc*P[16][3]*SK_ACC[1] + Kacc*P[16][1]*SK_ACC[4] + Kacc*P[16][5]*SK_ACC[6] - Kacc*P[16][6]*SK_ACC[5] - Kacc*P[16][23]*SK_ACC[6]);
// Kfusion[17] = -SK_ACC[0]*(Kacc*P[17][4]*SH_ACC[0] - Kacc*P[17][22]*SH_ACC[0] + Kacc*P[17][0]*SK_ACC[3] - Kacc*P[17][2]*SK_ACC[2] + Kacc*P[17][3]*SK_ACC[1] + Kacc*P[17][1]*SK_ACC[4] + Kacc*P[17][5]*SK_ACC[6] - Kacc*P[17][6]*SK_ACC[5] - Kacc*P[17][23]*SK_ACC[6]);
// Kfusion[18] = -SK_ACC[0]*(Kacc*P[18][4]*SH_ACC[0] - Kacc*P[18][22]*SH_ACC[0] + Kacc*P[18][0]*SK_ACC[3] - Kacc*P[18][2]*SK_ACC[2] + Kacc*P[18][3]*SK_ACC[1] + Kacc*P[18][1]*SK_ACC[4] + Kacc*P[18][5]*SK_ACC[6] - Kacc*P[18][6]*SK_ACC[5] - Kacc*P[18][23]*SK_ACC[6]);
// Kfusion[19] = -SK_ACC[0]*(Kacc*P[19][4]*SH_ACC[0] - Kacc*P[19][22]*SH_ACC[0] + Kacc*P[19][0]*SK_ACC[3] - Kacc*P[19][2]*SK_ACC[2] + Kacc*P[19][3]*SK_ACC[1] + Kacc*P[19][1]*SK_ACC[4] + Kacc*P[19][5]*SK_ACC[6] - Kacc*P[19][6]*SK_ACC[5] - Kacc*P[19][23]*SK_ACC[6]);
// Kfusion[20] = -SK_ACC[0]*(Kacc*P[20][4]*SH_ACC[0] - Kacc*P[20][22]*SH_ACC[0] + Kacc*P[20][0]*SK_ACC[3] - Kacc*P[20][2]*SK_ACC[2] + Kacc*P[20][3]*SK_ACC[1] + Kacc*P[20][1]*SK_ACC[4] + Kacc*P[20][5]*SK_ACC[6] - Kacc*P[20][6]*SK_ACC[5] - Kacc*P[20][23]*SK_ACC[6]);
// Kfusion[21] = -SK_ACC[0]*(Kacc*P[21][4]*SH_ACC[0] - Kacc*P[21][22]*SH_ACC[0] + Kacc*P[21][0]*SK_ACC[3] - Kacc*P[21][2]*SK_ACC[2] + Kacc*P[21][3]*SK_ACC[1] + Kacc*P[21][1]*SK_ACC[4] + Kacc*P[21][5]*SK_ACC[6] - Kacc*P[21][6]*SK_ACC[5] - Kacc*P[21][23]*SK_ACC[6]);
Kfusion[22] = -SK_ACC[0]*(Kacc*P[22][4]*SH_ACC[0] - Kacc*P[22][22]*SH_ACC[0] + Kacc*P[22][0]*SK_ACC[3] - Kacc*P[22][2]*SK_ACC[2] + Kacc*P[22][3]*SK_ACC[1] + Kacc*P[22][1]*SK_ACC[4] + Kacc*P[22][5]*SK_ACC[6] - Kacc*P[22][6]*SK_ACC[5] - Kacc*P[22][23]*SK_ACC[6]);
Kfusion[23] = -SK_ACC[0]*(Kacc*P[23][4]*SH_ACC[0] - Kacc*P[23][22]*SH_ACC[0] + Kacc*P[23][0]*SK_ACC[3] - Kacc*P[23][2]*SK_ACC[2] + Kacc*P[23][3]*SK_ACC[1] + Kacc*P[23][1]*SK_ACC[4] + Kacc*P[23][5]*SK_ACC[6] - Kacc*P[23][6]*SK_ACC[5] - Kacc*P[23][23]*SK_ACC[6]);
// calculate the predicted acceleration and innovation measured along the X body axis
float drag_sign;
if (rel_wind(axis_index) >= 0.0f) {
drag_sign = 1.0f;
} else {
drag_sign = -1.0f;
}
float predAccel = -BC_inv_x * 0.5f*rho*sq(rel_wind(axis_index)) * drag_sign;
_drag_innov[axis_index] = predAccel - mea_acc;
_drag_test_ratio[axis_index] = sq(_drag_innov[axis_index]) / (25.0f * _drag_innov_var[axis_index]);
} else if (axis_index == 1) {
// Estimate the airspeed from the measured drag force and ballistic coefficient
float mea_acc = (_imu_sample_delayed.delta_vel(axis_index) - _state.accel_bias(axis_index))/_imu_sample_delayed.delta_vel_dt;
float airSpd = sqrtf((2.0f * fabsf(mea_acc)) / (BC_inv_y * rho));
// Estimate the derivative of specific force wrt airspeed along the X axis
// Limit lower value to prevent artithmetic exceptions
float Kacc = fmax(1e-1f,rho * BC_inv_y * airSpd);
SH_ACC[0] = sq(q0) - sq(q1) + sq(q2) - sq(q3);
SH_ACC[1] = vn - vwn;
SH_ACC[2] = ve - vwe;
H_ACC[0] = -Kacc*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd);
H_ACC[1] = -Kacc*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd);
H_ACC[2] = -Kacc*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd);
H_ACC[3] = Kacc*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd);
H_ACC[4] = Kacc*(2.0f*q0*q3 - 2.0f*q1*q2);
H_ACC[5] = -Kacc*SH_ACC[0];
H_ACC[6] = -Kacc*(2.0f*q0*q1 + 2.0f*q2*q3);
H_ACC[22] = -2.0f*Kacc*(q0*q3 - q1*q2);
H_ACC[23] = Kacc*SH_ACC[0];
_drag_innov_var[1] = (R_ACC + Kacc*SH_ACC[0]*(Kacc*P[5][5]*SH_ACC[0] - Kacc*P[23][5]*SH_ACC[0] - Kacc*P[4][5]*(2.0f*q0*q3 - 2.0f*q1*q2) + Kacc*P[6][5]*(2.0f*q0*q1 + 2.0f*q2*q3) + 2*Kacc*P[22][5]*(q0*q3 - q1*q2) + Kacc*P[0][5]*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd) + Kacc*P[1][5]*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P[2][5]*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P[3][5]*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd)) - Kacc*SH_ACC[0]*(Kacc*P[5][23]*SH_ACC[0] - Kacc*P[23][23]*SH_ACC[0] - Kacc*P[4][23]*(2.0f*q0*q3 - 2.0f*q1*q2) + Kacc*P[6][23]*(2.0f*q0*q1 + 2.0f*q2*q3) + 2*Kacc*P[22][23]*(q0*q3 - q1*q2) + Kacc*P[0][23]*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd) + Kacc*P[1][23]*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P[2][23]*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P[3][23]*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd)) - Kacc*(2.0f*q0*q3 - 2.0f*q1*q2)*(Kacc*P[5][4]*SH_ACC[0] - Kacc*P[23][4]*SH_ACC[0] - Kacc*P[4][4]*(2.0f*q0*q3 - 2.0f*q1*q2) + Kacc*P[6][4]*(2.0f*q0*q1 + 2.0f*q2*q3) + 2*Kacc*P[22][4]*(q0*q3 - q1*q2) + Kacc*P[0][4]*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd) + Kacc*P[1][4]*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P[2][4]*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P[3][4]*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd)) + Kacc*(2.0f*q0*q1 + 2.0f*q2*q3)*(Kacc*P[5][6]*SH_ACC[0] - Kacc*P[23][6]*SH_ACC[0] - Kacc*P[4][6]*(2.0f*q0*q3 - 2.0f*q1*q2) + Kacc*P[6][6]*(2.0f*q0*q1 + 2.0f*q2*q3) + 2*Kacc*P[22][6]*(q0*q3 - q1*q2) + Kacc*P[0][6]*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd) + Kacc*P[1][6]*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P[2][6]*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P[3][6]*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd)) + 2*Kacc*(q0*q3 - q1*q2)*(Kacc*P[5][22]*SH_ACC[0] - Kacc*P[23][22]*SH_ACC[0] - Kacc*P[4][22]*(2.0f*q0*q3 - 2.0f*q1*q2) + Kacc*P[6][22]*(2.0f*q0*q1 + 2.0f*q2*q3) + 2*Kacc*P[22][22]*(q0*q3 - q1*q2) + Kacc*P[0][22]*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd) + Kacc*P[1][22]*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P[2][22]*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P[3][22]*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd)) + Kacc*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd)*(Kacc*P[5][0]*SH_ACC[0] - Kacc*P[23][0]*SH_ACC[0] - Kacc*P[4][0]*(2.0f*q0*q3 - 2.0f*q1*q2) + Kacc*P[6][0]*(2.0f*q0*q1 + 2.0f*q2*q3) + 2*Kacc*P[22][0]*(q0*q3 - q1*q2) + Kacc*P[0][0]*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd) + Kacc*P[1][0]*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P[2][0]*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P[3][0]*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd)) + Kacc*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd)*(Kacc*P[5][1]*SH_ACC[0] - Kacc*P[23][1]*SH_ACC[0] - Kacc*P[4][1]*(2.0f*q0*q3 - 2.0f*q1*q2) + Kacc*P[6][1]*(2.0f*q0*q1 + 2.0f*q2*q3) + 2*Kacc*P[22][1]*(q0*q3 - q1*q2) + Kacc*P[0][1]*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd) + Kacc*P[1][1]*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P[2][1]*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P[3][1]*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd)) + Kacc*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd)*(Kacc*P[5][2]*SH_ACC[0] - Kacc*P[23][2]*SH_ACC[0] - Kacc*P[4][2]*(2.0f*q0*q3 - 2.0f*q1*q2) + Kacc*P[6][2]*(2.0f*q0*q1 + 2.0f*q2*q3) + 2*Kacc*P[22][2]*(q0*q3 - q1*q2) + Kacc*P[0][2]*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd) + Kacc*P[1][2]*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P[2][2]*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P[3][2]*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd)) - Kacc*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd)*(Kacc*P[5][3]*SH_ACC[0] - Kacc*P[23][3]*SH_ACC[0] - Kacc*P[4][3]*(2.0f*q0*q3 - 2.0f*q1*q2) + Kacc*P[6][3]*(2.0f*q0*q1 + 2.0f*q2*q3) + 2*Kacc*P[22][3]*(q0*q3 - q1*q2) + Kacc*P[0][3]*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd) + Kacc*P[1][3]*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd) + Kacc*P[2][3]*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd) - Kacc*P[3][3]*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd)));
if (_drag_innov_var[1] < R_ACC) {
// calculation is badly conditioned
return;
}
SK_ACC[0] = 1.0f/_drag_innov_var[1];
SK_ACC[1] = 2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd;
SK_ACC[2] = 2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd;
SK_ACC[3] = 2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd;
SK_ACC[4] = 2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd;
SK_ACC[5] = 2.0f*q0*q3 - 2.0f*q1*q2;
SK_ACC[6] = q0*q3 - q1*q2;
SK_ACC[7] = 2.0f*q0*q1 + 2.0f*q2*q3;
SK_ACC[8] = SH_ACC[0];
// Don't allow modification of any states other than wind velocity at this stage of development - we only need a wind estimate.
// Kfusion[0] = -SK_ACC[0]*(Kacc*P[0][0]*SK_ACC[3] + Kacc*P[0][1]*SK_ACC[2] - Kacc*P[0][3]*SK_ACC[1] + Kacc*P[0][2]*SK_ACC[4] - Kacc*P[0][4]*SK_ACC[5] + Kacc*P[0][5]*SK_ACC[8] + Kacc*P[0][6]*SK_ACC[7] + 2*Kacc*P[0][22]*SK_ACC[6] - Kacc*P[0][23]*SK_ACC[8]);
// Kfusion[1] = -SK_ACC[0]*(Kacc*P[1][0]*SK_ACC[3] + Kacc*P[1][1]*SK_ACC[2] - Kacc*P[1][3]*SK_ACC[1] + Kacc*P[1][2]*SK_ACC[4] - Kacc*P[1][4]*SK_ACC[5] + Kacc*P[1][5]*SK_ACC[8] + Kacc*P[1][6]*SK_ACC[7] + 2*Kacc*P[1][22]*SK_ACC[6] - Kacc*P[1][23]*SK_ACC[8]);
// Kfusion[2] = -SK_ACC[0]*(Kacc*P[2][0]*SK_ACC[3] + Kacc*P[2][1]*SK_ACC[2] - Kacc*P[2][3]*SK_ACC[1] + Kacc*P[2][2]*SK_ACC[4] - Kacc*P[2][4]*SK_ACC[5] + Kacc*P[2][5]*SK_ACC[8] + Kacc*P[2][6]*SK_ACC[7] + 2*Kacc*P[2][22]*SK_ACC[6] - Kacc*P[2][23]*SK_ACC[8]);
// Kfusion[3] = -SK_ACC[0]*(Kacc*P[3][0]*SK_ACC[3] + Kacc*P[3][1]*SK_ACC[2] - Kacc*P[3][3]*SK_ACC[1] + Kacc*P[3][2]*SK_ACC[4] - Kacc*P[3][4]*SK_ACC[5] + Kacc*P[3][5]*SK_ACC[8] + Kacc*P[3][6]*SK_ACC[7] + 2*Kacc*P[3][22]*SK_ACC[6] - Kacc*P[3][23]*SK_ACC[8]);
// Kfusion[4] = -SK_ACC[0]*(Kacc*P[4][0]*SK_ACC[3] + Kacc*P[4][1]*SK_ACC[2] - Kacc*P[4][3]*SK_ACC[1] + Kacc*P[4][2]*SK_ACC[4] - Kacc*P[4][4]*SK_ACC[5] + Kacc*P[4][5]*SK_ACC[8] + Kacc*P[4][6]*SK_ACC[7] + 2*Kacc*P[4][22]*SK_ACC[6] - Kacc*P[4][23]*SK_ACC[8]);
// Kfusion[5] = -SK_ACC[0]*(Kacc*P[5][0]*SK_ACC[3] + Kacc*P[5][1]*SK_ACC[2] - Kacc*P[5][3]*SK_ACC[1] + Kacc*P[5][2]*SK_ACC[4] - Kacc*P[5][4]*SK_ACC[5] + Kacc*P[5][5]*SK_ACC[8] + Kacc*P[5][6]*SK_ACC[7] + 2*Kacc*P[5][22]*SK_ACC[6] - Kacc*P[5][23]*SK_ACC[8]);
// Kfusion[6] = -SK_ACC[0]*(Kacc*P[6][0]*SK_ACC[3] + Kacc*P[6][1]*SK_ACC[2] - Kacc*P[6][3]*SK_ACC[1] + Kacc*P[6][2]*SK_ACC[4] - Kacc*P[6][4]*SK_ACC[5] + Kacc*P[6][5]*SK_ACC[8] + Kacc*P[6][6]*SK_ACC[7] + 2*Kacc*P[6][22]*SK_ACC[6] - Kacc*P[6][23]*SK_ACC[8]);
// Kfusion[7] = -SK_ACC[0]*(Kacc*P[7][0]*SK_ACC[3] + Kacc*P[7][1]*SK_ACC[2] - Kacc*P[7][3]*SK_ACC[1] + Kacc*P[7][2]*SK_ACC[4] - Kacc*P[7][4]*SK_ACC[5] + Kacc*P[7][5]*SK_ACC[8] + Kacc*P[7][6]*SK_ACC[7] + 2*Kacc*P[7][22]*SK_ACC[6] - Kacc*P[7][23]*SK_ACC[8]);
// Kfusion[8] = -SK_ACC[0]*(Kacc*P[8][0]*SK_ACC[3] + Kacc*P[8][1]*SK_ACC[2] - Kacc*P[8][3]*SK_ACC[1] + Kacc*P[8][2]*SK_ACC[4] - Kacc*P[8][4]*SK_ACC[5] + Kacc*P[8][5]*SK_ACC[8] + Kacc*P[8][6]*SK_ACC[7] + 2*Kacc*P[8][22]*SK_ACC[6] - Kacc*P[8][23]*SK_ACC[8]);
// Kfusion[9] = -SK_ACC[0]*(Kacc*P[9][0]*SK_ACC[3] + Kacc*P[9][1]*SK_ACC[2] - Kacc*P[9][3]*SK_ACC[1] + Kacc*P[9][2]*SK_ACC[4] - Kacc*P[9][4]*SK_ACC[5] + Kacc*P[9][5]*SK_ACC[8] + Kacc*P[9][6]*SK_ACC[7] + 2*Kacc*P[9][22]*SK_ACC[6] - Kacc*P[9][23]*SK_ACC[8]);
// Kfusion[10] = -SK_ACC[0]*(Kacc*P[10][0]*SK_ACC[3] + Kacc*P[10][1]*SK_ACC[2] - Kacc*P[10][3]*SK_ACC[1] + Kacc*P[10][2]*SK_ACC[4] - Kacc*P[10][4]*SK_ACC[5] + Kacc*P[10][5]*SK_ACC[8] + Kacc*P[10][6]*SK_ACC[7] + 2*Kacc*P[10][22]*SK_ACC[6] - Kacc*P[10][23]*SK_ACC[8]);
// Kfusion[11] = -SK_ACC[0]*(Kacc*P[11][0]*SK_ACC[3] + Kacc*P[11][1]*SK_ACC[2] - Kacc*P[11][3]*SK_ACC[1] + Kacc*P[11][2]*SK_ACC[4] - Kacc*P[11][4]*SK_ACC[5] + Kacc*P[11][5]*SK_ACC[8] + Kacc*P[11][6]*SK_ACC[7] + 2*Kacc*P[11][22]*SK_ACC[6] - Kacc*P[11][23]*SK_ACC[8]);
// Kfusion[12] = -SK_ACC[0]*(Kacc*P[12][0]*SK_ACC[3] + Kacc*P[12][1]*SK_ACC[2] - Kacc*P[12][3]*SK_ACC[1] + Kacc*P[12][2]*SK_ACC[4] - Kacc*P[12][4]*SK_ACC[5] + Kacc*P[12][5]*SK_ACC[8] + Kacc*P[12][6]*SK_ACC[7] + 2*Kacc*P[12][22]*SK_ACC[6] - Kacc*P[12][23]*SK_ACC[8]);
// Kfusion[13] = -SK_ACC[0]*(Kacc*P[13][0]*SK_ACC[3] + Kacc*P[13][1]*SK_ACC[2] - Kacc*P[13][3]*SK_ACC[1] + Kacc*P[13][2]*SK_ACC[4] - Kacc*P[13][4]*SK_ACC[5] + Kacc*P[13][5]*SK_ACC[8] + Kacc*P[13][6]*SK_ACC[7] + 2*Kacc*P[13][22]*SK_ACC[6] - Kacc*P[13][23]*SK_ACC[8]);
// Kfusion[14] = -SK_ACC[0]*(Kacc*P[14][0]*SK_ACC[3] + Kacc*P[14][1]*SK_ACC[2] - Kacc*P[14][3]*SK_ACC[1] + Kacc*P[14][2]*SK_ACC[4] - Kacc*P[14][4]*SK_ACC[5] + Kacc*P[14][5]*SK_ACC[8] + Kacc*P[14][6]*SK_ACC[7] + 2*Kacc*P[14][22]*SK_ACC[6] - Kacc*P[14][23]*SK_ACC[8]);
// Kfusion[15] = -SK_ACC[0]*(Kacc*P[15][0]*SK_ACC[3] + Kacc*P[15][1]*SK_ACC[2] - Kacc*P[15][3]*SK_ACC[1] + Kacc*P[15][2]*SK_ACC[4] - Kacc*P[15][4]*SK_ACC[5] + Kacc*P[15][5]*SK_ACC[8] + Kacc*P[15][6]*SK_ACC[7] + 2*Kacc*P[15][22]*SK_ACC[6] - Kacc*P[15][23]*SK_ACC[8]);
// Kfusion[16] = -SK_ACC[0]*(Kacc*P[16][0]*SK_ACC[3] + Kacc*P[16][1]*SK_ACC[2] - Kacc*P[16][3]*SK_ACC[1] + Kacc*P[16][2]*SK_ACC[4] - Kacc*P[16][4]*SK_ACC[5] + Kacc*P[16][5]*SK_ACC[8] + Kacc*P[16][6]*SK_ACC[7] + 2*Kacc*P[16][22]*SK_ACC[6] - Kacc*P[16][23]*SK_ACC[8]);
// Kfusion[17] = -SK_ACC[0]*(Kacc*P[17][0]*SK_ACC[3] + Kacc*P[17][1]*SK_ACC[2] - Kacc*P[17][3]*SK_ACC[1] + Kacc*P[17][2]*SK_ACC[4] - Kacc*P[17][4]*SK_ACC[5] + Kacc*P[17][5]*SK_ACC[8] + Kacc*P[17][6]*SK_ACC[7] + 2*Kacc*P[17][22]*SK_ACC[6] - Kacc*P[17][23]*SK_ACC[8]);
// Kfusion[18] = -SK_ACC[0]*(Kacc*P[18][0]*SK_ACC[3] + Kacc*P[18][1]*SK_ACC[2] - Kacc*P[18][3]*SK_ACC[1] + Kacc*P[18][2]*SK_ACC[4] - Kacc*P[18][4]*SK_ACC[5] + Kacc*P[18][5]*SK_ACC[8] + Kacc*P[18][6]*SK_ACC[7] + 2*Kacc*P[18][22]*SK_ACC[6] - Kacc*P[18][23]*SK_ACC[8]);
// Kfusion[19] = -SK_ACC[0]*(Kacc*P[19][0]*SK_ACC[3] + Kacc*P[19][1]*SK_ACC[2] - Kacc*P[19][3]*SK_ACC[1] + Kacc*P[19][2]*SK_ACC[4] - Kacc*P[19][4]*SK_ACC[5] + Kacc*P[19][5]*SK_ACC[8] + Kacc*P[19][6]*SK_ACC[7] + 2*Kacc*P[19][22]*SK_ACC[6] - Kacc*P[19][23]*SK_ACC[8]);
// Kfusion[20] = -SK_ACC[0]*(Kacc*P[20][0]*SK_ACC[3] + Kacc*P[20][1]*SK_ACC[2] - Kacc*P[20][3]*SK_ACC[1] + Kacc*P[20][2]*SK_ACC[4] - Kacc*P[20][4]*SK_ACC[5] + Kacc*P[20][5]*SK_ACC[8] + Kacc*P[20][6]*SK_ACC[7] + 2*Kacc*P[20][22]*SK_ACC[6] - Kacc*P[20][23]*SK_ACC[8]);
// Kfusion[21] = -SK_ACC[0]*(Kacc*P[21][0]*SK_ACC[3] + Kacc*P[21][1]*SK_ACC[2] - Kacc*P[21][3]*SK_ACC[1] + Kacc*P[21][2]*SK_ACC[4] - Kacc*P[21][4]*SK_ACC[5] + Kacc*P[21][5]*SK_ACC[8] + Kacc*P[21][6]*SK_ACC[7] + 2*Kacc*P[21][22]*SK_ACC[6] - Kacc*P[21][23]*SK_ACC[8]);
Kfusion[22] = -SK_ACC[0]*(Kacc*P[22][0]*SK_ACC[3] + Kacc*P[22][1]*SK_ACC[2] - Kacc*P[22][3]*SK_ACC[1] + Kacc*P[22][2]*SK_ACC[4] - Kacc*P[22][4]*SK_ACC[5] + Kacc*P[22][5]*SK_ACC[8] + Kacc*P[22][6]*SK_ACC[7] + 2*Kacc*P[22][22]*SK_ACC[6] - Kacc*P[22][23]*SK_ACC[8]);
Kfusion[23] = -SK_ACC[0]*(Kacc*P[23][0]*SK_ACC[3] + Kacc*P[23][1]*SK_ACC[2] - Kacc*P[23][3]*SK_ACC[1] + Kacc*P[23][2]*SK_ACC[4] - Kacc*P[23][4]*SK_ACC[5] + Kacc*P[23][5]*SK_ACC[8] + Kacc*P[23][6]*SK_ACC[7] + 2*Kacc*P[23][22]*SK_ACC[6] - Kacc*P[23][23]*SK_ACC[8]);
// calculate the predicted acceleration and innovation measured along the Y body axis
float drag_sign;
if (rel_wind(axis_index) >= 0.0f) {
drag_sign = 1.0f;
} else {
drag_sign = -1.0f;
}
float predAccel = -BC_inv_y * 0.5f*rho*sq(rel_wind(axis_index)) * drag_sign;
_drag_innov[axis_index] = predAccel - mea_acc;
_drag_test_ratio[axis_index] = sq(_drag_innov[axis_index]) / (25.0f * _drag_innov_var[axis_index]);
}
// if the innovation consistency check fails then don't fuse the sample
if (_drag_test_ratio[axis_index] <= 1.0f) {
// apply covariance correction via P_new = (I -K*H)*P
// first calculate expression for KHP
// then calculate P - KHP
float KHP[_k_num_states][_k_num_states];
float KH[9];
for (unsigned row = 0; row < _k_num_states; row++) {
KH[0] = Kfusion[row] * H_ACC[0];
KH[1] = Kfusion[row] * H_ACC[1];
KH[2] = Kfusion[row] * H_ACC[2];
KH[3] = Kfusion[row] * H_ACC[3];
KH[4] = Kfusion[row] * H_ACC[4];
KH[5] = Kfusion[row] * H_ACC[5];
KH[6] = Kfusion[row] * H_ACC[6];
KH[7] = Kfusion[row] * H_ACC[22];
KH[8] = Kfusion[row] * H_ACC[23];
for (unsigned column = 0; column < _k_num_states; column++) {
float tmp = KH[0] * P[0][column];
tmp += KH[1] * P[1][column];
tmp += KH[2] * P[2][column];
tmp += KH[3] * P[3][column];
tmp += KH[4] * P[4][column];
tmp += KH[5] * P[5][column];
tmp += KH[6] * P[6][column];
tmp += KH[7] * P[22][column];
tmp += KH[8] * P[23][column];
KHP[row][column] = tmp;
}
}
// if the covariance correction will result in a negative variance, then
// the covariance marix is unhealthy and must be corrected
bool healthy = true;
//_fault_status.flags.bad_sideslip = false;
for (int i = 0; i < _k_num_states; i++) {
if (P[i][i] < KHP[i][i]) {
// zero rows and columns
zeroRows(P,i,i);
zeroCols(P,i,i);
//flag as unhealthy
healthy = false;
// update individual measurement health status
//_fault_status.flags.bad_sideslip = true;
}
}
// only apply covariance and state corrrections if healthy
if (healthy) {
// apply the covariance corrections
for (unsigned row = 0; row < _k_num_states; row++) {
for (unsigned column = 0; column < _k_num_states; column++) {
P[row][column] = P[row][column] - KHP[row][column];
}
}
// correct the covariance marix for gross errors
fixCovarianceErrors();
// apply the state corrections
fuse(Kfusion, _drag_innov[axis_index]);
}
}
}
}
EKF/ekf.cpp
+2
View File
@@ -150,6 +150,8 @@ Ekf::Ekf():
_gps_check_fail_status.value = 0;
_state_reset_status = {};
_dt_ekf_avg = 0.001f * (float)(FILTER_UPDATE_PERIOD_MS);
memset(_drag_innov, 0, sizeof(_drag_innov));
memset(_drag_innov_var, 0, sizeof(_drag_innov_var));
}
bool Ekf::init(uint64_t timestamp)
EKF/ekf.h
+15
View File
@@ -94,6 +94,12 @@ public:
// gets the innovation of the flow measurement
void get_flow_innov(float flow_innov[2]);
// gets the innovation variance of the drag specific force measurement
void get_drag_innov_var(float drag_innov_var[2]);
// gets the innovation of the drag specific force measurement
void get_drag_innov(float drag_innov[2]);
// gets the innovation variance of the HAGL measurement
void get_hagl_innov_var(float *hagl_innov_var);
@@ -272,6 +278,9 @@ private:
float _beta_innov; // synthetic sideslip measurement innovation
float _beta_innov_var; // synthetic sideslip measurement innovation variance
float _drag_innov[2]; // multirotor drag measurement innovation
float _drag_innov_var[2]; // multirotor drag measurement innovation variance
float _heading_innov; // heading measurement innovation
float _heading_innov_var; // heading measurement innovation variance
@@ -383,6 +392,9 @@ private:
// fuse synthetic zero sideslip measurement
void fuseSideslip();
// fuse body frame drag specific forces for multi-rotor wind estimation
void fuseDrag();
// fuse velocity and position measurements (also barometer height)
void fuseVelPosHeight();
@@ -464,6 +476,9 @@ private:
// control fusion of synthetic sideslip observations
void controlBetaFusion();
// control fusion of multi-rotor drag specific force observations
void controlDragFusion();
// control fusion of pressure altitude observations
void controlBaroFusion();
EKF/estimator_interface.cpp
+1
View File
@@ -75,6 +75,7 @@ EstimatorInterface::EstimatorInterface():
_tas_test_ratio(0.0f),
_terr_test_ratio(0.0f),
_beta_test_ratio(0.0f),
_drag_test_ratio{},
_is_dead_reckoning(false),
_vibe_metrics{},
_time_last_imu(0),
EKF/estimator_interface.h
+7
View File
@@ -107,6 +107,12 @@ public:
// gets the innovation of the flow measurement
virtual void get_flow_innov(float flow_innov[2]) = 0;
// gets the innovation variance of the drag specific force measurement
virtual void get_drag_innov_var(float drag_innov_var[2]) = 0;
// gets the innovation of the drag specific force measurement
virtual void get_drag_innov(float drag_innov[2]) = 0;
// gets the innovation variance of the HAGL measurement
virtual void get_hagl_innov_var(float *flow_innov_var) = 0;
@@ -339,6 +345,7 @@ protected:
float _tas_test_ratio; // tas innovation consistency check ratio
float _terr_test_ratio; // height above terrain measurement innovation consistency check ratio
float _beta_test_ratio; // sideslip innovation consistency check ratio
float _drag_test_ratio[2]; // drag innovation cinsistency check ratio
innovation_fault_status_u _innov_check_fail_status{};
bool _is_dead_reckoning; // true if we are no longer fusing measurements that constrain horizontal velocity drift
EKF/optflow_fusion.cpp
+11
View File
@@ -510,6 +510,17 @@ void Ekf::get_flow_innov_var(float flow_innov_var[2])
memcpy(flow_innov_var, _flow_innov_var, sizeof(_flow_innov_var));
}
void Ekf::get_drag_innov(float drag_innov[2])
{
memcpy(drag_innov, _drag_innov, sizeof(_drag_innov));
}
void Ekf::get_drag_innov_var(float drag_innov_var[2])
{
memcpy(drag_innov_var, _drag_innov_var, sizeof(_drag_innov_var));
}
// calculate optical flow gyro bias errors
void Ekf::calcOptFlowBias()
{