PX4 Firmware
PX4 Autopilot Software http://px4.io
optflow_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 vel_pos_fusion.cpp
36  * Function for fusing gps and baro measurements/
37  *
38  * @author Paul Riseborough <p_riseborough@live.com.au>
39  * @author Siddharth Bharat Purohit <siddharthbharatpurohit@gmail.com>
40  *
41  */
42 
43 #include "ekf.h"
44 #include <ecl.h>
45 #include <mathlib/mathlib.h>
46 #include <float.h>
47 
48 void Ekf::fuseOptFlow()
49 {
50  float gndclearance = fmaxf(_params.rng_gnd_clearance, 0.1f);
51  float optflow_test_ratio[2] = {0};
52 
53  // get latest estimated orientation
54  float q0 = _state.quat_nominal(0);
55  float q1 = _state.quat_nominal(1);
56  float q2 = _state.quat_nominal(2);
57  float q3 = _state.quat_nominal(3);
58 
59  // get latest velocity in earth frame
60  float vn = _state.vel(0);
61  float ve = _state.vel(1);
62  float vd = _state.vel(2);
63 
64  // calculate the optical flow observation variance
65  float R_LOS = calcOptFlowMeasVar();
66 
67  float H_LOS[2][24] = {}; // Optical flow observation Jacobians
68  float Kfusion[24][2] = {}; // Optical flow Kalman gains
69 
70  // get rotation matrix from earth to body
71  Dcmf earth_to_body = quat_to_invrotmat(_state.quat_nominal);
72 
73  // calculate the sensor position relative to the IMU
74  Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body;
75 
76  // calculate the velocity of the sensor relative to the imu in body frame
77  // Note: _flow_sample_delayed.gyroXYZ is the negative of the body angular velocity, thus use minus sign
78  Vector3f vel_rel_imu_body = cross_product(-_flow_sample_delayed.gyroXYZ / _flow_sample_delayed.dt, pos_offset_body);
79 
80  // calculate the velocity of the sensor in the earth frame
81  Vector3f vel_rel_earth = _state.vel + _R_to_earth * vel_rel_imu_body;
82 
83  // rotate into body frame
84  Vector3f vel_body = earth_to_body * vel_rel_earth;
85 
86  // height above ground of the IMU
87  float heightAboveGndEst = _terrain_vpos - _state.pos(2);
88 
89  // calculate the sensor position relative to the IMU in earth frame
90  Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
91 
92  // calculate the height above the ground of the optical flow camera. Since earth frame is NED
93  // a positive offset in earth frame leads to a smaller height above the ground.
94  heightAboveGndEst -= pos_offset_earth(2);
95 
96  // constrain minimum height above ground
97  heightAboveGndEst = math::max(heightAboveGndEst, gndclearance);
98 
99  // calculate range from focal point to centre of image
100  float range = heightAboveGndEst / earth_to_body(2, 2); // absolute distance to the frame region in view
101 
102  // calculate optical LOS rates using optical flow rates that have had the body angular rate contribution removed
103  // correct for gyro bias errors in the data used to do the motion compensation
104  // Note the sign convention used: A positive LOS rate is a RH rotation of the scene about that axis.
105  Vector2f opt_flow_rate;
106  opt_flow_rate(0) = _flowRadXYcomp(0) / _flow_sample_delayed.dt + _flow_gyro_bias(0);
107  opt_flow_rate(1) = _flowRadXYcomp(1) / _flow_sample_delayed.dt + _flow_gyro_bias(1);
108 
109  if (opt_flow_rate.norm() < _flow_max_rate) {
110  _flow_innov[0] = vel_body(1) / range - opt_flow_rate(0); // flow around the X axis
111  _flow_innov[1] = -vel_body(0) / range - opt_flow_rate(1); // flow around the Y axis
112 
113  } else {
114  return;
115  }
116 
117 
118  // Fuse X and Y axis measurements sequentially assuming observation errors are uncorrelated
119  // Calculate Obser ation Jacobians and Kalman gans for each measurement axis
120  for (uint8_t obs_index = 0; obs_index <= 1; obs_index++) {
121 
122  if (obs_index == 0) {
123 
124  // calculate X axis observation Jacobian
125  float t2 = 1.0f / range;
126  H_LOS[0][0] = t2*(q1*vd*2.0f+q0*ve*2.0f-q3*vn*2.0f);
127  H_LOS[0][1] = t2*(q0*vd*2.0f-q1*ve*2.0f+q2*vn*2.0f);
128  H_LOS[0][2] = t2*(q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f);
129  H_LOS[0][3] = -t2*(q2*vd*-2.0f+q3*ve*2.0f+q0*vn*2.0f);
130  H_LOS[0][4] = -t2*(q0*q3*2.0f-q1*q2*2.0f);
131  H_LOS[0][5] = t2*(q0*q0-q1*q1+q2*q2-q3*q3);
132  H_LOS[0][6] = t2*(q0*q1*2.0f+q2*q3*2.0f);
133 
134  // calculate intermediate variables for the X observation innovatoin variance and Kalman gains
135  float t3 = q1*vd*2.0f;
136  float t4 = q0*ve*2.0f;
137  float t11 = q3*vn*2.0f;
138  float t5 = t3+t4-t11;
139  float t6 = q0*q3*2.0f;
140  float t29 = q1*q2*2.0f;
141  float t7 = t6-t29;
142  float t8 = q0*q1*2.0f;
143  float t9 = q2*q3*2.0f;
144  float t10 = t8+t9;
145  float t12 = P[0][0]*t2*t5;
146  float t13 = q0*vd*2.0f;
147  float t14 = q2*vn*2.0f;
148  float t28 = q1*ve*2.0f;
149  float t15 = t13+t14-t28;
150  float t16 = q3*vd*2.0f;
151  float t17 = q2*ve*2.0f;
152  float t18 = q1*vn*2.0f;
153  float t19 = t16+t17+t18;
154  float t20 = q3*ve*2.0f;
155  float t21 = q0*vn*2.0f;
156  float t30 = q2*vd*2.0f;
157  float t22 = t20+t21-t30;
158  float t23 = q0*q0;
159  float t24 = q1*q1;
160  float t25 = q2*q2;
161  float t26 = q3*q3;
162  float t27 = t23-t24+t25-t26;
163  float t31 = P[1][1]*t2*t15;
164  float t32 = P[6][0]*t2*t10;
165  float t33 = P[1][0]*t2*t15;
166  float t34 = P[2][0]*t2*t19;
167  float t35 = P[5][0]*t2*t27;
168  float t79 = P[4][0]*t2*t7;
169  float t80 = P[3][0]*t2*t22;
170  float t36 = t12+t32+t33+t34+t35-t79-t80;
171  float t37 = t2*t5*t36;
172  float t38 = P[6][1]*t2*t10;
173  float t39 = P[0][1]*t2*t5;
174  float t40 = P[2][1]*t2*t19;
175  float t41 = P[5][1]*t2*t27;
176  float t81 = P[4][1]*t2*t7;
177  float t82 = P[3][1]*t2*t22;
178  float t42 = t31+t38+t39+t40+t41-t81-t82;
179  float t43 = t2*t15*t42;
180  float t44 = P[6][2]*t2*t10;
181  float t45 = P[0][2]*t2*t5;
182  float t46 = P[1][2]*t2*t15;
183  float t47 = P[2][2]*t2*t19;
184  float t48 = P[5][2]*t2*t27;
185  float t83 = P[4][2]*t2*t7;
186  float t84 = P[3][2]*t2*t22;
187  float t49 = t44+t45+t46+t47+t48-t83-t84;
188  float t50 = t2*t19*t49;
189  float t51 = P[6][3]*t2*t10;
190  float t52 = P[0][3]*t2*t5;
191  float t53 = P[1][3]*t2*t15;
192  float t54 = P[2][3]*t2*t19;
193  float t55 = P[5][3]*t2*t27;
194  float t85 = P[4][3]*t2*t7;
195  float t86 = P[3][3]*t2*t22;
196  float t56 = t51+t52+t53+t54+t55-t85-t86;
197  float t57 = P[6][5]*t2*t10;
198  float t58 = P[0][5]*t2*t5;
199  float t59 = P[1][5]*t2*t15;
200  float t60 = P[2][5]*t2*t19;
201  float t61 = P[5][5]*t2*t27;
202  float t88 = P[4][5]*t2*t7;
203  float t89 = P[3][5]*t2*t22;
204  float t62 = t57+t58+t59+t60+t61-t88-t89;
205  float t63 = t2*t27*t62;
206  float t64 = P[6][4]*t2*t10;
207  float t65 = P[0][4]*t2*t5;
208  float t66 = P[1][4]*t2*t15;
209  float t67 = P[2][4]*t2*t19;
210  float t68 = P[5][4]*t2*t27;
211  float t90 = P[4][4]*t2*t7;
212  float t91 = P[3][4]*t2*t22;
213  float t69 = t64+t65+t66+t67+t68-t90-t91;
214  float t70 = P[6][6]*t2*t10;
215  float t71 = P[0][6]*t2*t5;
216  float t72 = P[1][6]*t2*t15;
217  float t73 = P[2][6]*t2*t19;
218  float t74 = P[5][6]*t2*t27;
219  float t93 = P[4][6]*t2*t7;
220  float t94 = P[3][6]*t2*t22;
221  float t75 = t70+t71+t72+t73+t74-t93-t94;
222  float t76 = t2*t10*t75;
223  float t87 = t2*t22*t56;
224  float t92 = t2*t7*t69;
225  float t77 = R_LOS+t37+t43+t50+t63+t76-t87-t92;
226  float t78;
227 
228  // calculate innovation variance for X axis observation and protect against a badly conditioned calculation
229  if (t77 >= R_LOS) {
230  t78 = 1.0f / t77;
231  _flow_innov_var[0] = t77;
232 
233  } else {
234  // we need to reinitialise the covariance matrix and abort this fusion step
235  initialiseCovariance();
236  return;
237  }
238 
239  // calculate Kalman gains for X-axis observation
240  Kfusion[0][0] = t78*(t12-P[0][4]*t2*t7+P[0][1]*t2*t15+P[0][6]*t2*t10+P[0][2]*t2*t19-P[0][3]*t2*t22+P[0][5]*t2*t27);
241  Kfusion[1][0] = t78*(t31+P[1][0]*t2*t5-P[1][4]*t2*t7+P[1][6]*t2*t10+P[1][2]*t2*t19-P[1][3]*t2*t22+P[1][5]*t2*t27);
242  Kfusion[2][0] = t78*(t47+P[2][0]*t2*t5-P[2][4]*t2*t7+P[2][1]*t2*t15+P[2][6]*t2*t10-P[2][3]*t2*t22+P[2][5]*t2*t27);
243  Kfusion[3][0] = t78*(-t86+P[3][0]*t2*t5-P[3][4]*t2*t7+P[3][1]*t2*t15+P[3][6]*t2*t10+P[3][2]*t2*t19+P[3][5]*t2*t27);
244  Kfusion[4][0] = t78*(-t90+P[4][0]*t2*t5+P[4][1]*t2*t15+P[4][6]*t2*t10+P[4][2]*t2*t19-P[4][3]*t2*t22+P[4][5]*t2*t27);
245  Kfusion[5][0] = t78*(t61+P[5][0]*t2*t5-P[5][4]*t2*t7+P[5][1]*t2*t15+P[5][6]*t2*t10+P[5][2]*t2*t19-P[5][3]*t2*t22);
246  Kfusion[6][0] = t78*(t70+P[6][0]*t2*t5-P[6][4]*t2*t7+P[6][1]*t2*t15+P[6][2]*t2*t19-P[6][3]*t2*t22+P[6][5]*t2*t27);
247  Kfusion[7][0] = t78*(P[7][0]*t2*t5-P[7][4]*t2*t7+P[7][1]*t2*t15+P[7][6]*t2*t10+P[7][2]*t2*t19-P[7][3]*t2*t22+P[7][5]*t2*t27);
248  Kfusion[8][0] = t78*(P[8][0]*t2*t5-P[8][4]*t2*t7+P[8][1]*t2*t15+P[8][6]*t2*t10+P[8][2]*t2*t19-P[8][3]*t2*t22+P[8][5]*t2*t27);
249  Kfusion[9][0] = t78*(P[9][0]*t2*t5-P[9][4]*t2*t7+P[9][1]*t2*t15+P[9][6]*t2*t10+P[9][2]*t2*t19-P[9][3]*t2*t22+P[9][5]*t2*t27);
250  Kfusion[10][0] = t78*(P[10][0]*t2*t5-P[10][4]*t2*t7+P[10][1]*t2*t15+P[10][6]*t2*t10+P[10][2]*t2*t19-P[10][3]*t2*t22+P[10][5]*t2*t27);
251  Kfusion[11][0] = t78*(P[11][0]*t2*t5-P[11][4]*t2*t7+P[11][1]*t2*t15+P[11][6]*t2*t10+P[11][2]*t2*t19-P[11][3]*t2*t22+P[11][5]*t2*t27);
252  Kfusion[12][0] = t78*(P[12][0]*t2*t5-P[12][4]*t2*t7+P[12][1]*t2*t15+P[12][6]*t2*t10+P[12][2]*t2*t19-P[12][3]*t2*t22+P[12][5]*t2*t27);
253  Kfusion[13][0] = t78*(P[13][0]*t2*t5-P[13][4]*t2*t7+P[13][1]*t2*t15+P[13][6]*t2*t10+P[13][2]*t2*t19-P[13][3]*t2*t22+P[13][5]*t2*t27);
254  Kfusion[14][0] = t78*(P[14][0]*t2*t5-P[14][4]*t2*t7+P[14][1]*t2*t15+P[14][6]*t2*t10+P[14][2]*t2*t19-P[14][3]*t2*t22+P[14][5]*t2*t27);
255  Kfusion[15][0] = t78*(P[15][0]*t2*t5-P[15][4]*t2*t7+P[15][1]*t2*t15+P[15][6]*t2*t10+P[15][2]*t2*t19-P[15][3]*t2*t22+P[15][5]*t2*t27);
256  Kfusion[16][0] = t78*(P[16][0]*t2*t5-P[16][4]*t2*t7+P[16][1]*t2*t15+P[16][6]*t2*t10+P[16][2]*t2*t19-P[16][3]*t2*t22+P[16][5]*t2*t27);
257  Kfusion[17][0] = t78*(P[17][0]*t2*t5-P[17][4]*t2*t7+P[17][1]*t2*t15+P[17][6]*t2*t10+P[17][2]*t2*t19-P[17][3]*t2*t22+P[17][5]*t2*t27);
258  Kfusion[18][0] = t78*(P[18][0]*t2*t5-P[18][4]*t2*t7+P[18][1]*t2*t15+P[18][6]*t2*t10+P[18][2]*t2*t19-P[18][3]*t2*t22+P[18][5]*t2*t27);
259  Kfusion[19][0] = t78*(P[19][0]*t2*t5-P[19][4]*t2*t7+P[19][1]*t2*t15+P[19][6]*t2*t10+P[19][2]*t2*t19-P[19][3]*t2*t22+P[19][5]*t2*t27);
260  Kfusion[20][0] = t78*(P[20][0]*t2*t5-P[20][4]*t2*t7+P[20][1]*t2*t15+P[20][6]*t2*t10+P[20][2]*t2*t19-P[20][3]*t2*t22+P[20][5]*t2*t27);
261  Kfusion[21][0] = t78*(P[21][0]*t2*t5-P[21][4]*t2*t7+P[21][1]*t2*t15+P[21][6]*t2*t10+P[21][2]*t2*t19-P[21][3]*t2*t22+P[21][5]*t2*t27);
262  Kfusion[22][0] = t78*(P[22][0]*t2*t5-P[22][4]*t2*t7+P[22][1]*t2*t15+P[22][6]*t2*t10+P[22][2]*t2*t19-P[22][3]*t2*t22+P[22][5]*t2*t27);
263  Kfusion[23][0] = t78*(P[23][0]*t2*t5-P[23][4]*t2*t7+P[23][1]*t2*t15+P[23][6]*t2*t10+P[23][2]*t2*t19-P[23][3]*t2*t22+P[23][5]*t2*t27);
264 
265  // run innovation consistency checks
266  optflow_test_ratio[0] = sq(_flow_innov[0]) / (sq(math::max(_params.flow_innov_gate, 1.0f)) * _flow_innov_var[0]);
267 
268  } else if (obs_index == 1) {
269 
270  // calculate Y axis observation Jacobian
271  float t2 = 1.0f / range;
272  H_LOS[1][0] = -t2*(q2*vd*-2.0f+q3*ve*2.0f+q0*vn*2.0f);
273  H_LOS[1][1] = -t2*(q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f);
274  H_LOS[1][2] = t2*(q0*vd*2.0f-q1*ve*2.0f+q2*vn*2.0f);
275  H_LOS[1][3] = -t2*(q1*vd*2.0f+q0*ve*2.0f-q3*vn*2.0f);
276  H_LOS[1][4] = -t2*(q0*q0+q1*q1-q2*q2-q3*q3);
277  H_LOS[1][5] = -t2*(q0*q3*2.0f+q1*q2*2.0f);
278  H_LOS[1][6] = t2*(q0*q2*2.0f-q1*q3*2.0f);
279 
280  // calculate intermediate variables for the Y observation innovatoin variance and Kalman gains
281  float t3 = q3*ve*2.0f;
282  float t4 = q0*vn*2.0f;
283  float t11 = q2*vd*2.0f;
284  float t5 = t3+t4-t11;
285  float t6 = q0*q3*2.0f;
286  float t7 = q1*q2*2.0f;
287  float t8 = t6+t7;
288  float t9 = q0*q2*2.0f;
289  float t28 = q1*q3*2.0f;
290  float t10 = t9-t28;
291  float t12 = P[0][0]*t2*t5;
292  float t13 = q3*vd*2.0f;
293  float t14 = q2*ve*2.0f;
294  float t15 = q1*vn*2.0f;
295  float t16 = t13+t14+t15;
296  float t17 = q0*vd*2.0f;
297  float t18 = q2*vn*2.0f;
298  float t29 = q1*ve*2.0f;
299  float t19 = t17+t18-t29;
300  float t20 = q1*vd*2.0f;
301  float t21 = q0*ve*2.0f;
302  float t30 = q3*vn*2.0f;
303  float t22 = t20+t21-t30;
304  float t23 = q0*q0;
305  float t24 = q1*q1;
306  float t25 = q2*q2;
307  float t26 = q3*q3;
308  float t27 = t23+t24-t25-t26;
309  float t31 = P[1][1]*t2*t16;
310  float t32 = P[5][0]*t2*t8;
311  float t33 = P[1][0]*t2*t16;
312  float t34 = P[3][0]*t2*t22;
313  float t35 = P[4][0]*t2*t27;
314  float t80 = P[6][0]*t2*t10;
315  float t81 = P[2][0]*t2*t19;
316  float t36 = t12+t32+t33+t34+t35-t80-t81;
317  float t37 = t2*t5*t36;
318  float t38 = P[5][1]*t2*t8;
319  float t39 = P[0][1]*t2*t5;
320  float t40 = P[3][1]*t2*t22;
321  float t41 = P[4][1]*t2*t27;
322  float t82 = P[6][1]*t2*t10;
323  float t83 = P[2][1]*t2*t19;
324  float t42 = t31+t38+t39+t40+t41-t82-t83;
325  float t43 = t2*t16*t42;
326  float t44 = P[5][2]*t2*t8;
327  float t45 = P[0][2]*t2*t5;
328  float t46 = P[1][2]*t2*t16;
329  float t47 = P[3][2]*t2*t22;
330  float t48 = P[4][2]*t2*t27;
331  float t79 = P[2][2]*t2*t19;
332  float t84 = P[6][2]*t2*t10;
333  float t49 = t44+t45+t46+t47+t48-t79-t84;
334  float t50 = P[5][3]*t2*t8;
335  float t51 = P[0][3]*t2*t5;
336  float t52 = P[1][3]*t2*t16;
337  float t53 = P[3][3]*t2*t22;
338  float t54 = P[4][3]*t2*t27;
339  float t86 = P[6][3]*t2*t10;
340  float t87 = P[2][3]*t2*t19;
341  float t55 = t50+t51+t52+t53+t54-t86-t87;
342  float t56 = t2*t22*t55;
343  float t57 = P[5][4]*t2*t8;
344  float t58 = P[0][4]*t2*t5;
345  float t59 = P[1][4]*t2*t16;
346  float t60 = P[3][4]*t2*t22;
347  float t61 = P[4][4]*t2*t27;
348  float t88 = P[6][4]*t2*t10;
349  float t89 = P[2][4]*t2*t19;
350  float t62 = t57+t58+t59+t60+t61-t88-t89;
351  float t63 = t2*t27*t62;
352  float t64 = P[5][5]*t2*t8;
353  float t65 = P[0][5]*t2*t5;
354  float t66 = P[1][5]*t2*t16;
355  float t67 = P[3][5]*t2*t22;
356  float t68 = P[4][5]*t2*t27;
357  float t90 = P[6][5]*t2*t10;
358  float t91 = P[2][5]*t2*t19;
359  float t69 = t64+t65+t66+t67+t68-t90-t91;
360  float t70 = t2*t8*t69;
361  float t71 = P[5][6]*t2*t8;
362  float t72 = P[0][6]*t2*t5;
363  float t73 = P[1][6]*t2*t16;
364  float t74 = P[3][6]*t2*t22;
365  float t75 = P[4][6]*t2*t27;
366  float t92 = P[6][6]*t2*t10;
367  float t93 = P[2][6]*t2*t19;
368  float t76 = t71+t72+t73+t74+t75-t92-t93;
369  float t85 = t2*t19*t49;
370  float t94 = t2*t10*t76;
371  float t77 = R_LOS+t37+t43+t56+t63+t70-t85-t94;
372  float t78;
373  // calculate innovation variance for Y axis observation and protect against a badly conditioned calculation
374  if (t77 >= R_LOS) {
375  t78 = 1.0f / t77;
376  _flow_innov_var[1] = t77;
377 
378  } else {
379  // we need to reinitialise the covariance matrix and abort this fusion step
380  initialiseCovariance();
381  return;
382  }
383 
384  // calculate Kalman gains for Y-axis observation
385  Kfusion[0][1] = -t78*(t12+P[0][5]*t2*t8-P[0][6]*t2*t10+P[0][1]*t2*t16-P[0][2]*t2*t19+P[0][3]*t2*t22+P[0][4]*t2*t27);
386  Kfusion[1][1] = -t78*(t31+P[1][0]*t2*t5+P[1][5]*t2*t8-P[1][6]*t2*t10-P[1][2]*t2*t19+P[1][3]*t2*t22+P[1][4]*t2*t27);
387  Kfusion[2][1] = -t78*(-t79+P[2][0]*t2*t5+P[2][5]*t2*t8-P[2][6]*t2*t10+P[2][1]*t2*t16+P[2][3]*t2*t22+P[2][4]*t2*t27);
388  Kfusion[3][1] = -t78*(t53+P[3][0]*t2*t5+P[3][5]*t2*t8-P[3][6]*t2*t10+P[3][1]*t2*t16-P[3][2]*t2*t19+P[3][4]*t2*t27);
389  Kfusion[4][1] = -t78*(t61+P[4][0]*t2*t5+P[4][5]*t2*t8-P[4][6]*t2*t10+P[4][1]*t2*t16-P[4][2]*t2*t19+P[4][3]*t2*t22);
390  Kfusion[5][1] = -t78*(t64+P[5][0]*t2*t5-P[5][6]*t2*t10+P[5][1]*t2*t16-P[5][2]*t2*t19+P[5][3]*t2*t22+P[5][4]*t2*t27);
391  Kfusion[6][1] = -t78*(-t92+P[6][0]*t2*t5+P[6][5]*t2*t8+P[6][1]*t2*t16-P[6][2]*t2*t19+P[6][3]*t2*t22+P[6][4]*t2*t27);
392  Kfusion[7][1] = -t78*(P[7][0]*t2*t5+P[7][5]*t2*t8-P[7][6]*t2*t10+P[7][1]*t2*t16-P[7][2]*t2*t19+P[7][3]*t2*t22+P[7][4]*t2*t27);
393  Kfusion[8][1] = -t78*(P[8][0]*t2*t5+P[8][5]*t2*t8-P[8][6]*t2*t10+P[8][1]*t2*t16-P[8][2]*t2*t19+P[8][3]*t2*t22+P[8][4]*t2*t27);
394  Kfusion[9][1] = -t78*(P[9][0]*t2*t5+P[9][5]*t2*t8-P[9][6]*t2*t10+P[9][1]*t2*t16-P[9][2]*t2*t19+P[9][3]*t2*t22+P[9][4]*t2*t27);
395  Kfusion[10][1] = -t78*(P[10][0]*t2*t5+P[10][5]*t2*t8-P[10][6]*t2*t10+P[10][1]*t2*t16-P[10][2]*t2*t19+P[10][3]*t2*t22+P[10][4]*t2*t27);
396  Kfusion[11][1] = -t78*(P[11][0]*t2*t5+P[11][5]*t2*t8-P[11][6]*t2*t10+P[11][1]*t2*t16-P[11][2]*t2*t19+P[11][3]*t2*t22+P[11][4]*t2*t27);
397  Kfusion[12][1] = -t78*(P[12][0]*t2*t5+P[12][5]*t2*t8-P[12][6]*t2*t10+P[12][1]*t2*t16-P[12][2]*t2*t19+P[12][3]*t2*t22+P[12][4]*t2*t27);
398  Kfusion[13][1] = -t78*(P[13][0]*t2*t5+P[13][5]*t2*t8-P[13][6]*t2*t10+P[13][1]*t2*t16-P[13][2]*t2*t19+P[13][3]*t2*t22+P[13][4]*t2*t27);
399  Kfusion[14][1] = -t78*(P[14][0]*t2*t5+P[14][5]*t2*t8-P[14][6]*t2*t10+P[14][1]*t2*t16-P[14][2]*t2*t19+P[14][3]*t2*t22+P[14][4]*t2*t27);
400  Kfusion[15][1] = -t78*(P[15][0]*t2*t5+P[15][5]*t2*t8-P[15][6]*t2*t10+P[15][1]*t2*t16-P[15][2]*t2*t19+P[15][3]*t2*t22+P[15][4]*t2*t27);
401  Kfusion[16][1] = -t78*(P[16][0]*t2*t5+P[16][5]*t2*t8-P[16][6]*t2*t10+P[16][1]*t2*t16-P[16][2]*t2*t19+P[16][3]*t2*t22+P[16][4]*t2*t27);
402  Kfusion[17][1] = -t78*(P[17][0]*t2*t5+P[17][5]*t2*t8-P[17][6]*t2*t10+P[17][1]*t2*t16-P[17][2]*t2*t19+P[17][3]*t2*t22+P[17][4]*t2*t27);
403  Kfusion[18][1] = -t78*(P[18][0]*t2*t5+P[18][5]*t2*t8-P[18][6]*t2*t10+P[18][1]*t2*t16-P[18][2]*t2*t19+P[18][3]*t2*t22+P[18][4]*t2*t27);
404  Kfusion[19][1] = -t78*(P[19][0]*t2*t5+P[19][5]*t2*t8-P[19][6]*t2*t10+P[19][1]*t2*t16-P[19][2]*t2*t19+P[19][3]*t2*t22+P[19][4]*t2*t27);
405  Kfusion[20][1] = -t78*(P[20][0]*t2*t5+P[20][5]*t2*t8-P[20][6]*t2*t10+P[20][1]*t2*t16-P[20][2]*t2*t19+P[20][3]*t2*t22+P[20][4]*t2*t27);
406  Kfusion[21][1] = -t78*(P[21][0]*t2*t5+P[21][5]*t2*t8-P[21][6]*t2*t10+P[21][1]*t2*t16-P[21][2]*t2*t19+P[21][3]*t2*t22+P[21][4]*t2*t27);
407  Kfusion[22][1] = -t78*(P[22][0]*t2*t5+P[22][5]*t2*t8-P[22][6]*t2*t10+P[22][1]*t2*t16-P[22][2]*t2*t19+P[22][3]*t2*t22+P[22][4]*t2*t27);
408  Kfusion[23][1] = -t78*(P[23][0]*t2*t5+P[23][5]*t2*t8-P[23][6]*t2*t10+P[23][1]*t2*t16-P[23][2]*t2*t19+P[23][3]*t2*t22+P[23][4]*t2*t27);
409 
410  // run innovation consistency check
411  optflow_test_ratio[1] = sq(_flow_innov[1]) / (sq(math::max(_params.flow_innov_gate, 1.0f)) * _flow_innov_var[1]);
412 
413  }
414  }
415 
416  // record the innovation test pass/fail
417  bool flow_fail = false;
418 
419  for (uint8_t obs_index = 0; obs_index <= 1; obs_index++) {
420  if (optflow_test_ratio[obs_index] > 1.0f) {
421  flow_fail = true;
422  _innov_check_fail_status.value |= (1 << (obs_index + 10));
423 
424  } else {
425  _innov_check_fail_status.value &= ~(1 << (obs_index + 10));
426 
427  }
428  }
429 
430  // if either axis fails we abort the fusion
431  if (flow_fail) {
432  return;
433 
434  }
435 
436  for (uint8_t obs_index = 0; obs_index <= 1; obs_index++) {
437 
438  // copy the Kalman gain vector for the axis we are fusing
439  float gain[24];
440 
441  for (unsigned row = 0; row <= 23; row++) {
442  gain[row] = Kfusion[row][obs_index];
443  }
444 
445  // apply covariance correction via P_new = (I -K*H)*P
446  // first calculate expression for KHP
447  // then calculate P - KHP
448  float KHP[_k_num_states][_k_num_states];
449  float KH[7];
450 
451  for (unsigned row = 0; row < _k_num_states; row++) {
452 
453  KH[0] = gain[row] * H_LOS[obs_index][0];
454  KH[1] = gain[row] * H_LOS[obs_index][1];
455  KH[2] = gain[row] * H_LOS[obs_index][2];
456  KH[3] = gain[row] * H_LOS[obs_index][3];
457  KH[4] = gain[row] * H_LOS[obs_index][4];
458  KH[5] = gain[row] * H_LOS[obs_index][5];
459  KH[6] = gain[row] * H_LOS[obs_index][6];
460 
461  for (unsigned column = 0; column < _k_num_states; column++) {
462  float tmp = KH[0] * P[0][column];
463  tmp += KH[1] * P[1][column];
464  tmp += KH[2] * P[2][column];
465  tmp += KH[3] * P[3][column];
466  tmp += KH[4] * P[4][column];
467  tmp += KH[5] * P[5][column];
468  tmp += KH[6] * P[6][column];
469  KHP[row][column] = tmp;
470  }
471  }
472 
473  // if the covariance correction will result in a negative variance, then
474  // the covariance matrix is unhealthy and must be corrected
475  bool healthy = true;
476  _fault_status.flags.bad_optflow_X = false;
477  _fault_status.flags.bad_optflow_Y = false;
478 
479  for (int i = 0; i < _k_num_states; i++) {
480  if (P[i][i] < KHP[i][i]) {
481  // zero rows and columns
482  zeroRows(P, i, i);
483  zeroCols(P, i, i);
484 
485  //flag as unhealthy
486  healthy = false;
487 
488  // update individual measurement health status
489  if (obs_index == 0) {
490  _fault_status.flags.bad_optflow_X = true;
491 
492  } else if (obs_index == 1) {
493  _fault_status.flags.bad_optflow_Y = true;
494  }
495  }
496  }
497 
498  // only apply covariance and state corrections if healthy
499  if (healthy) {
500  // apply the covariance corrections
501  for (unsigned row = 0; row < _k_num_states; row++) {
502  for (unsigned column = 0; column < _k_num_states; column++) {
503  P[row][column] = P[row][column] - KHP[row][column];
504  }
505  }
506 
507  // correct the covariance matrix for gross errors
508  fixCovarianceErrors();
509 
510  // apply the state corrections
511  fuse(gain, _flow_innov[obs_index]);
512 
513  _time_last_of_fuse = _time_last_imu;
514  }
515  }
516 }
517 
518 void Ekf::get_flow_innov(float flow_innov[2])
519 {
520  memcpy(flow_innov, _flow_innov, sizeof(_flow_innov));
521 }
522 
523 
524 void Ekf::get_flow_innov_var(float flow_innov_var[2])
525 {
526  memcpy(flow_innov_var, _flow_innov_var, sizeof(_flow_innov_var));
527 }
528 
529 void Ekf::get_drag_innov(float drag_innov[2])
530 {
531  memcpy(drag_innov, _drag_innov, sizeof(_drag_innov));
532 }
533 
534 
535 void Ekf::get_drag_innov_var(float drag_innov_var[2])
536 {
537  memcpy(drag_innov_var, _drag_innov_var, sizeof(_drag_innov_var));
538 }
539 
540 // calculate optical flow body angular rate compensation
541 // returns false if bias corrected body rate data is unavailable
542 bool Ekf::calcOptFlowBodyRateComp()
543 {
544  // reset the accumulators if the time interval is too large
545  if (_delta_time_of > 1.0f) {
546  _imu_del_ang_of.setZero();
547  _delta_time_of = 0.0f;
548  return false;
549  }
550 
551  bool use_flow_sensor_gyro = ISFINITE(_flow_sample_delayed.gyroXYZ(0)) && ISFINITE(_flow_sample_delayed.gyroXYZ(1)) && ISFINITE(_flow_sample_delayed.gyroXYZ(2));
552 
553  if (use_flow_sensor_gyro) {
554 
555  // if accumulation time differences are not excessive and accumulation time is adequate
556  // compare the optical flow and and navigation rate data and calculate a bias error
557  if ((fabsf(_delta_time_of - _flow_sample_delayed.dt) < 0.1f) && (_delta_time_of > FLT_EPSILON)) {
558  // calculate a reference angular rate
559  Vector3f reference_body_rate;
560  reference_body_rate = _imu_del_ang_of * (1.0f / _delta_time_of);
561 
562  // calculate the optical flow sensor measured body rate
563  Vector3f of_body_rate;
564  of_body_rate = _flow_sample_delayed.gyroXYZ * (1.0f / _flow_sample_delayed.dt);
565 
566  // calculate the bias estimate using a combined LPF and spike filter
567  _flow_gyro_bias(0) = 0.99f * _flow_gyro_bias(0) + 0.01f * math::constrain((of_body_rate(0) - reference_body_rate(0)),
568  -0.1f, 0.1f);
569  _flow_gyro_bias(1) = 0.99f * _flow_gyro_bias(1) + 0.01f * math::constrain((of_body_rate(1) - reference_body_rate(1)),
570  -0.1f, 0.1f);
571  _flow_gyro_bias(2) = 0.99f * _flow_gyro_bias(2) + 0.01f * math::constrain((of_body_rate(2) - reference_body_rate(2)),
572  -0.1f, 0.1f);
573  }
574 
575  } else {
576  // Use the EKF gyro data if optical flow sensor gyro data is not available
577  // for clarification of the sign see definition of flowSample and imuSample in common.h
578  _flow_sample_delayed.gyroXYZ = - _imu_del_ang_of;
579  _flow_gyro_bias.zero();
580  }
581 
582  // reset the accumulators
583  _imu_del_ang_of.setZero();
584  _delta_time_of = 0.0f;
585  return true;
586 }
587 
588 // calculate the measurement variance for the optical flow sensor (rad/sec)^2
589 float Ekf::calcOptFlowMeasVar()
590 {
591  // calculate the observation noise variance - scaling noise linearly across flow quality range
592  float R_LOS_best = fmaxf(_params.flow_noise, 0.05f);
593  float R_LOS_worst = fmaxf(_params.flow_noise_qual_min, 0.05f);
594 
595  // calculate a weighting that varies between 1 when flow quality is best and 0 when flow quality is worst
596  float weighting = (255.0f - (float)_params.flow_qual_min);
597 
598  if (weighting >= 1.0f) {
599  weighting = math::constrain(((float)_flow_sample_delayed.quality - (float)_params.flow_qual_min) / weighting, 0.0f,
600  1.0f);
601 
602  } else {
603  weighting = 0.0f;
604  }
605 
606  // take the weighted average of the observation noise for the best and wort flow quality
607  float R_LOS = sq(R_LOS_best * weighting + R_LOS_worst * (1.0f - weighting));
608 
609  return R_LOS;
610 }
EstimatorInterface::quat_to_invrotmat
Matrix3f quat_to_invrotmat(const Quatf &quat)
Definition: ekf_helper.cpp:1358
t38
t38
Definition: K_LOSX.c:39
math::constrain
constexpr _Tp constrain(_Tp val, _Tp min_val, _Tp max_val)
Definition: Limits.hpp:66
t34
t34
Definition: calcH_YAWGPS.c:33
t8
t8
Definition: calcH_YAW312.c:9
ecl.h
Adapter / shim layer for system calls needed by ECL.
t94
t94
Definition: K_LOSX.c:87
Ekf::_state
stateSample _state
state struct of the ekf running at the delayed time horizon
Definition: ekf.h:288
t12
t12
Definition: calcH_YAW312.c:11
t54
t54
Definition: K_LOSX.c:59
t24
t24
Definition: calcH_YAWGPS.c:23
Ekf::_flow_innov
float _flow_innov[2]
flow measurement innovation (rad/sec)
Definition: ekf.h:392
t52
t52
Definition: K_LOSX.c:57
t9
t9
Definition: calcH_YAW312.c:1
t59
t59
Definition: K_LOSX.c:66
t77
t77
Definition: K_LOSX.c:92
t51
t51
Definition: K_LOSX.c:56
t43
t43
Definition: K_LOSX.c:46
estimator::fault_status_u::bad_optflow_X
bool bad_optflow_X
7 - true if fusion of the optical flow X axis has encountered a numerical error
Definition: common.h:387
estimator::parameters::flow_noise
float flow_noise
observation noise for optical flow LOS rate measurements (rad/sec)
Definition: common.h:308
t4
t4
Definition: calcH_YAW312.c:5
matrix::Dcmf
Dcm< float > Dcmf
Definition: Dcm.hpp:185
t28
t28
Definition: calcH_YAWGPS.c:27
t87
t87
Definition: K_LOSX.c:90
Ekf::_imu_del_ang_of
Vector3f _imu_del_ang_of
bias corrected delta angle measurements accumulated across the same time frame as the optical flow ra...
Definition: ekf.h:395
t36
t36
Definition: K_LOSX.c:37
t81
t81
Definition: K_LOSX.c:43
Ekf::_drag_innov
float _drag_innov[2]
multirotor drag measurement innovation (m/sec**2)
Definition: ekf.h:385
t6
t6
Definition: calcH_YAW312.c:7
Ekf::get_drag_innov_var
void get_drag_innov_var(float drag_innov_var[2]) override
Definition: optflow_fusion.cpp:535
t80
t80
Definition: K_LOSX.c:36
t90
t90
Definition: K_LOSX.c:78
Ekf::_flow_gyro_bias
Vector3f _flow_gyro_bias
bias errors in optical flow sensor rate gyro outputs (rad/sec)
Definition: ekf.h:394
t5
t5
Definition: calcH_YAW312.c:6
t14
t14
Definition: calcH_YAW312.c:13
Ekf::initialiseCovariance
void initialiseCovariance()
Definition: covariance.cpp:49
EstimatorInterface::cross_product
Vector3f cross_product(const Vector3f &vecIn1, const Vector3f &vecIn2)
Definition: ekf_helper.cpp:1347
t70
t70
Definition: K_LOSX.c:81
t44
t44
Definition: K_LOSX.c:47
H_LOS
H_LOS[0]
Definition: LOSX.c:130
t19
t19
Definition: calcH_YAWGPS.c:17
estimator::fault_status_u::flags
struct estimator::fault_status_u::@57 flags
t63
t63
Definition: K_LOSX.c:72
t57
t57
Definition: K_LOSX.c:64
estimator::flowSample::gyroXYZ
Vector3f gyroXYZ
measured delta angle of the inertial frame about the body axes obtained from rate gyro measurements (...
Definition: common.h:150
t29
t29
Definition: calcH_YAWGPS.c:28
EstimatorInterface::_flow_max_rate
float _flow_max_rate
maximum angular flow rate that the optical flow sensor can measure (rad/s)
Definition: estimator_interface.h:460
t21
t21
Definition: calcH_YAWGPS.c:20
Ekf::get_flow_innov_var
void get_flow_innov_var(float flow_innov_var[2]) override
Definition: optflow_fusion.cpp:524
t85
t85
Definition: K_LOSX.c:61
estimator::flowSample::dt
float dt
amount of integration time (sec)
Definition: common.h:151
FLT_EPSILON
#define FLT_EPSILON
Definition: stdlib_imports.hpp:26
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
t27
t27
Definition: calcH_YAWGPS.c:26
t67
t67
Definition: K_LOSX.c:76
Ekf::_terrain_vpos
float _terrain_vpos
estimated vertical position of the terrain underneath the vehicle in local NED frame (m) ...
Definition: ekf.h:457
t42
t42
Definition: K_LOSX.c:45
t73
t73
Definition: K_LOSX.c:84
t72
t72
Definition: K_LOSX.c:83
t39
t39
Definition: K_LOSX.c:40
Ekf::_time_last_of_fuse
uint64_t _time_last_of_fuse
time the last fusion of optical flow measurements were performed (uSec)
Definition: ekf.h:334
t33
t33
Definition: calcH_YAWGPS.c:32
t88
t88
Definition: K_LOSX.c:69
t83
t83
Definition: K_LOSX.c:52
t22
t22
Definition: calcH_YAWGPS.c:21
t31
t31
Definition: calcH_YAWGPS.c:30
Ekf::fuse
void fuse(float *K, float innovation)
Definition: ekf_helper.cpp:1201
t10
t10
Definition: calcH_YAW312.c:2
Ekf::_R_to_earth
Dcmf _R_to_earth
transformation matrix from body frame to earth frame from last EKF prediction
Definition: ekf.h:348
t55
t55
Definition: K_LOSX.c:60
t15
t15
Definition: calcH_YAWGPS.c:14
t71
t71
Definition: K_LOSX.c:82
t41
t41
Definition: K_LOSX.c:42
t16
t16
Definition: calcH_YAWGPS.c:15
t86
t86
Definition: K_LOSX.c:62
t20
t20
Definition: calcH_YAWGPS.c:18
estimator::parameters::flow_innov_gate
float flow_innov_gate
optical flow fusion innovation consistency gate size (STD)
Definition: common.h:311
Ekf::_flowRadXYcomp
Vector2f _flowRadXYcomp
measured delta angle of the image about the X and Y body axes after removal of body rotation (rad)...
Definition: ekf.h:400
t92
t92
Definition: K_LOSX.c:91
t37
t37
Definition: K_LOSX.c:38
estimator::parameters::flow_pos_body
Vector3f flow_pos_body
xyz position of range sensor focal point in body frame (m)
Definition: common.h:328
t26
t26
Definition: calcH_YAWGPS.c:25
f
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
matrix::Vector2f
Vector2< float > Vector2f
Definition: Vector2.hpp:69
Ekf::_drag_innov_var
float _drag_innov_var[2]
multirotor drag measurement innovation variance ((m/sec**2)**2)
Definition: ekf.h:386
estimator::parameters::rng_gnd_clearance
float rng_gnd_clearance
minimum valid value for range when on ground (m)
Definition: common.h:291
t13
t13
Definition: calcH_YAW312.c:12
estimator::flowSample::quality
uint8_t quality
quality indicator between 0 and 255
Definition: common.h:148
t35
t35
Definition: calcH_YAWGPS.c:34
t62
t62
Definition: K_LOSX.c:71
estimator::stateSample::vel
Vector3f vel
NED velocity in earth frame in m/s.
Definition: common.h:369
t61
t61
Definition: K_LOSX.c:68
t69
t69
Definition: K_LOSX.c:80
t47
t47
Definition: K_LOSX.c:50
t82
t82
Definition: K_LOSX.c:44
t64
t64
Definition: K_LOSX.c:73
Ekf::sq
static constexpr float sq(float var)
Definition: ekf.h:695
t30
t30
Definition: calcH_YAWGPS.c:29
EstimatorInterface::_fault_status
fault_status_u _fault_status
Definition: estimator_interface.h:551
t60
t60
Definition: K_LOSX.c:67
Ekf::_delta_time_of
float _delta_time_of
time in sec that _imu_del_ang_of was accumulated over (sec)
Definition: ekf.h:396
t32
t32
Definition: calcH_YAWGPS.c:31
Ekf::fixCovarianceErrors
void fixCovarianceErrors()
Definition: covariance.cpp:728
t76
t76
Definition: K_LOSX.c:89
estimator::parameters::flow_noise_qual_min
float flow_noise_qual_min
observation noise for optical flow LOS rate measurements when flow sensor quality is at the minimum u...
Definition: common.h:309
Ekf::get_flow_innov
void get_flow_innov(float flow_innov[2]) override
Definition: optflow_fusion.cpp:518
estimator::stateSample::quat_nominal
Quatf quat_nominal
quaternion defining the rotation from body to earth frame
Definition: common.h:368
t11
t11
Definition: calcH_YAW312.c:10
t18
t18
Definition: calcH_YAWGPS.c:19
estimator::stateSample::pos
Vector3f pos
NED position in earth frame in m.
Definition: common.h:370
t49
t49
Definition: K_LOSX.c:54
EstimatorInterface::_innov_check_fail_status
innovation_fault_status_u _innov_check_fail_status
Definition: estimator_interface.h:494
t93
t93
Definition: K_LOSX.c:86
matrix::Vector3f
Vector3< float > Vector3f
Definition: Vector3.hpp:136
t74
t74
Definition: K_LOSX.c:85
t79
t79
Definition: K_LOSX.c:35
t68
t68
Definition: K_LOSX.c:77
t91
t91
Definition: K_LOSX.c:79
Kfusion
Kfusion[0]
Definition: K_VELX.c:93
t66
t66
Definition: K_LOSX.c:75
math::max
constexpr _Tp max(_Tp a, _Tp b)
Definition: Limits.hpp:60
Ekf::_flow_innov_var
float _flow_innov_var[2]
flow innovation variance ((rad/sec)**2)
Definition: ekf.h:393
Ekf::zeroRows
void zeroRows(float(&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last)
Definition: ekf_helper.cpp:1239
t2
t2
Definition: calcH_YAW312.c:3
t84
t84
Definition: K_LOSX.c:53
estimator::innovation_fault_status_u::value
uint16_t value
Definition: common.h:417
t53
t53
Definition: K_LOSX.c:58
t75
t75
Definition: K_LOSX.c:88
t78
t78
Definition: K_LOSX.c:93
estimator::parameters::imu_pos_body
Vector3f imu_pos_body
xyz position of IMU in body frame (m)
Definition: common.h:325
t65
t65
Definition: K_LOSX.c:74
estimator::fault_status_u::bad_optflow_Y
bool bad_optflow_Y
8 - true if fusion of the optical flow Y axis has encountered a numerical error
Definition: common.h:388
t56
t56
Definition: K_LOSX.c:63
estimator::parameters::flow_qual_min
int32_t flow_qual_min
minimum acceptable quality integer from the flow sensor
Definition: common.h:310
t46
t46
Definition: K_LOSX.c:49
t48
t48
Definition: K_LOSX.c:51
t45
t45
Definition: K_LOSX.c:48
t17
t17
Definition: calcH_YAWGPS.c:16
t23
t23
Definition: calcH_YAWGPS.c:22
t7
t7
Definition: calcH_YAW312.c:8
Ekf::fuseOptFlow
void fuseOptFlow()
Definition: optflow_fusion.cpp:48
Ekf::_k_num_states
static constexpr uint8_t _k_num_states
number of EKF states
Definition: ekf.h:270
t89
t89
Definition: K_LOSX.c:70
Ekf::calcOptFlowBodyRateComp
bool calcOptFlowBodyRateComp()
Definition: optflow_fusion.cpp:542
Ekf::get_drag_innov
void get_drag_innov(float drag_innov[2]) override
Definition: optflow_fusion.cpp:529
ekf.h
Class for core functions for ekf attitude and position estimator.
t58
t58
Definition: K_LOSX.c:65
t40
t40
Definition: K_LOSX.c:41
Ekf::calcOptFlowMeasVar
float calcOptFlowMeasVar()
Definition: optflow_fusion.cpp:589
t3
t3
Definition: calcH_YAW312.c:4
ISFINITE
#define ISFINITE(x)
Definition: ecl.h:100
t50
t50
Definition: K_LOSX.c:55
t25
t25
Definition: calcH_YAWGPS.c:24