Da Vinci Firmware 1
Firmware for the DaVinci-M rocket avionics board.
Loading...
Searching...
No Matches
mekf.c
Go to the documentation of this file.
1/*
2 * Academic License - for use in teaching, academic research, and meeting
3 * course requirements at degree granting institutions only. Not for
4 * government, commercial, or other organizational use.
5 * File: mekf.c
6 *
7 * MATLAB Coder version : 24.2
8 * C/C++ source code generated on : 05-May-2025 18:59:11
9 */
10
11/* Include Files */
12#include "mekf.h"
13#include "eye.h"
14#include "mpower.h"
15#include "norm.h"
16#include "rt_nonfinite.h"
17#include "rt_nonfinite.h"
18#include <math.h>
19#include <string.h>
20
21/* Function Declarations */
22static double rt_powd_snf(double u0, double u1);
23
24/* Function Definitions */
25/*
26 * Arguments : double u0
27 * double u1
28 * Return Type : double
29 */
30static double rt_powd_snf(double u0, double u1)
31{
32 double y;
33 if (rtIsNaN(u0) || rtIsNaN(u1)) {
34 y = rtNaN;
35 } else {
36 double d;
37 y = fabs(u0);
38 d = fabs(u1);
39 if (rtIsInf(u1)) {
40 if (y == 1.0) {
41 y = 1.0;
42 } else if (y > 1.0) {
43 if (u1 > 0.0) {
44 y = rtInf;
45 } else {
46 y = 0.0;
47 }
48 } else if (u1 > 0.0) {
49 y = 0.0;
50 } else {
51 y = rtInf;
52 }
53 } else if (d == 0.0) {
54 y = 1.0;
55 } else if (d == 1.0) {
56 if (u1 > 0.0) {
57 y = u0;
58 } else {
59 y = 1.0 / u0;
60 }
61 } else if (u1 == 2.0) {
62 y = u0 * u0;
63 } else if ((u1 == 0.5) && (u0 >= 0.0)) {
64 y = sqrt(u0);
65 } else if ((u0 < 0.0) && (u1 > floor(u1))) {
66 y = rtNaN;
67 } else {
68 y = pow(u0, u1);
69 }
70 }
71 return y;
72}
73
74/*
75 * MULTIPLICATIVE EXTENDED KALMAN FILTER
76 *
77 * Arguments : const double q[4]
78 * const double beta[3]
79 * double P[36]
80 * const double omega[3]
81 * const double b_B[3]
82 * double height_AGL
83 * double q_new[4]
84 * double beta_new[3]
85 * double P_new[36]
86 * Return Type : void
87 */
88void mekf(const double q[4], const double beta[3], double P[36],
89 const double omega[3], const double b_B[3], double height_AGL,
90 double q_new[4], double beta_new[3], double P_new[36])
91{
92 static const double Q[36] = {4.3956900000000333E-11,
93 0.0,
94 0.0,
95 -5.0000000000000008E-23,
96 -0.0,
97 -0.0,
98 0.0,
99 4.3956900000000333E-11,
100 0.0,
101 -0.0,
102 -5.0000000000000008E-23,
103 -0.0,
104 0.0,
105 0.0,
106 4.3956900000000333E-11,
107 -0.0,
108 -0.0,
109 -5.0000000000000008E-23,
110 -5.0000000000000008E-23,
111 -0.0,
112 -0.0,
113 1.0000000000000001E-20,
114 0.0,
115 0.0,
116 -0.0,
117 -5.0000000000000008E-23,
118 -0.0,
119 0.0,
120 1.0000000000000001E-20,
121 0.0,
122 -0.0,
123 -0.0,
124 -5.0000000000000008E-23,
125 0.0,
126 0.0,
127 1.0000000000000001E-20};
128 static const double dv[11] = {23950.03, 23938.16, 23926.3, 23914.44,
129 23902.6, 23890.76, 23878.93, 23867.11,
130 23855.29, 23843.49, 23831.69};
131 static const double dv1[11] = {2585.64, 2583.71, 2581.78, 2579.85,
132 2577.92, 2576.0, 2574.08, 2572.15,
133 2570.24, 2568.32, 2566.4};
134 static const double dv2[11] = {39593.08, 39573.32, 39553.57, 39533.84,
135 39514.11, 39494.41, 39474.71, 39455.03,
136 39435.36, 39415.7, 39396.06};
137 static const double b_y[9] = {0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01};
138 static const int R[9] = {HIGHEST_NUMBER, 0, 0, 0, HIGHEST_NUMBER, 0, 0, 0, HIGHEST_NUMBER};
139 static const signed char iv[18] = {0, 0, 0, 0, 0, 0, 0, 0, 0,
140 1, 0, 0, 0, 1, 0, 0, 0, 1};
141 static const signed char b[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
142 double Phi[36];
143 double b_Phi[36];
144 double H[18];
145 double K[18];
146 double b_H[18];
147 double cos_term[16];
148 double A[9];
149 double dv3[9];
150 double skew_omega_sq[9];
151 double delta_x[6];
152 double delta_q[4];
153 double axis[3];
154 double b_W[3] = {0};
155 double A_tmp;
156 double a;
157 double angle;
158 double b_a;
159 double d;
160 double omega_norm;
161 double sin_term;
162 double x;
163 double y;
164 int H_tmp;
165 int b_i;
166 int cos_term_tmp;
167 int i;
168 /* Input parameters: */
169 /* q Attitude quaternion [qx,qy,qz,qw] */
170 /* beta Gyro bias estimate (3x1) */
171 /* P Covariance matrix (6x6) */
172 /* omega Gyroscope measurement (3x1) */
173 /* b_B Measured magnetic field vector (3x1) */
174 /* height_AGL Height Above Ground Level */
175 /* Output parameters: */
176 /* q_new Updated quaternion */
177 /* beta_new Updated gyro bias estimate */
178 /* P_new Updated covariance matrix */
179 /* PARAMETERS INITIALIZATION */
180 /* sigma_v Gyro noise standard deviation */
181 /* sigma_u Gyro bias random walk noise standard deviation */
182 /* sigma_b Magnetometer noise standard deviation */
183 /* dt Time step */
184 /* spec-sheet */
185 /* placeholder */
186 /* spec-sheet */
187 /* MEASUREMENT UPDATE */
188 /* Normalize b_B and b_W */
189 y = floor(height_AGL / 1000.0);
190 omega_norm = ceil(height_AGL / 1000.0);
191 //-----------------------------------------------------------------------------------------------------------------------------------
192 //x = (height_AGL / 1000.0 - (y + 1.0)) + 1.0;
193 // sin_term = dv[(int)(y + 1.0) - 1];
194 // b_W[0] = (dv[(int)(omega_norm + 1.0) - 1] - sin_term) * x + sin_term;
195 // sin_term = dv1[(int)(y + 1.0) - 1];
196 // b_W[1] = (dv1[(int)(omega_norm + 1.0) - 1] - sin_term) * x + sin_term;
197 // sin_term = dv2[(int)(y + 1.0) - 1];
198 // b_W[2] = (dv2[(int)(omega_norm + 1.0) - 1] - sin_term) * x + sin_term;
199 //---------------------------------------------------------------------------------------------------------------------------------- commented because of the magnetometers question
200 /* Assemble point-rotation matrix for q */
201 y = 2.0 * (q[2] * q[2]);
202 omega_norm = 2.0 * (q[1] * q[1]);
203 A[0] = (1.0 - omega_norm) - y;
204 x = q[0] * q[1];
205 sin_term = q[2] * q[3];
206 A[3] = 2.0 * (x - sin_term);
207 A_tmp = q[0] * q[2];
208 angle = q[1] * q[3];
209 A[6] = 2.0 * (A_tmp + angle);
210 A[1] = 2.0 * (x + sin_term);
211 x = 1.0 - 2.0 * (q[0] * q[0]);
212 A[4] = x - y;
213 y = q[1] * q[2];
214 sin_term = q[0] * q[3];
215 A[7] = 2.0 * (y - sin_term);
216 A[2] = 2.0 * (A_tmp - angle);
217 A[5] = 2.0 * (y + sin_term);
218 A[8] = x - omega_norm;
219 /* Calculate predicted measurement */
220 /* Define sensitivity matrix */
221 /* Create skew-symmetric matrix from a 3x1 vector */
222 H[0] = 0.0;
223 H[4] = 0.0;
224 H[8] = 0.0;
225 d = b_W[0];
226 A_tmp = b_W[1];
227 y = b_W[2];
228 for (i = 0; i < 3; i++) {
229 H_tmp = 3 * (i + 3);
230 H[H_tmp] = 0.0;
231 H[H_tmp + 1] = 0.0;
232 H[H_tmp + 2] = 0.0;
233 axis[i] = (A[i] * d + A[i + 3] * A_tmp) + A[i + 6] * y;
234 }
235 H[3] = -axis[2];
236 H[6] = axis[1];
237 H[1] = axis[2];
238 H[7] = -axis[0];
239 H[2] = -axis[1];
240 H[5] = axis[0];
241 /* Calculate Kalman gain */
242 for (i = 0; i < 3; i++) {
243 for (cos_term_tmp = 0; cos_term_tmp < 6; cos_term_tmp++) {
244 d = 0.0;
245 for (H_tmp = 0; H_tmp < 6; H_tmp++) {
246 d += H[i + 3 * H_tmp] * P[H_tmp + 6 * cos_term_tmp];
247 }
248 b_H[i + 3 * cos_term_tmp] = d;
249 }
250 for (cos_term_tmp = 0; cos_term_tmp < 3; cos_term_tmp++) {
251 d = 0.0;
252 for (H_tmp = 0; H_tmp < 6; H_tmp++) {
253 d += b_H[i + 3 * H_tmp] * H[cos_term_tmp + 3 * H_tmp];
254 }
255 H_tmp = i + 3 * cos_term_tmp;
256 skew_omega_sq[H_tmp] = d + (double)R[H_tmp];
257 }
258 }
259 mpower(skew_omega_sq, dv3);
260 for (i = 0; i < 6; i++) {
261 for (cos_term_tmp = 0; cos_term_tmp < 3; cos_term_tmp++) {
262 d = 0.0;
263 for (H_tmp = 0; H_tmp < 6; H_tmp++) {
264 d += P[i + 6 * H_tmp] * H[cos_term_tmp + 3 * H_tmp];
265 }
266 b_H[i + 6 * cos_term_tmp] = d;
267 }
268 d = b_H[i];
269 A_tmp = b_H[i + 6];
270 y = b_H[i + 12];
271 for (cos_term_tmp = 0; cos_term_tmp < 3; cos_term_tmp++) {
272 K[i + 6 * cos_term_tmp] =
273 (d * dv3[3 * cos_term_tmp] + A_tmp * dv3[3 * cos_term_tmp + 1]) +
274 y * dv3[3 * cos_term_tmp + 2];
275 }
276 }
277 /* Update error state */
278 d = b_norm(b_B);
279 A_tmp = b_norm(b_W);
280 //-----------------------------------------TODO SETTATO A 0 PER VIA DEL TINKERING, EVITARE 0/0
281 omega_norm = 0;// b_W[0] / A_tmp;
282 x = 0;//b_W[1] / A_tmp;
283 y = 0;//b_W[2] / A_tmp;
284 //for (i = 0; i < 3; i++) {
285 //b_W[i] = b_B[i] / d - ((A[i] * omega_norm + A[i + 3] * x) + A[i + 6] * y);
286 //}
287 //-----------------------------------------TODO SETTATO A 0 PER VIA DEL TINKERING, EVITARE 0/0
288 /* Update global states */
289 /* QUATERNION UPDATE */
290 /* delta_q quaternion definition */
291 /* Small angle approx */
292 /* Scalar part */
293 /* Multiply delta_q with the current quaternion */
294 /* Normalize the result */
295 /* BIAS UPDATE */
296 /* COVARIANCE MATRIX UPDATE */
297 b_eye(Phi);
298 for (i = 0; i < 6; i++) {
299 d = K[i];
300 A_tmp = K[i + 6];
301 y = K[i + 12];
302 delta_x[i] = (d * b_W[0] + A_tmp * b_W[1]) + y * b_W[2];
303 for (cos_term_tmp = 0; cos_term_tmp < 6; cos_term_tmp++) {
304 H_tmp = i + 6 * cos_term_tmp;
305 b_Phi[H_tmp] =
306 Phi[H_tmp] -
307 ((d * H[3 * cos_term_tmp] + A_tmp * H[3 * cos_term_tmp + 1]) +
308 y * H[3 * cos_term_tmp + 2]);
309 }
310 for (cos_term_tmp = 0; cos_term_tmp < 6; cos_term_tmp++) {
311 d = 0.0;
312 for (H_tmp = 0; H_tmp < 6; H_tmp++) {
313 d += b_Phi[i + 6 * H_tmp] * P[H_tmp + 6 * cos_term_tmp];
314 }
315 Phi[i + 6 * cos_term_tmp] = d;
316 }
317 }
318 delta_q[0] = 0.5 * delta_x[0];
319 delta_q[1] = 0.5 * delta_x[1];
320 delta_q[2] = 0.5 * delta_x[2];
321 q_new[0] =
322 ((delta_q[0] * q[3] + q[0]) + q[1] * delta_q[2]) - delta_q[1] * q[2];
323 q_new[1] =
324 ((delta_q[1] * q[3] - q[0] * delta_q[2]) + q[1]) + delta_q[0] * q[2];
325 q_new[2] =
326 ((delta_q[2] * q[3] + q[0] * delta_q[1]) - delta_q[0] * q[1]) + q[2];
327 q_new[3] =
328 ((q[3] - q[0] * delta_q[0]) - q[1] * delta_q[1]) - q[2] * delta_q[2];
329 d = c_norm(q_new);
330 q_new[0] /= d;
331 q_new[1] /= d;
332 q_new[2] /= d;
333 q_new[3] /= d;
334 beta_new[0] = beta[0] + delta_x[3];
335 beta_new[1] = beta[1] + delta_x[4];
336 beta_new[2] = beta[2] + delta_x[5];
337 memcpy(&P[0], &Phi[0], 36U * sizeof(double));
338 /* STATE PROPAGATION */
339 /* Calculate the gyroscope measurement without the bias */
340 b_W[0] = omega[0] - beta_new[0];
341 b_W[1] = omega[1] - beta_new[1];
342 b_W[2] = omega[2] - beta_new[2];
343 /* Calculate the magnitude of the angular velocity */
344 omega_norm = b_norm(b_W);
345 /* Calculate the angle and axis of rotation */
346 angle = omega_norm * 0.01;
347 /* Compute the matrix exponential */
348 x = cos(angle);
349 sin_term = sin(angle);
350 /* Define the skew symmetric matrix for omega_hat */
351 /* Create skew-symmetric matrix from a 3x1 vector */
352 A[0] = 0.0;
353 A[3] = -b_W[2];
354 A[6] = b_W[1];
355 A[1] = b_W[2];
356 A[4] = 0.0;
357 A[7] = -b_W[0];
358 A[2] = -b_W[1];
359 A[5] = b_W[0];
360 A[8] = 0.0;
361 /* Calculate the squared product of the skew matrix of omega_hat */
362 for (b_i = 0; b_i < 3; b_i++) {
363 axis[b_i] = b_W[b_i] / omega_norm;
364 for (i = 0; i < 3; i++) {
365 skew_omega_sq[b_i + 3 * i] =
366 (A[b_i] * A[3 * i] + A[b_i + 3] * A[3 * i + 1]) +
367 A[b_i + 6] * A[3 * i + 2];
368 }
369 }
370 /* Calculate the elements of the first row of the phi matrix */
371 a = sin_term / omega_norm;
372 y = omega_norm * omega_norm;
373 b_a = x / y;
374 x = (1.0 - x) / y;
375 y = (angle - sin_term) / rt_powd_snf(omega_norm, 3.0);
376 /* Assemble phi matrix */
377 eye(dv3);
378 for (i = 0; i < 3; i++) {
379 d = A[3 * i];
380 A_tmp = skew_omega_sq[3 * i];
381 Phi[6 * i] = (dv3[3 * i] - a * d) + b_a * A_tmp;
382 H_tmp = 6 * (i + 3);
383 Phi[H_tmp] = (x * d - y * A_tmp) - b_y[3 * i];
384 cos_term_tmp = 3 * i + 1;
385 d = A[cos_term_tmp];
386 A_tmp = skew_omega_sq[cos_term_tmp];
387 Phi[6 * i + 1] = (dv3[cos_term_tmp] - a * d) + b_a * A_tmp;
388 Phi[H_tmp + 1] = (x * d - y * A_tmp) - b_y[cos_term_tmp];
389 cos_term_tmp = 3 * i + 2;
390 d = A[cos_term_tmp];
391 A_tmp = skew_omega_sq[cos_term_tmp];
392 Phi[6 * i + 2] = (dv3[cos_term_tmp] - a * d) + b_a * A_tmp;
393 Phi[H_tmp + 2] = (x * d - y * A_tmp) - b_y[cos_term_tmp];
394 }
395 for (i = 0; i < 6; i++) {
396 Phi[6 * i + 3] = iv[3 * i];
397 Phi[6 * i + 4] = iv[3 * i + 1];
398 Phi[6 * i + 5] = iv[3 * i + 2];
399 }
400 /* Calculate the elements of Q_k */
401 /* Assemble Q_k */
402 /* Covariance propagation */
403 for (i = 0; i < 6; i++) {
404 for (cos_term_tmp = 0; cos_term_tmp < 6; cos_term_tmp++) {
405 d = 0.0;
406 for (H_tmp = 0; H_tmp < 6; H_tmp++) {
407 d += Phi[i + 6 * H_tmp] * P[H_tmp + 6 * cos_term_tmp];
408 }
409 b_Phi[i + 6 * cos_term_tmp] = d;
410 }
411 for (cos_term_tmp = 0; cos_term_tmp < 6; cos_term_tmp++) {
412 d = 0.0;
413 for (H_tmp = 0; H_tmp < 6; H_tmp++) {
414 d += b_Phi[i + 6 * H_tmp] * Phi[cos_term_tmp + 6 * H_tmp];
415 }
416 H_tmp = i + 6 * cos_term_tmp;
417 P_new[H_tmp] = d + Q[H_tmp];
418 }
419 }
420 /* Compute the skew-symmetric matrix for the axis */
421 /* Create skew-symmetric matrix from a 3x1 vector */
422 /* Recompute the sine and cosine terms */
423 x = cos(0.5 * angle);
424 sin_term = sin(0.5 * angle);
425 /* Construct the Theta matrix */
426 /* Propagate the quaternion */
427 skew_omega_sq[0] = sin_term * 0.0;
428 skew_omega_sq[3] = sin_term * -axis[2];
429 skew_omega_sq[6] = sin_term * axis[1];
430 skew_omega_sq[1] = sin_term * axis[2];
431 skew_omega_sq[4] = sin_term * 0.0;
432 skew_omega_sq[7] = sin_term * -axis[0];
433 skew_omega_sq[2] = sin_term * -axis[1];
434 skew_omega_sq[5] = sin_term * axis[0];
435 skew_omega_sq[8] = sin_term * 0.0;
436 for (b_i = 0; b_i < 3; b_i++) {
437 d = sin_term * axis[b_i];
438 H_tmp = b_i << 2;
439 cos_term[H_tmp] = x * (double)b[3 * b_i] - skew_omega_sq[3 * b_i];
440 cos_term_tmp = 3 * b_i + 1;
441 cos_term[H_tmp + 1] =
442 x * (double)b[cos_term_tmp] - skew_omega_sq[cos_term_tmp];
443 cos_term_tmp = 3 * b_i + 2;
444 cos_term[H_tmp + 2] =
445 x * (double)b[cos_term_tmp] - skew_omega_sq[cos_term_tmp];
446 cos_term[b_i + 12] = d;
447 cos_term[H_tmp + 3] = -d;
448 }
449 cos_term[15] = x;
450 d = q_new[0];
451 A_tmp = q_new[1];
452 y = q_new[2];
453 omega_norm = q_new[3];
454 for (i = 0; i < 4; i++) {
455 delta_q[i] =
456 ((cos_term[i] * d + cos_term[i + 4] * A_tmp) + cos_term[i + 8] * y) +
457 cos_term[i + 12] * omega_norm;
458 }
459 /* Normalize the propagated quaternion */
460 d = c_norm(delta_q);
461 q_new[0] = delta_q[0] / d;
462 q_new[1] = delta_q[1] / d;
463 q_new[2] = delta_q[2] / d;
464 q_new[3] = delta_q[3] / d;
465}
466
467/*
468 * File trailer for mekf.c
469 *
470 * [EOF]
471 */
b_eye
void b_eye(double b_I[36])
Creates a 6x6 identity matrix.
Definition eye.c:21
eye
void eye(double b_I[9])
Creates a 3x3 identity matrix.
Definition eye.c:34
eye.h
Utility functions for creating identity matrices of various sizes.
rtIsInf
boolean_T rtIsInf(real_T value)
Checks if a double-precision value is positive or negative infinity.
Definition rt_nonfinite.c:40
rtIsNaN
boolean_T rtIsNaN(real_T value)
Checks if a double-precision value is Not-a-Number (NaN).
Definition rt_nonfinite.c:60
rtInf
real_T rtInf
Global constant for positive infinity (double precision).
Definition rt_nonfinite.c:25
rtNaN
real_T rtNaN
Global constant for Not-a-Number (double precision).
Definition rt_nonfinite.c:24
b_B
double b_B[]
Definition main.c:241
beta
double beta[]
Definition main.c:238
q_new
double q_new[]
Definition main.c:243
omega
double omega[]
Definition main.c:240
R
const double R[]
Definition main.c:253
beta_new
double beta_new[]
Definition main.c:244
q
double q[]
Definition main.c:237
Q
const double Q[]
Definition main.c:252
mekf
void mekf(const double q[4], const double beta[3], double P[36], const double omega[3], const double b_B[3], double height_AGL, double q_new[4], double beta_new[3], double P_new[36])
Executes one prediction and correction step of the MEKF.
Definition mekf.c:88
rt_powd_snf
static double rt_powd_snf(double u0, double u1)
Definition mekf.c:30
mekf.h
Multiplicative Extended Kalman Filter (MEKF) for attitude and gyroscope bias estimation.
HIGHEST_NUMBER
#define HIGHEST_NUMBER
Definition mekf.h:34
mpower
void mpower(const double a[9], double c[9])
Calculates the inverse of a 3x3 matrix.
Definition mpower.c:24
mpower.h
A MATLAB Coder helper function for calculating the inverse of a 3x3 matrix.
b_norm
double b_norm(const double x[3])
Calculates the Euclidean norm (magnitude) of a 3-element vector.
Definition norm.c:21
c_norm
double c_norm(const double x[4])
Calculates the Euclidean norm (magnitude) of a 4-element vector.
Definition norm.c:61
norm.h
Provides MATLAB Coder helper functions to calculate the Euclidean norm of a vector.
rt_nonfinite.h
Provides definitions and utility functions for non-finite floating-point values.