Line data Source code
1 : /*
2 : Definition of functions used to provide a backup estimate of yaw angle
3 : that doesn't use a magnetometer. Uses a bank of 3-state EKF's organised
4 : as a Gaussian sum filter where states are velocity N,E (m/s) and yaw angle (rad)
5 :
6 : Written by Paul Riseborough <[email protected]>
7 :
8 : This program is free software: you can redistribute it and/or modify
9 : it under the terms of the GNU General Public License as published by
10 : the Free Software Foundation, either version 3 of the License, or
11 : (at your option) any later version.
12 :
13 : This program is distributed in the hope that it will be useful,
14 : but WITHOUT ANY WARRANTY; without even the implied warranty of
15 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16 : GNU General Public License for more details.
17 :
18 : You should have received a copy of the GNU General Public License
19 : along with this program. If not, see <http://www.gnu.org/licenses/>.
20 : */
21 :
22 : #include <AP_HAL/AP_HAL.h>
23 :
24 : #include "AP_NavEKF/EKFGSF_yaw.h"
25 : #include <AP_AHRS/AP_AHRS.h>
26 : #include <GCS_MAVLink/GCS.h>
27 :
28 29856 : EKFGSF_yaw::EKFGSF_yaw() {};
29 :
30 23005950 : void EKFGSF_yaw::update(const Vector3F &delAng,
31 : const Vector3F &delVel,
32 : const ftype delAngDT,
33 : const ftype delVelDT,
34 : bool runEKF,
35 : ftype TAS)
36 : {
37 :
38 : // copy to class variables
39 23005950 : delta_angle = delAng;
40 23005950 : delta_velocity = delVel;
41 23005950 : angle_dt = delAngDT;
42 23005950 : velocity_dt = delVelDT;
43 23005950 : run_ekf_gsf = runEKF;
44 23005950 : true_airspeed = TAS;
45 :
46 : // Calculate a low pass filtered acceleration vector that will be used to keep the AHRS tilt aligned
47 : // The time constant of the filter is a fixed ratio relative to the time constant of the AHRS tilt correction loop
48 23005950 : const ftype filter_coef = fminF(EKFGSF_accelFiltRatio * delVelDT * EKFGSF_tiltGain, 1.0f);
49 23005950 : const Vector3F accel = delVel / fmaxF(delVelDT, 0.001f);
50 23005950 : ahrs_accel = ahrs_accel * (1.0f - filter_coef) + accel * filter_coef;
51 :
52 : // Iniitialise states and only when acceleration is close to 1g to prevent vehicle movement casuing a large initial tilt error
53 23005950 : if (!ahrs_tilt_aligned) {
54 28643 : const ftype accel_norm_sq = accel.length_squared();
55 : const ftype upper_accel_limit = GRAVITY_MSS * 1.1f;
56 : const ftype lower_accel_limit = GRAVITY_MSS * 0.9f;
57 28643 : const bool ok_to_align = ((accel_norm_sq > lower_accel_limit * lower_accel_limit &&
58 : accel_norm_sq < upper_accel_limit * upper_accel_limit));
59 : if (ok_to_align) {
60 3627 : alignTilt();
61 3627 : ahrs_tilt_aligned = true;
62 3627 : ahrs_accel = accel;
63 : }
64 28643 : return;
65 : }
66 :
67 : // Calculate common variables used by the AHRS prediction models
68 22977307 : ahrs_accel_norm = ahrs_accel.length();
69 :
70 : // Calculate AHRS acceleration fusion gain using a quadratic weighting function that is unity at 1g
71 : // and zero at the min and max g limits. This reduces the effect of large g transients on the attitude
72 : // estimates.
73 22977307 : ftype EKFGSF_ahrs_ng = ahrs_accel_norm / GRAVITY_MSS;
74 22977307 : if (EKFGSF_ahrs_ng > 1.0f) {
75 19475551 : if (is_positive(true_airspeed)) {
76 : // When flying in fixed wing mode we need to allow for more positive g due to coordinated turns
77 : // Gain varies from unity at 1g to zero at 2g
78 3452148 : accel_gain = EKFGSF_tiltGain * sq(MAX(2.0f - EKFGSF_ahrs_ng, 0.0f));
79 16023403 : } else if (EKFGSF_ahrs_ng <= 1.5f) {
80 : // Gain varies from unity at 1g to zero at 1.5g
81 15831052 : accel_gain = EKFGSF_tiltGain * sq(3.0f - 2.0f * EKFGSF_ahrs_ng);
82 : } else {
83 : // Gain is zero above max g
84 192351 : accel_gain = 0.0f;
85 : }
86 3501756 : } else if (EKFGSF_ahrs_ng > 0.5f) {
87 : // Gain varies from zero at 0.5g to unity at 1g
88 3448951 : accel_gain = EKFGSF_tiltGain * sq(2.0f * EKFGSF_ahrs_ng - 1.0f);
89 : } else {
90 : // Gain is zero below min g
91 52805 : accel_gain = 0.0f;
92 : }
93 :
94 : // Always run the AHRS prediction cycle for each model
95 137863842 : for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx++) {
96 114886535 : predict(mdl_idx);
97 : }
98 :
99 22977307 : if (vel_fuse_running && !run_ekf_gsf) {
100 1396 : vel_fuse_running = false;
101 : }
102 :
103 : // Calculate a composite yaw as a weighted average of the states for each model.
104 : // To avoid issues with angle wrapping, the yaw state is converted to a vector with legnth
105 : // equal to the weighting value before it is summed.
106 22977307 : Vector2F yaw_vector;
107 137863842 : for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx++) {
108 114886535 : yaw_vector[0] += GSF.weights[mdl_idx] * cosF(EKF[mdl_idx].X[2]);
109 114886535 : yaw_vector[1] += GSF.weights[mdl_idx] * sinF(EKF[mdl_idx].X[2]);
110 : }
111 22977307 : GSF.yaw = atan2F(yaw_vector[1],yaw_vector[0]);
112 :
113 : // Example for future reference showing how a full GSF covariance matrix could be calculated if required
114 : /*
115 : memset(&GSF.P, 0, sizeof(GSF.P));
116 : for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx++) {
117 : ftype delta[3];
118 : for (uint8_t row = 0; row < 3; row++) {
119 : delta[row] = EKF[mdl_idx].X[row] - GSF.X[row];
120 : }
121 : for (uint8_t row = 0; row < 3; row++) {
122 : for (uint8_t col = 0; col < 3; col++) {
123 : GSF.P[row][col] += GSF.weights[mdl_idx] * (EKF[mdl_idx].P[row][col] + delta[row] * delta[col]);
124 : }
125 : }
126 : }
127 : */
128 :
129 22977307 : GSF.yaw_variance = 0.0f;
130 137863842 : for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx++) {
131 114886535 : ftype yawDelta = wrap_PI(EKF[mdl_idx].X[2] - GSF.yaw);
132 114886535 : GSF.yaw_variance += GSF.weights[mdl_idx] * (EKF[mdl_idx].P[2][2] + sq(yawDelta));
133 : }
134 : }
135 :
136 1232687 : void EKFGSF_yaw::fuseVelData(const Vector2F &vel, const ftype velAcc)
137 : {
138 : // convert reported accuracy to a variance, but limit lower value to protect algorithm stability
139 1232687 : const ftype velObsVar = sq(fmaxF(velAcc, 0.5f));
140 :
141 : // The 3-state EKF models only run when flying to avoid corrupted estimates due to operator handling and GPS interference
142 1232687 : if (run_ekf_gsf) {
143 1232687 : if (!vel_fuse_running) {
144 : // Perform in-flight alignment
145 3657 : resetEKFGSF();
146 21942 : for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx++) {
147 : // Use the firstGPS measurement to set the velocities and corresponding variances
148 18285 : EKF[mdl_idx].X[0] = vel[0];
149 18285 : EKF[mdl_idx].X[1] = vel[1];
150 18285 : EKF[mdl_idx].P[0][0] = velObsVar;
151 18285 : EKF[mdl_idx].P[1][1] = velObsVar;
152 : }
153 3657 : alignYaw();
154 3657 : vel_fuse_running = true;
155 : } else {
156 : ftype total_w = 0.0f;
157 : ftype newWeight[(uint8_t)N_MODELS_EKFGSF];
158 : bool state_update_failed = false;
159 7374180 : for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx++) {
160 : // Update states and covariances using GPS NE velocity measurements fused as direct state observations
161 6145150 : if (!correct(mdl_idx, vel, velObsVar)) {
162 : state_update_failed = true;
163 : }
164 : }
165 :
166 1229030 : if (!state_update_failed) {
167 : // Calculate weighting for each model assuming a normal error distribution
168 : const ftype min_weight = 1e-5f;
169 1229030 : n_clips = 0;
170 7374180 : for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx++) {
171 6145150 : newWeight[mdl_idx] = gaussianDensity(mdl_idx) * GSF.weights[mdl_idx];
172 6145150 : if (newWeight[mdl_idx] < min_weight) {
173 319311 : n_clips++;
174 319311 : newWeight[mdl_idx] = min_weight;
175 : }
176 6145150 : total_w += newWeight[mdl_idx];
177 : }
178 :
179 : // Normalise the sum of weights to unity
180 : // Reset the filters if all weights have underflowed due to excessive innovation variances
181 1229030 : if (vel_fuse_running && n_clips < N_MODELS_EKFGSF) {
182 1227278 : ftype total_w_inv = 1.0f / total_w;
183 7363668 : for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx++) {
184 6136390 : GSF.weights[mdl_idx] = newWeight[mdl_idx] * total_w_inv;
185 : }
186 : } else {
187 1752 : resetEKFGSF();
188 : }
189 : }
190 : }
191 : }
192 1232687 : }
193 :
194 114886535 : void EKFGSF_yaw::predictAHRS(const uint8_t mdl_idx)
195 : {
196 : // Generate attitude solution using simple complementary filter for the selected model
197 :
198 : // Calculate 'k' unit vector of earth frame rotated into body frame
199 114886535 : const Vector3F k{AHRS[mdl_idx].R[2][0], AHRS[mdl_idx].R[2][1], AHRS[mdl_idx].R[2][2]};
200 :
201 : // Calculate angular rate vector in rad/sec averaged across last sample interval
202 114886535 : const Vector3F ang_rate_delayed_raw { delta_angle / angle_dt };
203 :
204 : // Perform angular rate correction using accel data and reduce correction as accel magnitude moves away from 1 g (reduces drift when vehicle picked up and moved).
205 : // During fixed wing flight, compensate for centripetal acceleration assuming coordinated turns and X axis forward
206 :
207 114886535 : Vector3F tilt_error_gyro_correction; // (rad/sec)
208 :
209 114886535 : if (accel_gain > 0.0f) {
210 :
211 113408375 : Vector3F accel = ahrs_accel;
212 :
213 113408375 : if (is_positive(true_airspeed)) {
214 : // Calculate centripetal acceleration in body frame from cross product of body rate and body frame airspeed vector
215 : // NOTE: this assumes X axis is aligned with airspeed vector
216 : const Vector3F centripetal_accel_vec_bf {
217 : 0.0f,
218 21774595 : ang_rate_delayed_raw[2] * true_airspeed,
219 21774595 : - ang_rate_delayed_raw[1] * true_airspeed
220 21774595 : };
221 : // Correct measured accel for centripetal acceleration
222 21774595 : accel -= centripetal_accel_vec_bf;
223 : }
224 :
225 113408375 : tilt_error_gyro_correction = (k % accel) * (accel_gain / ahrs_accel_norm);
226 :
227 : }
228 :
229 : // Gyro bias estimation
230 : const ftype gyro_bias_limit = radians(5.0f);
231 114886535 : const ftype spinRate_squared = ang_rate_delayed_raw.length_squared();
232 114886535 : if (spinRate_squared < sq(0.175f)) {
233 95509410 : AHRS[mdl_idx].gyro_bias -= tilt_error_gyro_correction * (EKFGSF_gyroBiasGain * angle_dt);
234 :
235 : // sanity check
236 95509410 : if (AHRS[mdl_idx].gyro_bias.is_nan()) {
237 0 : AHRS[mdl_idx].gyro_bias.zero();
238 : }
239 :
240 382037640 : for (uint8_t i = 0; i < 3; i++) {
241 286528230 : AHRS[mdl_idx].gyro_bias[i] = constrain_ftype(AHRS[mdl_idx].gyro_bias[i], -gyro_bias_limit, gyro_bias_limit);
242 : }
243 : }
244 :
245 : // Calculate the corrected body frame rotation vector for the last sample interval and apply to the rotation matrix
246 114886535 : const Vector3F ahrs_delta_angle = delta_angle + (tilt_error_gyro_correction - AHRS[mdl_idx].gyro_bias) * angle_dt;
247 114886535 : AHRS[mdl_idx].R = updateRotMat(AHRS[mdl_idx].R, ahrs_delta_angle);
248 :
249 114886535 : }
250 :
251 3627 : void EKFGSF_yaw::alignTilt()
252 : {
253 : // Rotation matrix is constructed directly from acceleration measurement and will be the same for
254 : // all models so only need to calculate it once. Assumptions are:
255 : // 1) Yaw angle is zero - yaw is aligned later for each model when velocity fusion commences.
256 : // 2) The vehicle is not accelerating so all of the measured acceleration is due to gravity.
257 :
258 : // Calculate earth frame Down axis unit vector rotated into body frame
259 3627 : Vector3F down_in_bf = -delta_velocity;
260 3627 : down_in_bf.normalize();
261 :
262 : // Calculate earth frame North axis unit vector rotated into body frame, orthogonal to 'down_in_bf'
263 : // * operator is overloaded to provide a dot product
264 3627 : const Vector3F i_vec_bf(1.0f,0.0f,0.0f);
265 3627 : Vector3F north_in_bf = i_vec_bf - down_in_bf * (i_vec_bf * down_in_bf);
266 3627 : north_in_bf.normalize();
267 :
268 : // Calculate earth frame East axis unit vector rotated into body frame, orthogonal to 'down_in_bf' and 'north_in_bf'
269 : // % operator is overloaded to provide a cross product
270 3627 : const Vector3F east_in_bf = down_in_bf % north_in_bf;
271 :
272 : // Each column in a rotation matrix from earth frame to body frame represents the projection of the
273 : // corresponding earth frame unit vector rotated into the body frame, eg 'north_in_bf' would be the first column.
274 : // We need the rotation matrix from body frame to earth frame so the earth frame unit vectors rotated into body
275 : // frame are copied into corresponding rows instead to create the transpose.
276 3627 : Matrix3F R;
277 14508 : for (uint8_t col=0; col<3; col++) {
278 10881 : R[0][col] = north_in_bf[col];
279 10881 : R[1][col] = east_in_bf[col];
280 10881 : R[2][col] = down_in_bf[col];
281 : }
282 :
283 : // record alignment
284 21762 : for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx++) {
285 18135 : AHRS[mdl_idx].R = R;
286 18135 : AHRS[mdl_idx].aligned = true;
287 : }
288 3627 : }
289 :
290 3657 : void EKFGSF_yaw::alignYaw()
291 : {
292 : // Align yaw angle for each model
293 21942 : for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx++) {
294 18285 : if (fabsF(AHRS[mdl_idx].R[2][0]) < fabsF(AHRS[mdl_idx].R[2][1])) {
295 : // get the roll, pitch, yaw estimates from the rotation matrix using a 321 Tait-Bryan rotation sequence
296 : ftype roll,pitch,yaw;
297 10090 : AHRS[mdl_idx].R.to_euler(&roll, &pitch, &yaw);
298 :
299 : // set the yaw angle
300 10090 : yaw = wrap_PI(EKF[mdl_idx].X[2]);
301 :
302 : // update the body to earth frame rotation matrix
303 10090 : AHRS[mdl_idx].R.from_euler(roll, pitch, yaw);
304 :
305 : } else {
306 : // Calculate the 312 Tait-Bryan rotation sequence that rotates from earth to body frame
307 8195 : Vector3F euler312 = AHRS[mdl_idx].R.to_euler312();
308 8195 : euler312[2] = wrap_PI(EKF[mdl_idx].X[2]); // first rotation (yaw) taken from EKF model state
309 :
310 : // update the body to earth frame rotation matrix
311 8195 : AHRS[mdl_idx].R.from_euler312(euler312[0], euler312[1], euler312[2]);
312 :
313 : }
314 : }
315 3657 : }
316 :
317 : // predict states and covariance for specified model index
318 114886535 : void EKFGSF_yaw::predict(const uint8_t mdl_idx)
319 : {
320 : // generate an attitude reference using IMU data
321 114886535 : predictAHRS(mdl_idx);
322 :
323 : // we don't start running the EKF part of the algorithm until there are regular velocity observations
324 114886535 : if (!vel_fuse_running) {
325 34231590 : return;
326 : }
327 :
328 : // Calculate the yaw state using a projection onto the horizontal that avoids gimbal lock
329 80654945 : if (fabsF(AHRS[mdl_idx].R[2][0]) < fabsF(AHRS[mdl_idx].R[2][1])) {
330 : // use 321 Tait-Bryan rotation to define yaw state
331 37589525 : EKF[mdl_idx].X[2] = atan2F(AHRS[mdl_idx].R[1][0], AHRS[mdl_idx].R[0][0]);
332 : } else {
333 : // use 312 Tait-Bryan rotation to define yaw state
334 43065420 : EKF[mdl_idx].X[2] = atan2F(-AHRS[mdl_idx].R[0][1], AHRS[mdl_idx].R[1][1]); // first rotation (yaw)
335 : }
336 :
337 : // calculate delta velocity in a horizontal front-right frame
338 80654945 : const Vector3F del_vel_NED = AHRS[mdl_idx].R * delta_velocity;
339 80654945 : const ftype dvx = del_vel_NED[0] * cosF(EKF[mdl_idx].X[2]) + del_vel_NED[1] * sinF(EKF[mdl_idx].X[2]);
340 80654945 : const ftype dvy = - del_vel_NED[0] * sinF(EKF[mdl_idx].X[2]) + del_vel_NED[1] * cosF(EKF[mdl_idx].X[2]);
341 :
342 : // sum delta velocities in earth frame:
343 80654945 : EKF[mdl_idx].X[0] += del_vel_NED[0];
344 80654945 : EKF[mdl_idx].X[1] += del_vel_NED[1];
345 :
346 : // predict covariance - autocode from https://github.com/priseborough/3_state_filter/blob/flightLogReplay-wip/calcPupdate.txt
347 :
348 : // Local short variable name copies required for readability
349 : // Compiler might be smart enough to optimise these out
350 80654945 : const ftype P00 = EKF[mdl_idx].P[0][0];
351 80654945 : const ftype P01 = EKF[mdl_idx].P[0][1];
352 80654945 : const ftype P02 = EKF[mdl_idx].P[0][2];
353 80654945 : const ftype P10 = EKF[mdl_idx].P[1][0];
354 80654945 : const ftype P11 = EKF[mdl_idx].P[1][1];
355 80654945 : const ftype P12 = EKF[mdl_idx].P[1][2];
356 80654945 : const ftype P20 = EKF[mdl_idx].P[2][0];
357 80654945 : const ftype P21 = EKF[mdl_idx].P[2][1];
358 80654945 : const ftype P22 = EKF[mdl_idx].P[2][2];
359 :
360 : // Use fixed values for delta velocity and delta angle process noise variances
361 80654945 : const ftype dvxVar = sq(EKFGSF_accelNoise * velocity_dt); // variance of forward delta velocity - (m/s)^2
362 : const ftype dvyVar = dvxVar; // variance of right delta velocity - (m/s)^2
363 80654945 : const ftype dazVar = sq(EKFGSF_gyroNoise * angle_dt); // variance of yaw delta angle - rad^2
364 :
365 : const ftype t2 = sinF(EKF[mdl_idx].X[2]);
366 : const ftype t3 = cosF(EKF[mdl_idx].X[2]);
367 80654945 : const ftype t4 = dvy*t3;
368 80654945 : const ftype t5 = dvx*t2;
369 80654945 : const ftype t6 = t4+t5;
370 80654945 : const ftype t8 = P22*t6;
371 80654945 : const ftype t7 = P02-t8;
372 80654945 : const ftype t9 = dvx*t3;
373 80654945 : const ftype t11 = dvy*t2;
374 80654945 : const ftype t10 = t9-t11;
375 80654945 : const ftype t12 = dvxVar*t2*t3;
376 80654945 : const ftype t13 = t2*t2;
377 80654945 : const ftype t14 = t3*t3;
378 80654945 : const ftype t15 = P22*t10;
379 80654945 : const ftype t16 = P12+t15;
380 :
381 : const ftype min_var = 1e-6f;
382 80654945 : EKF[mdl_idx].P[0][0] = fmaxF(P00-P20*t6+dvxVar*t14+dvyVar*t13-t6*t7, min_var);
383 80654945 : EKF[mdl_idx].P[0][1] = P01+t12-P21*t6+t7*t10-dvyVar*t2*t3;
384 80654945 : EKF[mdl_idx].P[0][2] = t7;
385 80654945 : EKF[mdl_idx].P[1][0] = P10+t12+P20*t10-t6*t16-dvyVar*t2*t3;
386 80654945 : EKF[mdl_idx].P[1][1] = fmaxF(P11+P21*t10+dvxVar*t13+dvyVar*t14+t10*t16, min_var);
387 80654945 : EKF[mdl_idx].P[1][2] = t16;
388 80654945 : EKF[mdl_idx].P[2][0] = P20-t8;
389 80654945 : EKF[mdl_idx].P[2][1] = P21+t15;
390 80654945 : EKF[mdl_idx].P[2][2] = fmaxF(P22+dazVar, min_var);
391 :
392 : // force symmetry
393 80654945 : forceSymmetry(mdl_idx);
394 : }
395 :
396 : // Update EKF states and covariance for specified model index using velocity measurement
397 : // Returns false if the state and covariance correction failed
398 6145150 : bool EKFGSF_yaw::correct(const uint8_t mdl_idx, const Vector2F &vel, const ftype velObsVar)
399 : {
400 : // calculate velocity observation innovations
401 6145150 : EKF[mdl_idx].innov[0] = EKF[mdl_idx].X[0] - vel[0];
402 6145150 : EKF[mdl_idx].innov[1] = EKF[mdl_idx].X[1] - vel[1];
403 :
404 : // copy covariance matrix to temporary variables
405 6145150 : const ftype P00 = EKF[mdl_idx].P[0][0];
406 6145150 : const ftype P01 = EKF[mdl_idx].P[0][1];
407 6145150 : const ftype P02 = EKF[mdl_idx].P[0][2];
408 6145150 : const ftype P10 = EKF[mdl_idx].P[1][0];
409 6145150 : const ftype P11 = EKF[mdl_idx].P[1][1];
410 6145150 : const ftype P12 = EKF[mdl_idx].P[1][2];
411 6145150 : const ftype P20 = EKF[mdl_idx].P[2][0];
412 6145150 : const ftype P21 = EKF[mdl_idx].P[2][1];
413 6145150 : const ftype P22 = EKF[mdl_idx].P[2][2];
414 :
415 : // calculate innovation variance
416 6145150 : EKF[mdl_idx].S[0][0] = P00 + velObsVar;
417 6145150 : EKF[mdl_idx].S[1][1] = P11 + velObsVar;
418 6145150 : EKF[mdl_idx].S[0][1] = P01;
419 6145150 : EKF[mdl_idx].S[1][0] = P10;
420 :
421 : // Perform a chi-square innovation consistency test and calculate a compression scale factor that limits the magnitude of innovations to 5-sigma
422 6145150 : ftype S_det_inv = (EKF[mdl_idx].S[0][0]*EKF[mdl_idx].S[1][1] - EKF[mdl_idx].S[0][1]*EKF[mdl_idx].S[1][0]);
423 : ftype innov_comp_scale_factor = 1.0f;
424 6145150 : if (fabsF(S_det_inv) > 1E-6f) {
425 : // Calculate elements for innovation covariance inverse matrix assuming symmetry
426 6145150 : S_det_inv = 1.0f / S_det_inv;
427 6145150 : const ftype S_inv_NN = EKF[mdl_idx].S[1][1] * S_det_inv;
428 6145150 : const ftype S_inv_EE = EKF[mdl_idx].S[0][0] * S_det_inv;
429 6145150 : const ftype S_inv_NE = EKF[mdl_idx].S[0][1] * S_det_inv;
430 :
431 : // The following expression was derived symbolically from test ratio = transpose(innovation) * inverse(innovation variance) * innovation = [1x2] * [2,2] * [2,1] = [1,1]
432 6145150 : const ftype test_ratio = EKF[mdl_idx].innov[0]*(EKF[mdl_idx].innov[0]*S_inv_NN + EKF[mdl_idx].innov[1]*S_inv_NE) + EKF[mdl_idx].innov[1]*(EKF[mdl_idx].innov[0]*S_inv_NE + EKF[mdl_idx].innov[1]*S_inv_EE);
433 :
434 : // If the test ratio is greater than 25 (5 Sigma) then reduce the length of the innovation vector to clip it at 5-Sigma
435 : // This protects from large measurement spikes
436 6145150 : if (test_ratio > 25.0f) {
437 87799 : innov_comp_scale_factor = sqrtF(25.0f / test_ratio);
438 : }
439 : } else {
440 : // skip this fusion step because calculation is badly conditioned
441 : return false;
442 : }
443 :
444 : // calculate Kalman gain K and covariance matrix P
445 : // autocode from https://github.com/priseborough/3_state_filter/blob/flightLogReplay-wip/calcK.txt
446 : // and https://github.com/priseborough/3_state_filter/blob/flightLogReplay-wip/calcPmat.txt
447 6145150 : const ftype t2 = P00*velObsVar;
448 6145150 : const ftype t3 = P11*velObsVar;
449 6145150 : const ftype t4 = velObsVar*velObsVar;
450 6145150 : const ftype t5 = P00*P11;
451 : const ftype t9 = P01*P10;
452 6145150 : const ftype t6 = t2+t3+t4+t5-t9;
453 : ftype t7;
454 6145150 : if (fabsF(t6) > 1e-6f) {
455 6145150 : t7 = 1.0f/t6;
456 : } else {
457 : // skip this fusion step
458 : return false;
459 : }
460 : const ftype t8 = P11+velObsVar;
461 : const ftype t10 = P00+velObsVar;
462 : ftype K[3][2];
463 :
464 6145150 : K[0][0] = -P01*P10*t7+P00*t7*t8;
465 6145150 : K[0][1] = -P00*P01*t7+P01*t7*t10;
466 6145150 : K[1][0] = -P10*P11*t7+P10*t7*t8;
467 6145150 : K[1][1] = -P01*P10*t7+P11*t7*t10;
468 6145150 : K[2][0] = -P10*P21*t7+P20*t7*t8;
469 6145150 : K[2][1] = -P01*P20*t7+P21*t7*t10;
470 :
471 6145150 : const ftype t11 = P00*P01*t7;
472 : const ftype t15 = P01*t7*t10;
473 6145150 : const ftype t12 = t11-t15;
474 : const ftype t13 = P01*P10*t7;
475 : const ftype t16 = P00*t7*t8;
476 6145150 : const ftype t14 = t13-t16;
477 6145150 : const ftype t17 = t8*t12;
478 6145150 : const ftype t18 = P01*t14;
479 6145150 : const ftype t19 = t17+t18;
480 6145150 : const ftype t20 = t10*t14;
481 6145150 : const ftype t21 = P10*t12;
482 6145150 : const ftype t22 = t20+t21;
483 : const ftype t27 = P11*t7*t10;
484 6145150 : const ftype t23 = t13-t27;
485 6145150 : const ftype t24 = P10*P11*t7;
486 : const ftype t26 = P10*t7*t8;
487 6145150 : const ftype t25 = t24-t26;
488 6145150 : const ftype t28 = t8*t23;
489 6145150 : const ftype t29 = P01*t25;
490 6145150 : const ftype t30 = t28+t29;
491 6145150 : const ftype t31 = t10*t25;
492 6145150 : const ftype t32 = P10*t23;
493 6145150 : const ftype t33 = t31+t32;
494 6145150 : const ftype t34 = P01*P20*t7;
495 : const ftype t38 = P21*t7*t10;
496 6145150 : const ftype t35 = t34-t38;
497 6145150 : const ftype t36 = P10*P21*t7;
498 : const ftype t39 = P20*t7*t8;
499 6145150 : const ftype t37 = t36-t39;
500 6145150 : const ftype t40 = t8*t35;
501 6145150 : const ftype t41 = P01*t37;
502 6145150 : const ftype t42 = t40+t41;
503 6145150 : const ftype t43 = t10*t37;
504 6145150 : const ftype t44 = P10*t35;
505 6145150 : const ftype t45 = t43+t44;
506 :
507 : const ftype min_var = 1e-6f;
508 6145150 : EKF[mdl_idx].P[0][0] = fmaxF(P00-t12*t19-t14*t22, min_var);
509 6145150 : EKF[mdl_idx].P[0][1] = P01-t19*t23-t22*t25;
510 6145150 : EKF[mdl_idx].P[0][2] = P02-t19*t35-t22*t37;
511 6145150 : EKF[mdl_idx].P[1][0] = P10-t12*t30-t14*t33;
512 6145150 : EKF[mdl_idx].P[1][1] = fmaxF(P11-t23*t30-t25*t33, min_var);
513 6145150 : EKF[mdl_idx].P[1][2] = P12-t30*t35-t33*t37;
514 6145150 : EKF[mdl_idx].P[2][0] = P20-t12*t42-t14*t45;
515 6145150 : EKF[mdl_idx].P[2][1] = P21-t23*t42-t25*t45;
516 6145150 : EKF[mdl_idx].P[2][2] = fmaxF(P22-t35*t42-t37*t45, min_var);
517 :
518 : // force symmetry
519 6145150 : forceSymmetry(mdl_idx);
520 :
521 : // Apply state corrections and capture change in yaw angle
522 6145150 : const ftype yaw_prev = EKF[mdl_idx].X[2];
523 18435450 : for (uint8_t obs_index = 0; obs_index < 2; obs_index++) {
524 : // apply the state corrections including the compression scale factor
525 49161200 : for (unsigned row = 0; row < 3; row++) {
526 36870900 : EKF[mdl_idx].X[row] -= K[row][obs_index] * EKF[mdl_idx].innov[obs_index] * innov_comp_scale_factor;
527 : }
528 : }
529 6145150 : const ftype yaw_delta = EKF[mdl_idx].X[2] - yaw_prev;
530 :
531 : // apply the change in yaw angle to the AHRS taking advantage of sparseness in the yaw rotation matrix
532 6145150 : const ftype cos_yaw = cosF(yaw_delta);
533 6145150 : const ftype sin_yaw = sinF(yaw_delta);
534 : ftype R_prev[2][3];
535 6145150 : memcpy(&R_prev, &AHRS[mdl_idx].R, sizeof(R_prev)); // copy first two rows from 3x3
536 6145150 : AHRS[mdl_idx].R[0][0] = R_prev[0][0] * cos_yaw - R_prev[1][0] * sin_yaw;
537 6145150 : AHRS[mdl_idx].R[0][1] = R_prev[0][1] * cos_yaw - R_prev[1][1] * sin_yaw;
538 6145150 : AHRS[mdl_idx].R[0][2] = R_prev[0][2] * cos_yaw - R_prev[1][2] * sin_yaw;
539 6145150 : AHRS[mdl_idx].R[1][0] = R_prev[0][0] * sin_yaw + R_prev[1][0] * cos_yaw;
540 6145150 : AHRS[mdl_idx].R[1][1] = R_prev[0][1] * sin_yaw + R_prev[1][1] * cos_yaw;
541 6145150 : AHRS[mdl_idx].R[1][2] = R_prev[0][2] * sin_yaw + R_prev[1][2] * cos_yaw;
542 :
543 6145150 : return true;
544 : }
545 :
546 5409 : void EKFGSF_yaw::resetEKFGSF()
547 : {
548 5409 : memset(&GSF, 0, sizeof(GSF));
549 5409 : vel_fuse_running = false;
550 5409 : run_ekf_gsf = false;
551 :
552 5409 : memset(&EKF, 0, sizeof(EKF));
553 : const ftype yaw_increment = M_2PI / (ftype)N_MODELS_EKFGSF;
554 32454 : for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx++) {
555 : // evenly space initial yaw estimates in the region between +-Pi
556 27045 : EKF[mdl_idx].X[2] = -M_PI + (0.5f * yaw_increment) + ((ftype)mdl_idx * yaw_increment);
557 :
558 : // All filter models start with the same weight
559 27045 : GSF.weights[mdl_idx] = 1.0f / (ftype)N_MODELS_EKFGSF;
560 :
561 : // Use half yaw interval for yaw uncertainty as that is the maximum that the best model can be away from truth
562 27045 : GSF.yaw_variance = sq(0.5f * yaw_increment);
563 27045 : EKF[mdl_idx].P[2][2] = GSF.yaw_variance;
564 : }
565 5409 : }
566 :
567 : // returns the probability of a selected model output assuming a gaussian error distribution
568 6145150 : ftype EKFGSF_yaw::gaussianDensity(const uint8_t mdl_idx) const
569 : {
570 6145150 : const ftype t2 = EKF[mdl_idx].S[0][0] * EKF[mdl_idx].S[1][1];
571 6145150 : const ftype t5 = EKF[mdl_idx].S[0][1] * EKF[mdl_idx].S[1][0];
572 6145150 : const ftype t3 = t2 - t5; // determinant
573 6145150 : const ftype t4 = 1.0f / MAX(t3, 1e-12f); // determinant inverse
574 :
575 : // inv(S)
576 : ftype invMat[2][2];
577 6145150 : invMat[0][0] = t4 * EKF[mdl_idx].S[1][1];
578 6145150 : invMat[1][1] = t4 * EKF[mdl_idx].S[0][0];
579 6145150 : invMat[0][1] = - t4 * EKF[mdl_idx].S[0][1];
580 6145150 : invMat[1][0] = - t4 * EKF[mdl_idx].S[1][0];
581 :
582 : // inv(S) * innovation
583 : ftype tempVec[2];
584 6145150 : tempVec[0] = invMat[0][0] * EKF[mdl_idx].innov[0] + invMat[0][1] * EKF[mdl_idx].innov[1];
585 6145150 : tempVec[1] = invMat[1][0] * EKF[mdl_idx].innov[0] + invMat[1][1] * EKF[mdl_idx].innov[1];
586 :
587 : // transpose(innovation) * inv(S) * innovation
588 6145150 : ftype normDist = tempVec[0] * EKF[mdl_idx].innov[0] + tempVec[1] * EKF[mdl_idx].innov[1];
589 :
590 : // convert from a normalised variance to a probability assuming a Gaussian distribution
591 6145150 : normDist = expf(-0.5f * normDist);
592 6145150 : normDist *= sqrtF(t4)/ M_2PI;
593 6145150 : return normDist;
594 : }
595 :
596 86800095 : void EKFGSF_yaw::forceSymmetry(const uint8_t mdl_idx)
597 : {
598 86800095 : ftype P01 = 0.5f * (EKF[mdl_idx].P[0][1] + EKF[mdl_idx].P[1][0]);
599 86800095 : ftype P02 = 0.5f * (EKF[mdl_idx].P[0][2] + EKF[mdl_idx].P[2][0]);
600 86800095 : ftype P12 = 0.5f * (EKF[mdl_idx].P[1][2] + EKF[mdl_idx].P[2][1]);
601 86800095 : EKF[mdl_idx].P[0][1] = EKF[mdl_idx].P[1][0] = P01;
602 86800095 : EKF[mdl_idx].P[0][2] = EKF[mdl_idx].P[2][0] = P02;
603 86800095 : EKF[mdl_idx].P[1][2] = EKF[mdl_idx].P[2][1] = P12;
604 86800095 : }
605 :
606 : // Apply a body frame delta angle to the body to earth frame rotation matrix using a small angle approximation
607 114886535 : Matrix3F EKFGSF_yaw::updateRotMat(const Matrix3F &R, const Vector3F &g) const
608 : {
609 114886535 : Matrix3F ret = R;
610 114886535 : ret[0][0] += R[0][1] * g[2] - R[0][2] * g[1];
611 114886535 : ret[0][1] += R[0][2] * g[0] - R[0][0] * g[2];
612 114886535 : ret[0][2] += R[0][0] * g[1] - R[0][1] * g[0];
613 114886535 : ret[1][0] += R[1][1] * g[2] - R[1][2] * g[1];
614 114886535 : ret[1][1] += R[1][2] * g[0] - R[1][0] * g[2];
615 114886535 : ret[1][2] += R[1][0] * g[1] - R[1][1] * g[0];
616 114886535 : ret[2][0] += R[2][1] * g[2] - R[2][2] * g[1];
617 114886535 : ret[2][1] += R[2][2] * g[0] - R[2][0] * g[2];
618 114886535 : ret[2][2] += R[2][0] * g[1] - R[2][1] * g[0];
619 :
620 : // Renormalise rows
621 : ftype rowLengthSq;
622 459546140 : for (uint8_t r = 0; r < 3; r++) {
623 344659605 : rowLengthSq = ret[r][0] * ret[r][0] + ret[r][1] * ret[r][1] + ret[r][2] * ret[r][2];
624 344659605 : if (is_positive(rowLengthSq)) {
625 : // Use linear approximation for inverse sqrt taking advantage of the row length being close to 1.0
626 344659605 : const ftype rowLengthInv = 1.5f - 0.5f * rowLengthSq;
627 : Vector3F &row = ret[r];
628 344659605 : row *= rowLengthInv;
629 : }
630 : }
631 :
632 114886535 : return ret;
633 : }
634 :
635 : // returns true if a yaw estimate is available. yaw and its variance
636 : // is returned, as well as the number of models which are *not* being
637 : // used to snthesise the yaw.
638 1012328 : bool EKFGSF_yaw::getYawData(ftype &yaw, ftype &yawVariance, uint8_t *_n_clips) const
639 : {
640 1012328 : if (!vel_fuse_running) {
641 : return false;
642 : }
643 1010038 : yaw = GSF.yaw;
644 1010038 : yawVariance = GSF.yaw_variance;
645 1010038 : if (_n_clips != nullptr) {
646 479 : *_n_clips = n_clips;
647 : }
648 : return true;
649 : }
650 :
651 343950 : bool EKFGSF_yaw::getVelInnovLength(ftype &velInnovLength) const
652 : {
653 343950 : if (!vel_fuse_running) {
654 : return false;
655 : }
656 343950 : velInnovLength = 0.0f;
657 2063700 : for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx++) {
658 1719750 : velInnovLength += GSF.weights[mdl_idx] * sqrtF((sq(EKF[mdl_idx].innov[0]) + sq(EKF[mdl_idx].innov[1])));
659 : }
660 : return true;
661 : }
662 :
663 2545 : void EKFGSF_yaw::setGyroBias(Vector3f &gyroBias)
664 : {
665 15270 : for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx++) {
666 12725 : AHRS[mdl_idx].gyro_bias = gyroBias.toftype();
667 : }
668 2545 : }
|
