PX4 Firmware
PX4 Autopilot Software http://px4.io
drag_fusion.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name ECL nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 /**
35  * @file drag_fusion.cpp
36  * body frame drag fusion methods used for multi-rotor wind estimation.
37  *
38  * @author Paul Riseborough <p_riseborough@live.com.au>
39  *
40  */
41 
42 #include "ekf.h"
43 #include <ecl.h>
44 #include <mathlib/mathlib.h>
45 
46 void Ekf::fuseDrag()
47 {
48  float SH_ACC[4] = {}; // Variable used to optimise calculations of measurement jacobian
49  float H_ACC[24] = {}; // Observation Jacobian
50  float SK_ACC[9] = {}; // Variable used to optimise calculations of the Kalman gain vector
51  float Kfusion[24] = {}; // Kalman gain vector
52  float R_ACC = _params.drag_noise; // observation noise variance in specific force drag (m/sec**2)**2
53 
54  float rho = fmaxf(_air_density, 0.1f); // air density (kg/m**3)
55 
56  // calculate inverse of ballistic coefficient
57  if (_params.bcoef_x < 1.0f || _params.bcoef_y < 1.0f) {
58  return;
59  }
60 
61  float BC_inv_x = 1.0f / _params.bcoef_x;
62  float BC_inv_y = 1.0f / _params.bcoef_y;
63 
64  // get latest estimated orientation
65  float q0 = _state.quat_nominal(0);
66  float q1 = _state.quat_nominal(1);
67  float q2 = _state.quat_nominal(2);
68  float q3 = _state.quat_nominal(3);
69 
70  // get latest velocity in earth frame
71  float vn = _state.vel(0);
72  float ve = _state.vel(1);
73  float vd = _state.vel(2);
74 
75  // get latest wind velocity in earth frame
76  float vwn = _state.wind_vel(0);
77  float vwe = _state.wind_vel(1);
78 
79  // predicted specific forces
80  // calculate relative wind velocity in earth frame and rotte into body frame
81  Vector3f rel_wind;
82  rel_wind(0) = vn - vwn;
83  rel_wind(1) = ve - vwe;
84  rel_wind(2) = vd;
85  Dcmf earth_to_body = quat_to_invrotmat(_state.quat_nominal);
86  rel_wind = earth_to_body * rel_wind;
87 
88  // perform sequential fusion of XY specific forces
89  for (uint8_t axis_index = 0; axis_index < 2; axis_index++) {
90  // calculate observation jacobiam and Kalman gain vectors
91  if (axis_index == 0) {
92  // Estimate the airspeed from the measured drag force and ballistic coefficient
93  float mea_acc = _drag_sample_delayed.accelXY(axis_index) - _state.accel_bias(axis_index) / _dt_ekf_avg;
94  float airSpd = sqrtf((2.0f * fabsf(mea_acc)) / (BC_inv_x * rho));
95 
96  // Estimate the derivative of specific force wrt airspeed along the X axis
97  // Limit lower value to prevent arithmetic exceptions
98  float Kacc = fmaxf(1e-1f, rho * BC_inv_x * airSpd);
99 
100  SH_ACC[0] = sq(q0) + sq(q1) - sq(q2) - sq(q3);
101  SH_ACC[1] = vn - vwn;
102  SH_ACC[2] = ve - vwe;
103  SH_ACC[3] = 2.0f*q0*q3 + 2.0f*q1*q2;
104  H_ACC[0] = -Kacc*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd);
105  H_ACC[1] = -Kacc*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd);
106  H_ACC[2] = Kacc*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd);
107  H_ACC[3] = -Kacc*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd);
108  H_ACC[4] = -Kacc*SH_ACC[0];
109  H_ACC[5] = -Kacc*SH_ACC[3];
110  H_ACC[6] = Kacc*(2.0f*q0*q2 - 2.0f*q1*q3);
111  H_ACC[22] = Kacc*SH_ACC[0];
112  H_ACC[23] = Kacc*SH_ACC[3];
113  _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)));
114  if (_drag_innov_var[0] < R_ACC) {
115  return;
116  }
117  SK_ACC[0] = 1.0f/_drag_innov_var[0];
118  SK_ACC[1] = 2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd;
119  SK_ACC[2] = 2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd;
120  SK_ACC[3] = 2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd;
121  SK_ACC[4] = 2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd;
122  SK_ACC[5] = 2.0f*q0*q2 - 2.0f*q1*q3;
123  SK_ACC[6] = SH_ACC[3];
124 // Don't allow modification of any states other than wind velocity at this stage of development - we only need a wind estimate.
125 // 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]);
126 // 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]);
127 // 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]);
128 // 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]);
129 // 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]);
130 // 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]);
131 // 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]);
132 // 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]);
133 // 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]);
134 // 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]);
135 // 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]);
136 // 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]);
137 // 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]);
138 // 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]);
139 // 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]);
140 // 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]);
141 // 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]);
142 // 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]);
143 // 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]);
144 // 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]);
145 // 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]);
146 // 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]);
147  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]);
148  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]);
149 
150  // calculate the predicted acceleration and innovation measured along the X body axis
151  float drag_sign;
152 
153  if (rel_wind(axis_index) >= 0.0f) {
154  drag_sign = 1.0f;
155 
156  } else {
157  drag_sign = -1.0f;
158  }
159 
160  float predAccel = -BC_inv_x * 0.5f * rho * sq(rel_wind(axis_index)) * drag_sign;
161  _drag_innov[axis_index] = predAccel - mea_acc;
162  _drag_test_ratio[axis_index] = sq(_drag_innov[axis_index]) / (25.0f * _drag_innov_var[axis_index]);
163 
164  } else if (axis_index == 1) {
165  // Estimate the airspeed from the measured drag force and ballistic coefficient
166  float mea_acc = _drag_sample_delayed.accelXY(axis_index) - _state.accel_bias(axis_index) / _dt_ekf_avg;
167  float airSpd = sqrtf((2.0f * fabsf(mea_acc)) / (BC_inv_y * rho));
168 
169  // Estimate the derivative of specific force wrt airspeed along the X axis
170  // Limit lower value to prevent arithmetic exceptions
171  float Kacc = fmaxf(1e-1f, rho * BC_inv_y * airSpd);
172 
173  SH_ACC[0] = sq(q0) - sq(q1) + sq(q2) - sq(q3);
174  SH_ACC[1] = vn - vwn;
175  SH_ACC[2] = ve - vwe;
176  H_ACC[0] = -Kacc*(2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd);
177  H_ACC[1] = -Kacc*(2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd);
178  H_ACC[2] = -Kacc*(2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd);
179  H_ACC[3] = Kacc*(2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd);
180  H_ACC[4] = Kacc*(2.0f*q0*q3 - 2.0f*q1*q2);
181  H_ACC[5] = -Kacc*SH_ACC[0];
182  H_ACC[6] = -Kacc*(2.0f*q0*q1 + 2.0f*q2*q3);
183  H_ACC[22] = -2.0f*Kacc*(q0*q3 - q1*q2);
184  H_ACC[23] = Kacc*SH_ACC[0];
185  _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)));
186  if (_drag_innov_var[1] < R_ACC) {
187  // calculation is badly conditioned
188  return;
189  }
190  SK_ACC[0] = 1.0f/_drag_innov_var[1];
191  SK_ACC[1] = 2.0f*q0*SH_ACC[1] + 2.0f*q3*SH_ACC[2] - 2.0f*q2*vd;
192  SK_ACC[2] = 2.0f*q2*SH_ACC[1] - 2.0f*q1*SH_ACC[2] + 2.0f*q0*vd;
193  SK_ACC[3] = 2.0f*q0*SH_ACC[2] - 2.0f*q3*SH_ACC[1] + 2.0f*q1*vd;
194  SK_ACC[4] = 2.0f*q1*SH_ACC[1] + 2.0f*q2*SH_ACC[2] + 2.0f*q3*vd;
195  SK_ACC[5] = 2.0f*q0*q3 - 2.0f*q1*q2;
196  SK_ACC[6] = q0*q3 - q1*q2;
197  SK_ACC[7] = 2.0f*q0*q1 + 2.0f*q2*q3;
198  SK_ACC[8] = SH_ACC[0];
199 // Don't allow modification of any states other than wind velocity at this stage of development - we only need a wind estimate.
200 // 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]);
201 // 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]);
202 // 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]);
203 // 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]);
204 // 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]);
205 // 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]);
206 // 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]);
207 // 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]);
208 // 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]);
209 // 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]);
210 // 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]);
211 // 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]);
212 // 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]);
213 // 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]);
214 // 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]);
215 // 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]);
216 // 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]);
217 // 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]);
218 // 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]);
219 // 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]);
220 // 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]);
221 // 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]);
222  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]);
223  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]);
224 
225  // calculate the predicted acceleration and innovation measured along the Y body axis
226  float drag_sign;
227 
228  if (rel_wind(axis_index) >= 0.0f) {
229  drag_sign = 1.0f;
230 
231  } else {
232  drag_sign = -1.0f;
233  }
234 
235  float predAccel = -BC_inv_y * 0.5f * rho * sq(rel_wind(axis_index)) * drag_sign;
236  _drag_innov[axis_index] = predAccel - mea_acc;
237  _drag_test_ratio[axis_index] = sq(_drag_innov[axis_index]) / (25.0f * _drag_innov_var[axis_index]);
238 
239  }
240 
241  // if the innovation consistency check fails then don't fuse the sample
242  if (_drag_test_ratio[axis_index] <= 1.0f) {
243  // apply covariance correction via P_new = (I -K*H)*P
244  // first calculate expression for KHP
245  // then calculate P - KHP
246  float KHP[_k_num_states][_k_num_states];
247  float KH[9];
248 
249  for (unsigned row = 0; row < _k_num_states; row++) {
250 
251  KH[0] = Kfusion[row] * H_ACC[0];
252  KH[1] = Kfusion[row] * H_ACC[1];
253  KH[2] = Kfusion[row] * H_ACC[2];
254  KH[3] = Kfusion[row] * H_ACC[3];
255  KH[4] = Kfusion[row] * H_ACC[4];
256  KH[5] = Kfusion[row] * H_ACC[5];
257  KH[6] = Kfusion[row] * H_ACC[6];
258  KH[7] = Kfusion[row] * H_ACC[22];
259  KH[8] = Kfusion[row] * H_ACC[23];
260 
261  for (unsigned column = 0; column < _k_num_states; column++) {
262  float tmp = KH[0] * P[0][column];
263  tmp += KH[1] * P[1][column];
264  tmp += KH[2] * P[2][column];
265  tmp += KH[3] * P[3][column];
266  tmp += KH[4] * P[4][column];
267  tmp += KH[5] * P[5][column];
268  tmp += KH[6] * P[6][column];
269  tmp += KH[7] * P[22][column];
270  tmp += KH[8] * P[23][column];
271  KHP[row][column] = tmp;
272  }
273  }
274 
275  // if the covariance correction will result in a negative variance, then
276  // the covariance matrix is unhealthy and must be corrected
277  bool healthy = true;
278 
279  //_fault_status.flags.bad_sideslip = false;
280  for (int i = 0; i < _k_num_states; i++) {
281  if (P[i][i] < KHP[i][i]) {
282  // zero rows and columns
283  zeroRows(P, i, i);
284  zeroCols(P, i, i);
285 
286  //flag as unhealthy
287  healthy = false;
288 
289  // update individual measurement health status
290  //_fault_status.flags.bad_sideslip = true;
291 
292  }
293  }
294 
295  // only apply covariance and state corrections if healthy
296  if (healthy) {
297  // apply the covariance corrections
298  for (unsigned row = 0; row < _k_num_states; row++) {
299  for (unsigned column = 0; column < _k_num_states; column++) {
300  P[row][column] = P[row][column] - KHP[row][column];
301  }
302  }
303 
304  // correct the covariance matrix for gross errors
305  fixCovarianceErrors();
306 
307  // apply the state corrections
308  fuse(Kfusion, _drag_innov[axis_index]);
309 
310  }
311  }
312  }
313 }
EstimatorInterface::quat_to_invrotmat
Matrix3f quat_to_invrotmat(const Quatf &quat)
Definition: ekf_helper.cpp:1358
ecl.h
Adapter / shim layer for system calls needed by ECL.
Ekf::_state
stateSample _state
state struct of the ekf running at the delayed time horizon
Definition: ekf.h:288
matrix::Dcmf
Dcm< float > Dcmf
Definition: Dcm.hpp:185
estimator::stateSample::accel_bias
Vector3f accel_bias
delta velocity bias estimate in m/s
Definition: common.h:372
Ekf::_drag_innov
float _drag_innov[2]
multirotor drag measurement innovation (m/sec**2)
Definition: ekf.h:385
estimator::dragSample::accelXY
Vector2f accelXY
measured specific force along the X and Y body axes (m/sec**2)
Definition: common.h:167
Ekf::P
float P[_k_num_states][_k_num_states]
state covariance matrix
Definition: ekf.h:367
Ekf::zeroCols
void zeroCols(float(&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last)
Definition: ekf_helper.cpp:1249
Ekf::fuse
void fuse(float *K, float innovation)
Definition: ekf_helper.cpp:1201
estimator::parameters::bcoef_x
float bcoef_x
ballistic coefficient along the X-axis (kg/m**2)
Definition: common.h:348
f
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
estimator::parameters::drag_noise
float drag_noise
observation noise variance for drag specific force measurements (m/sec**2)**2
Definition: common.h:347
Ekf::_drag_innov_var
float _drag_innov_var[2]
multirotor drag measurement innovation variance ((m/sec**2)**2)
Definition: ekf.h:386
estimator::stateSample::vel
Vector3f vel
NED velocity in earth frame in m/s.
Definition: common.h:369
Ekf::sq
static constexpr float sq(float var)
Definition: ekf.h:695
Ekf::fixCovarianceErrors
void fixCovarianceErrors()
Definition: covariance.cpp:728
estimator::parameters::bcoef_y
float bcoef_y
ballistic coefficient along the Y-axis (kg/m**2)
Definition: common.h:349
estimator::stateSample::quat_nominal
Quatf quat_nominal
quaternion defining the rotation from body to earth frame
Definition: common.h:368
matrix::Vector3f
Vector3< float > Vector3f
Definition: Vector3.hpp:136
Kfusion
Kfusion[0]
Definition: K_VELX.c:93
Ekf::zeroRows
void zeroRows(float(&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last)
Definition: ekf_helper.cpp:1239
Ekf::fuseDrag
void fuseDrag()
Definition: drag_fusion.cpp:46
estimator::stateSample::wind_vel
Vector2f wind_vel
wind velocity in m/s
Definition: common.h:375
Ekf::_k_num_states
static constexpr uint8_t _k_num_states
number of EKF states
Definition: ekf.h:270
Ekf::_dt_ekf_avg
float _dt_ekf_avg
average update rate of the ekf
Definition: ekf.h:285
ekf.h
Class for core functions for ekf attitude and position estimator.