Paparazzi UAS  v6.3_unstable
Paparazzi is a free software Unmanned Aircraft System.
ins_ext_pose.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2023 MAVLab
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
28 #include <time.h>
29 
30 #include "ins_ext_pose.h"
31 #include "state.h"
33 #include "modules/imu/imu.h"
34 #include "modules/ins/ins.h"
35 #include "generated/flight_plan.h"
36 
37 #include "modules/core/abi.h"
38 
39 #if 0
40 #include <stdio.h>
41 #define DEBUG_PRINT(...) printf(__VA_ARGS__)
42 #else
43 #define DEBUG_PRINT(...) {}
44 #endif
45 
46 
51 struct InsExtPose {
52  /* Inputs */
53  struct FloatRates gyros_f;
54  struct FloatVect3 accels_f;
57 
58  struct FloatVect3 ev_pos;
59  struct FloatEulers ev_att;
61 
62  /* Origin */
63  struct LtpDef_i ltp_def;
64 
65  /* output LTP NED */
66  struct NedCoor_i ltp_pos;
67  struct NedCoor_i ltp_speed;
68  struct NedCoor_i ltp_accel;
69 };
70 
71 struct InsExtPose ins_ext_pos;
72 
73 
75 {
76 
77  struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
78  llh_nav0.lat = NAV_LAT0;
79  llh_nav0.lon = NAV_LON0;
80  /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
81  llh_nav0.alt = NAV_ALT0 + NAV_MSL0;
82 
83  struct EcefCoor_i ecef_nav0;
84  ecef_of_lla_i(&ecef_nav0, &llh_nav0);
85 
87  ins_ext_pos.ltp_def.hmsl = NAV_ALT0;
89 }
90 
91 
95 #if PERIODIC_TELEMETRY
97 
98 static void send_ins(struct transport_tx *trans, struct link_device *dev)
99 {
100  pprz_msg_send_INS(trans, dev, AC_ID,
104 }
105 
106 static void send_ins_z(struct transport_tx *trans, struct link_device *dev)
107 {
108  static float fake_baro_z = 0.0;
109  pprz_msg_send_INS_Z(trans, dev, AC_ID,
110  (float *)&fake_baro_z, &ins_ext_pos.ltp_pos.z,
112 }
113 
114 static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
115 {
116  static float fake_qfe = 0.0;
117  pprz_msg_send_INS_REF(trans, dev, AC_ID,
120  &ins_ext_pos.ltp_def.hmsl, (float *)&fake_qfe);
121 }
122 #endif
123 
124 
129 #ifndef INS_EXT_POSE_IMU_ID
130 #define INS_EXT_POSE_IMU_ID ABI_BROADCAST
131 #endif
132 PRINT_CONFIG_VAR(INS_EXT_POSE_IMU_ID)
133 
136 
137 static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel);
138 static void gyro_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro);
139 
140 
141 
142 static void gyro_cb(uint8_t sender_id __attribute__((unused)),
143  uint32_t stamp __attribute__((unused)),
144  struct Int32Rates *gyro)
145 {
147  ins_ext_pos.has_new_gyro = true;
148 }
149 
150 static void accel_cb(uint8_t sender_id __attribute__((unused)),
151  uint32_t stamp __attribute__((unused)),
152  struct Int32Vect3 *accel)
153 {
155  ins_ext_pos.has_new_acc = true;
156 }
157 
158 
164 {
165  if (DL_EXTERNAL_POSE_ac_id(buf) != AC_ID) { return; } // not for this aircraft
166 
167  float enu_x = DL_EXTERNAL_POSE_enu_x(buf);
168  float enu_y = DL_EXTERNAL_POSE_enu_y(buf);
169  float enu_z = DL_EXTERNAL_POSE_enu_z(buf);
170 
171  float quat_i = DL_EXTERNAL_POSE_body_qi(buf);
172  float quat_x = DL_EXTERNAL_POSE_body_qx(buf);
173  float quat_y = DL_EXTERNAL_POSE_body_qy(buf);
174  float quat_z = DL_EXTERNAL_POSE_body_qz(buf);
175 
176  DEBUG_PRINT("EXT_UPDATE\n");
177 
178  struct FloatQuat orient;
179  struct FloatEulers orient_eulers;
180 
181  orient.qi = quat_i;
182  orient.qx = quat_y; //north
183  orient.qy = -quat_x; //east
184  orient.qz = -quat_z; //down
185 
186  float_eulers_of_quat(&orient_eulers, &orient);
187  orient_eulers.theta = -orient_eulers.theta;
188 
189  ins_ext_pos.ev_pos.x = enu_y;
190  ins_ext_pos.ev_pos.y = enu_x;
191  ins_ext_pos.ev_pos.z = -enu_z;
192  ins_ext_pos.ev_att.phi = orient_eulers.phi;
193  ins_ext_pos.ev_att.theta = orient_eulers.theta;
194  ins_ext_pos.ev_att.psi = orient_eulers.psi;
195 
197 
199 }
200 
202 {
203  // Ext pos does not allow geoinit: FP origin only
204 }
205 
207 {
208  // Ext pos does not allow geoinit: FP origin only
209 }
210 
211 
215 static inline void ekf_init(void);
216 static inline void ekf_run(void);
217 
223 {
224 
225  // Initialize inputs
226  ins_ext_pos.has_new_acc = false;
227  ins_ext_pos.has_new_gyro = false;
229 
230  // Get External Pose Origin From Flightplan
232 
233  // Provide telemetry
234 #if PERIODIC_TELEMETRY
238 #endif
239 
240  // Get IMU through ABI
241  AbiBindMsgIMU_ACCEL(INS_EXT_POSE_IMU_ID, &accel_ev, accel_cb);
242  AbiBindMsgIMU_GYRO(INS_EXT_POSE_IMU_ID, &gyro_ev, gyro_cb);
243 
244  // Get External Pose through datalink message: setup in xml
245 
246  // Initialize EKF
247  ekf_init();
248 }
249 
251 {
252  ekf_run();
253 }
254 
255 
256 
257 
258 /***************************************************
259  * Kalman Filter.
260  */
261 
262 
263 
264 static inline void ekf_f(const float X[EKF_NUM_STATES], const float U[EKF_NUM_INPUTS], float out[EKF_NUM_STATES]);
265 static inline void ekf_F(const float X[EKF_NUM_STATES], const float U[EKF_NUM_INPUTS],
266  float out[EKF_NUM_STATES][EKF_NUM_STATES]);
267 static inline void ekf_L(const float X[EKF_NUM_STATES], const float U[EKF_NUM_INPUTS],
268  float out[EKF_NUM_STATES][EKF_NUM_INPUTS]);
269 
270 static inline void ekf_f_rk4(const float X[EKF_NUM_STATES], const float U[EKF_NUM_INPUTS], const float dt,
271  float out[EKF_NUM_STATES]);
272 
273 static inline void ekf_step(const float U[EKF_NUM_INPUTS], const float Z[EKF_NUM_OUTPUTS], const float dt);
274 static inline void ekf_prediction_step(const float U[EKF_NUM_INPUTS], const float dt);
275 static inline void ekf_measurement_step(const float Z[EKF_NUM_OUTPUTS]);
276 
277 
278 
285 
286 float ekf_H[EKF_NUM_OUTPUTS][EKF_NUM_STATES] = {{1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0}};
287 
288 
289 float t0;
290 float t1;
291 
292 void ekf_set_diag(float **a, float *b, int n);
293 void ekf_set_diag(float **a, float *b, int n)
294 {
295  int i, j;
296  for (i = 0 ; i < n; i++) {
297  for (j = 0 ; j < n; j++) {
298  if (i == j) {
299  a[i][j] = b[i];
300  } else {
301  a[i][j] = 0.0;
302  }
303  }
304  }
305 }
306 
307 
308 
309 static inline void ekf_init(void)
310 {
311 
312  DEBUG_PRINT("ekf init");
313  float X0[EKF_NUM_STATES] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
314  float U0[EKF_NUM_INPUTS] = {0, 0, 0, 0, 0, 0};
315  float Z0[EKF_NUM_OUTPUTS] = {0, 0, 0, 0, 0, 0};
316 
317  float Pdiag[EKF_NUM_STATES] = {1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1.};
318  float Qdiag[EKF_NUM_INPUTS] = {0.5, 0.5, 0.5, 0.01, 0.01, 0.01};
319 
320  float Rdiag[EKF_NUM_OUTPUTS] = {0.001, 0.001, 0.001, 0.1, 0.1, 0.1};
321 
325 
326  ekf_set_diag(ekf_P_, Pdiag, EKF_NUM_STATES);
327  ekf_set_diag(ekf_Q_, Qdiag, EKF_NUM_INPUTS);
328  ekf_set_diag(ekf_R_, Rdiag, EKF_NUM_OUTPUTS);
332 }
333 
334 static inline void ekf_f(const float X[EKF_NUM_STATES], const float U[EKF_NUM_INPUTS], float out[EKF_NUM_STATES])
335 {
336  float x0 = cos(X[8]);
337  float x1 = U[0] - X[9];
338  float x2 = cos(X[7]);
339  float x3 = x1 * x2;
340  float x4 = U[2] - X[11];
341  float x5 = sin(X[6]);
342  float x6 = sin(X[8]);
343  float x7 = x5 * x6;
344  float x8 = sin(X[7]);
345  float x9 = cos(X[6]);
346  float x10 = x0 * x9;
347  float x11 = U[1] - X[10];
348  float x12 = x6 * x9;
349  float x13 = x0 * x5;
350  float x14 = tan(X[7]);
351  float x15 = U[4] - X[13];
352  float x16 = x15 * x5;
353  float x17 = U[5] - X[14];
354  float x18 = x17 * x9;
355  float x19 = 1.0 / x2;
356  out[0] = X[3];
357  out[1] = X[4];
358  out[2] = X[5];
359  out[3] = x0 * x3 + x11 * (-x12 + x13 * x8) + x4 * (x10 * x8 + x7);
360  out[4] = x11 * (x10 + x7 * x8) + x3 * x6 + x4 * (x12 * x8 - x13);
361  out[5] = -x1 * x8 + x11 * x2 * x5 + x2 * x4 * x9 + 9.8100000000000005;
362  out[6] = U[3] - X[12] + x14 * x16 + x14 * x18;
363  out[7] = x15 * x9 - x17 * x5;
364  out[8] = x16 * x19 + x18 * x19;
365  out[9] = 0;
366  out[10] = 0;
367  out[11] = 0;
368  out[12] = 0;
369  out[13] = 0;
370  out[14] = 0;
371 }
372 
373 static inline void ekf_F(const float X[EKF_NUM_STATES], const float U[EKF_NUM_INPUTS],
374  float out[EKF_NUM_STATES][EKF_NUM_STATES])
375 {
376  float x0 = U[1] - X[10];
377  float x1 = sin(X[6]);
378  float x2 = sin(X[8]);
379  float x3 = x1 * x2;
380  float x4 = sin(X[7]);
381  float x5 = cos(X[6]);
382  float x6 = cos(X[8]);
383  float x7 = x5 * x6;
384  float x8 = x4 * x7;
385  float x9 = x3 + x8;
386  float x10 = U[2] - X[11];
387  float x11 = x2 * x5;
388  float x12 = x1 * x6;
389  float x13 = x12 * x4;
390  float x14 = x11 - x13;
391  float x15 = U[0] - X[9];
392  float x16 = x15 * x4;
393  float x17 = cos(X[7]);
394  float x18 = x0 * x17;
395  float x19 = x10 * x17;
396  float x20 = x17 * x2;
397  float x21 = x11 * x4;
398  float x22 = x12 - x21;
399  float x23 = -x3 * x4 - x7;
400  float x24 = x17 * x6;
401  float x25 = x17 * x5;
402  float x26 = x1 * x17;
403  float x27 = x4 * x5;
404  float x28 = U[4] - X[13];
405  float x29 = tan(X[7]);
406  float x30 = x29 * x5;
407  float x31 = U[5] - X[14];
408  float x32 = x1 * x29;
409  float x33 = pow(x29, 2) + 1;
410  float x34 = x1 * x28;
411  float x35 = 1.0 / x17;
412  float x36 = x35 * x5;
413  float x37 = x1 * x35;
414  float x38 = pow(x17, -2);
415  out[0][0] = 0;
416  out[0][1] = 0;
417  out[0][2] = 0;
418  out[0][3] = 1;
419  out[0][4] = 0;
420  out[0][5] = 0;
421  out[0][6] = 0;
422  out[0][7] = 0;
423  out[0][8] = 0;
424  out[0][9] = 0;
425  out[0][10] = 0;
426  out[0][11] = 0;
427  out[0][12] = 0;
428  out[0][13] = 0;
429  out[0][14] = 0;
430  out[1][0] = 0;
431  out[1][1] = 0;
432  out[1][2] = 0;
433  out[1][3] = 0;
434  out[1][4] = 1;
435  out[1][5] = 0;
436  out[1][6] = 0;
437  out[1][7] = 0;
438  out[1][8] = 0;
439  out[1][9] = 0;
440  out[1][10] = 0;
441  out[1][11] = 0;
442  out[1][12] = 0;
443  out[1][13] = 0;
444  out[1][14] = 0;
445  out[2][0] = 0;
446  out[2][1] = 0;
447  out[2][2] = 0;
448  out[2][3] = 0;
449  out[2][4] = 0;
450  out[2][5] = 1;
451  out[2][6] = 0;
452  out[2][7] = 0;
453  out[2][8] = 0;
454  out[2][9] = 0;
455  out[2][10] = 0;
456  out[2][11] = 0;
457  out[2][12] = 0;
458  out[2][13] = 0;
459  out[2][14] = 0;
460  out[3][0] = 0;
461  out[3][1] = 0;
462  out[3][2] = 0;
463  out[3][3] = 0;
464  out[3][4] = 0;
465  out[3][5] = 0;
466  out[3][6] = x0 * x9 + x10 * x14;
467  out[3][7] = x12 * x18 - x16 * x6 + x19 * x7;
468  out[3][8] = x0 * x23 + x10 * x22 - x15 * x20;
469  out[3][9] = -x24;
470  out[3][10] = x14;
471  out[3][11] = -x3 - x8;
472  out[3][12] = 0;
473  out[3][13] = 0;
474  out[3][14] = 0;
475  out[4][0] = 0;
476  out[4][1] = 0;
477  out[4][2] = 0;
478  out[4][3] = 0;
479  out[4][4] = 0;
480  out[4][5] = 0;
481  out[4][6] = x0 * (-x12 + x21) + x10 * x23;
482  out[4][7] = x11 * x19 - x16 * x2 + x18 * x3;
483  out[4][8] = x0 * (-x11 + x13) + x10 * x9 + x15 * x24;
484  out[4][9] = -x20;
485  out[4][10] = x23;
486  out[4][11] = x22;
487  out[4][12] = 0;
488  out[4][13] = 0;
489  out[4][14] = 0;
490  out[5][0] = 0;
491  out[5][1] = 0;
492  out[5][2] = 0;
493  out[5][3] = 0;
494  out[5][4] = 0;
495  out[5][5] = 0;
496  out[5][6] = x0 * x25 - x10 * x26;
497  out[5][7] = -x0 * x1 * x4 - x10 * x27 + x17 * (-U[0] + X[9]);
498  out[5][8] = 0;
499  out[5][9] = x4;
500  out[5][10] = -x26;
501  out[5][11] = -x25;
502  out[5][12] = 0;
503  out[5][13] = 0;
504  out[5][14] = 0;
505  out[6][0] = 0;
506  out[6][1] = 0;
507  out[6][2] = 0;
508  out[6][3] = 0;
509  out[6][4] = 0;
510  out[6][5] = 0;
511  out[6][6] = x28 * x30 - x31 * x32;
512  out[6][7] = x31 * x33 * x5 + x33 * x34;
513  out[6][8] = 0;
514  out[6][9] = 0;
515  out[6][10] = 0;
516  out[6][11] = 0;
517  out[6][12] = -1;
518  out[6][13] = -x32;
519  out[6][14] = -x30;
520  out[7][0] = 0;
521  out[7][1] = 0;
522  out[7][2] = 0;
523  out[7][3] = 0;
524  out[7][4] = 0;
525  out[7][5] = 0;
526  out[7][6] = -x34 + x5 * (-U[5] + X[14]);
527  out[7][7] = 0;
528  out[7][8] = 0;
529  out[7][9] = 0;
530  out[7][10] = 0;
531  out[7][11] = 0;
532  out[7][12] = 0;
533  out[7][13] = -x5;
534  out[7][14] = x1;
535  out[8][0] = 0;
536  out[8][1] = 0;
537  out[8][2] = 0;
538  out[8][3] = 0;
539  out[8][4] = 0;
540  out[8][5] = 0;
541  out[8][6] = x28 * x36 - x31 * x37;
542  out[8][7] = x27 * x31 * x38 + x34 * x38 * x4;
543  out[8][8] = 0;
544  out[8][9] = 0;
545  out[8][10] = 0;
546  out[8][11] = 0;
547  out[8][12] = 0;
548  out[8][13] = -x37;
549  out[8][14] = -x36;
550  out[9][0] = 0;
551  out[9][1] = 0;
552  out[9][2] = 0;
553  out[9][3] = 0;
554  out[9][4] = 0;
555  out[9][5] = 0;
556  out[9][6] = 0;
557  out[9][7] = 0;
558  out[9][8] = 0;
559  out[9][9] = 0;
560  out[9][10] = 0;
561  out[9][11] = 0;
562  out[9][12] = 0;
563  out[9][13] = 0;
564  out[9][14] = 0;
565  out[10][0] = 0;
566  out[10][1] = 0;
567  out[10][2] = 0;
568  out[10][3] = 0;
569  out[10][4] = 0;
570  out[10][5] = 0;
571  out[10][6] = 0;
572  out[10][7] = 0;
573  out[10][8] = 0;
574  out[10][9] = 0;
575  out[10][10] = 0;
576  out[10][11] = 0;
577  out[10][12] = 0;
578  out[10][13] = 0;
579  out[10][14] = 0;
580  out[11][0] = 0;
581  out[11][1] = 0;
582  out[11][2] = 0;
583  out[11][3] = 0;
584  out[11][4] = 0;
585  out[11][5] = 0;
586  out[11][6] = 0;
587  out[11][7] = 0;
588  out[11][8] = 0;
589  out[11][9] = 0;
590  out[11][10] = 0;
591  out[11][11] = 0;
592  out[11][12] = 0;
593  out[11][13] = 0;
594  out[11][14] = 0;
595  out[12][0] = 0;
596  out[12][1] = 0;
597  out[12][2] = 0;
598  out[12][3] = 0;
599  out[12][4] = 0;
600  out[12][5] = 0;
601  out[12][6] = 0;
602  out[12][7] = 0;
603  out[12][8] = 0;
604  out[12][9] = 0;
605  out[12][10] = 0;
606  out[12][11] = 0;
607  out[12][12] = 0;
608  out[12][13] = 0;
609  out[12][14] = 0;
610  out[13][0] = 0;
611  out[13][1] = 0;
612  out[13][2] = 0;
613  out[13][3] = 0;
614  out[13][4] = 0;
615  out[13][5] = 0;
616  out[13][6] = 0;
617  out[13][7] = 0;
618  out[13][8] = 0;
619  out[13][9] = 0;
620  out[13][10] = 0;
621  out[13][11] = 0;
622  out[13][12] = 0;
623  out[13][13] = 0;
624  out[13][14] = 0;
625  out[14][0] = 0;
626  out[14][1] = 0;
627  out[14][2] = 0;
628  out[14][3] = 0;
629  out[14][4] = 0;
630  out[14][5] = 0;
631  out[14][6] = 0;
632  out[14][7] = 0;
633  out[14][8] = 0;
634  out[14][9] = 0;
635  out[14][10] = 0;
636  out[14][11] = 0;
637  out[14][12] = 0;
638  out[14][13] = 0;
639  out[14][14] = 0;
640 }
641 
642 static inline void ekf_L(const float X[EKF_NUM_STATES], __attribute__((unused)) const float U[EKF_NUM_INPUTS],
643  float out[EKF_NUM_STATES][EKF_NUM_INPUTS])
644 {
645  float x0 = cos(X[7]);
646  float x1 = cos(X[8]);
647  float x2 = sin(X[8]);
648  float x3 = cos(X[6]);
649  float x4 = x2 * x3;
650  float x5 = sin(X[7]);
651  float x6 = sin(X[6]);
652  float x7 = x1 * x6;
653  float x8 = x2 * x6;
654  float x9 = x1 * x3;
655  float x10 = tan(X[7]);
656  float x11 = 1.0 / x0;
657  out[0][0] = 0;
658  out[0][1] = 0;
659  out[0][2] = 0;
660  out[0][3] = 0;
661  out[0][4] = 0;
662  out[0][5] = 0;
663  out[1][0] = 0;
664  out[1][1] = 0;
665  out[1][2] = 0;
666  out[1][3] = 0;
667  out[1][4] = 0;
668  out[1][5] = 0;
669  out[2][0] = 0;
670  out[2][1] = 0;
671  out[2][2] = 0;
672  out[2][3] = 0;
673  out[2][4] = 0;
674  out[2][5] = 0;
675  out[3][0] = -x0 * x1;
676  out[3][1] = x4 - x5 * x7;
677  out[3][2] = -x5 * x9 - x8;
678  out[3][3] = 0;
679  out[3][4] = 0;
680  out[3][5] = 0;
681  out[4][0] = -x0 * x2;
682  out[4][1] = -x5 * x8 - x9;
683  out[4][2] = -x4 * x5 + x7;
684  out[4][3] = 0;
685  out[4][4] = 0;
686  out[4][5] = 0;
687  out[5][0] = x5;
688  out[5][1] = -x0 * x6;
689  out[5][2] = -x0 * x3;
690  out[5][3] = 0;
691  out[5][4] = 0;
692  out[5][5] = 0;
693  out[6][0] = 0;
694  out[6][1] = 0;
695  out[6][2] = 0;
696  out[6][3] = -1;
697  out[6][4] = -x10 * x6;
698  out[6][5] = -x10 * x3;
699  out[7][0] = 0;
700  out[7][1] = 0;
701  out[7][2] = 0;
702  out[7][3] = 0;
703  out[7][4] = -x3;
704  out[7][5] = x6;
705  out[8][0] = 0;
706  out[8][1] = 0;
707  out[8][2] = 0;
708  out[8][3] = 0;
709  out[8][4] = -x11 * x6;
710  out[8][5] = -x11 * x3;
711  out[9][0] = 0;
712  out[9][1] = 0;
713  out[9][2] = 0;
714  out[9][3] = 0;
715  out[9][4] = 0;
716  out[9][5] = 0;
717  out[10][0] = 0;
718  out[10][1] = 0;
719  out[10][2] = 0;
720  out[10][3] = 0;
721  out[10][4] = 0;
722  out[10][5] = 0;
723  out[11][0] = 0;
724  out[11][1] = 0;
725  out[11][2] = 0;
726  out[11][3] = 0;
727  out[11][4] = 0;
728  out[11][5] = 0;
729  out[12][0] = 0;
730  out[12][1] = 0;
731  out[12][2] = 0;
732  out[12][3] = 0;
733  out[12][4] = 0;
734  out[12][5] = 0;
735  out[13][0] = 0;
736  out[13][1] = 0;
737  out[13][2] = 0;
738  out[13][3] = 0;
739  out[13][4] = 0;
740  out[13][5] = 0;
741  out[14][0] = 0;
742  out[14][1] = 0;
743  out[14][2] = 0;
744  out[14][3] = 0;
745  out[14][4] = 0;
746  out[14][5] = 0;
747 }
748 
749 
750 
751 static inline void ekf_f_rk4(const float X[EKF_NUM_STATES], const float U[EKF_NUM_INPUTS], const float dt,
752  float out[EKF_NUM_STATES])
753 {
754  float k1[EKF_NUM_STATES];
755  float k2[EKF_NUM_STATES];
756  float k3[EKF_NUM_STATES];
757  float k4[EKF_NUM_STATES];
758 
759  float Xtmp[EKF_NUM_STATES];
760 
761  // k1 = f(X,U)
762  ekf_f(X, U, k1);
763 
764  // Xtmp = X+dt*k1/2
765  float_vect_smul(Xtmp, k1, dt / 2, EKF_NUM_STATES);
767 
768  // k2 = f(Xtmp,U)
769  ekf_f(Xtmp, U, k2);
770 
771  // Xtmp = X+dt*k2/2
772  float_vect_smul(Xtmp, k2, dt / 2, EKF_NUM_STATES);
774 
775  // k3 = f(Xtmp,U)
776  ekf_f(Xtmp, U, k3);
777 
778  // Xtmp = X+dt*k3
779  float_vect_smul(Xtmp, k3, dt, EKF_NUM_STATES);
781 
782  // k4 = f(Xtmp,U)
783  ekf_f(Xtmp, U, k4);
784 
785  // out = k2+k3
786  float_vect_sum(out, k2, k3, EKF_NUM_STATES);
787  // out *= 2
789  // out += k1
790  float_vect_add(out, k1, EKF_NUM_STATES);
791  // out += k4
792  float_vect_add(out, k4, EKF_NUM_STATES);
793  // out *= dt/6
794  float_vect_scale(out, dt / 6, EKF_NUM_STATES);
795  // out += X
797 }
798 
799 
800 static inline void ekf_step(const float U[EKF_NUM_INPUTS], const float Z[EKF_NUM_OUTPUTS], const float dt)
801 {
802  // [1] Predicted (a priori) state estimate:
803  float Xkk_1[EKF_NUM_STATES];
804  ekf_f_rk4(ekf_X, U, dt, Xkk_1);
805 
806 
807  // [2] Get matrices
808  float F[EKF_NUM_STATES][EKF_NUM_STATES];
809  float L[EKF_NUM_STATES][EKF_NUM_INPUTS];
810  ekf_F(ekf_X, U, F);
811  ekf_L(ekf_X, U, L);
812 
813 
814  // [3] Continuous to discrete
815  // Fd = eye(N) + F*dt + F*F*dt**2/2 = I + [I+F*dt/2]*F*dt
816  // Ld = L*dt+F*L*dt**2/2 = [I+F*dt/2]*L*dt
817  float Fd[EKF_NUM_STATES][EKF_NUM_STATES];
818  float Ld[EKF_NUM_STATES][EKF_NUM_INPUTS];
819  float tmp[EKF_NUM_STATES][EKF_NUM_STATES];
820 
825  MAKE_MATRIX_PTR(tmp_, tmp, EKF_NUM_STATES);
826 
827  // tmp = I+F*dt/2
830 
831  // Ld = tmp*L*dt
834 
835  // Fd = tmp*F*dt
838 
839  // Fd += I
840  int i;
841  for (i = 0; i < EKF_NUM_STATES; i++) {
842  Fd[i][i] += 1;
843  }
844 
845 
846  // [4] Predicted covariance estimate:
847  // Pkk_1 = Fd*P*Fd.T + Ld*Q*Ld.T
848  float Pkk_1[EKF_NUM_STATES][EKF_NUM_STATES];
849  float LdT[EKF_NUM_INPUTS][EKF_NUM_STATES];
850  float QLdT[EKF_NUM_INPUTS][EKF_NUM_STATES];
851 
852  MAKE_MATRIX_PTR(Pkk_1_, Pkk_1, EKF_NUM_STATES);
855  MAKE_MATRIX_PTR(LdT_, LdT, EKF_NUM_INPUTS);
856  MAKE_MATRIX_PTR(QLdT_, QLdT, EKF_NUM_INPUTS);
857 
858  // Fd = Fd.T
860 
861  // tmp = P*Fd
863 
864  // Fd = Fd.T
866 
867  // Pkk_1 = Fd*tmp
869 
870  // LdT = Ld.T
872 
873  // QLdT = Q*LdT
875 
876  // tmp = Ld*QLdT
878 
879  // Pkk_1 += tmp
881 
882 
883  // [5] Measurement residual:
884  // yk = Z - H*Xkk_1
885  float yk[EKF_NUM_OUTPUTS];
886 
888 
892 
893 
894  // [6] Residual covariance:
895  // Sk = H*Pkk_1*H.T + R
896  float Sk[EKF_NUM_OUTPUTS][EKF_NUM_OUTPUTS];
897  float PHT[EKF_NUM_STATES][EKF_NUM_OUTPUTS];
898 
900  MAKE_MATRIX_PTR(PHT_, PHT, EKF_NUM_STATES);
902 
903  // PHT = Pkk_1*H.T
906 
907  // Sk = H*PHT
909 
910  // Sk += R
912 
913 
914  // [7] Near-optimal Kalman gain:
915  // K = Pkk_1*H.T*inv(Sk)
916  float Sk_inv[EKF_NUM_OUTPUTS][EKF_NUM_OUTPUTS];
918 
919  MAKE_MATRIX_PTR(Sk_inv_, Sk_inv, EKF_NUM_OUTPUTS);
921 
922  // Sk_inv = inv(Sk)
923  float_mat_invert(Sk_inv_, Sk_, EKF_NUM_OUTPUTS);
924 
925  // K = PHT*Sk_inv
927 
928 
929  // [8] Updated state estimate
930  // Xkk = Xkk_1 + K*yk
933 
934 
935  // [9] Updated covariance estimate:
936  // Pkk = (I - K*H)*Pkk_1
937 
938  // tmp = K*H
940 
941  // tmp *= -1
943 
944  // tmp += I
945  for (i = 0; i < EKF_NUM_STATES; i++) {
946  tmp_[i][i] += 1;
947  }
948  // P = tmp*Pkk_1
950 }
951 
952 static inline void ekf_prediction_step(const float U[EKF_NUM_INPUTS], const float dt)
953 {
954  // [1] Predicted (a priori) state estimate:
955  float Xkk_1[EKF_NUM_STATES];
956  // Xkk_1 = f(X,U)
957  ekf_f(ekf_X, U, Xkk_1);
958  // Xkk_1 *= dt
959  float_vect_scale(Xkk_1, dt, EKF_NUM_STATES);
960  // Xkk_1 += X
962 
963 
964  // [2] Get matrices
965  float F[EKF_NUM_STATES][EKF_NUM_STATES];
966  float Ld[EKF_NUM_STATES][EKF_NUM_INPUTS];
967  ekf_F(ekf_X, U, F);
968  ekf_L(ekf_X, U, Ld);
969 
970 
971  // [3] Continuous to discrete
972  // Fd = eye(N) + F*dt
973  // Ld = L*dt
974  float Fd[EKF_NUM_STATES][EKF_NUM_STATES];
975 
979 
980  // Fd = I+F*dt/2
983 
984  // Ld = Ld*dt
986 
987 
988  // [4] Predicted covariance estimate:
989  // Pkk_1 = Fd*P*Fd.T + Ld*Q*Ld.T
990  float Pkk_1[EKF_NUM_STATES][EKF_NUM_STATES];
991  float LdT[EKF_NUM_INPUTS][EKF_NUM_STATES];
992  float QLdT[EKF_NUM_INPUTS][EKF_NUM_STATES];
993  float tmp[EKF_NUM_STATES][EKF_NUM_STATES];
994 
995  MAKE_MATRIX_PTR(Pkk_1_, Pkk_1, EKF_NUM_STATES);
998  MAKE_MATRIX_PTR(LdT_, LdT, EKF_NUM_INPUTS);
999  MAKE_MATRIX_PTR(QLdT_, QLdT, EKF_NUM_INPUTS);
1000  MAKE_MATRIX_PTR(tmp_, tmp, EKF_NUM_STATES);
1001 
1002  // Fd = Fd.T
1004 
1005  // tmp = P*Fd
1007 
1008  // Fd = Fd.T
1010 
1011  // Pkk_1 = Fd*tmp
1013 
1014  // LdT = Ld.T
1016 
1017  // QLdT = Q*LdT
1019 
1020  // tmp = Ld*QLdT
1022 
1023  // Pkk_1 += tmp
1025 
1026  // X = Xkk_1
1028 
1029  // P = Pkk_1
1030  float_mat_copy(ekf_P_, Pkk_1_, EKF_NUM_STATES, EKF_NUM_STATES);
1031 }
1032 
1033 static inline void ekf_measurement_step(const float Z[EKF_NUM_OUTPUTS])
1034 {
1035  // Xkk_1 = X
1036  float Xkk_1[EKF_NUM_STATES];
1038 
1039  // Pkk_1 = P
1040  float Pkk_1[EKF_NUM_STATES][EKF_NUM_STATES];
1041  MAKE_MATRIX_PTR(Pkk_1_, Pkk_1, EKF_NUM_STATES);
1043  float_mat_copy(Pkk_1_, ekf_P_, EKF_NUM_STATES, EKF_NUM_STATES);
1044 
1045  // [5] Measurement residual:
1046  // yk = Z - H*Xkk_1
1047  float yk[EKF_NUM_OUTPUTS];
1048 
1050 
1051  float_mat_vect_mul(yk, ekf_H_, Xkk_1, EKF_NUM_OUTPUTS, EKF_NUM_STATES);
1054 
1055 
1056  // [6] Residual covariance:
1057  // Sk = H*Pkk_1*H.T + R
1058  float Sk[EKF_NUM_OUTPUTS][EKF_NUM_OUTPUTS];
1059  float PHT[EKF_NUM_STATES][EKF_NUM_OUTPUTS];
1060 
1061  MAKE_MATRIX_PTR(Sk_, Sk, EKF_NUM_OUTPUTS);
1062  MAKE_MATRIX_PTR(PHT_, PHT, EKF_NUM_STATES);
1064 
1065  // PHT = Pkk_1*H.T
1068 
1069  // Sk = H*PHT
1071 
1072  // Sk += R
1074 
1075 
1076  // [7] Near-optimal Kalman gain:
1077  // K = Pkk_1*H.T*inv(Sk)
1078  float Sk_inv[EKF_NUM_OUTPUTS][EKF_NUM_OUTPUTS];
1080 
1081  MAKE_MATRIX_PTR(Sk_inv_, Sk_inv, EKF_NUM_OUTPUTS);
1083 
1084  // Sk_inv = inv(Sk)
1085  float_mat_invert(Sk_inv_, Sk_, EKF_NUM_OUTPUTS);
1086 
1087  // K = PHT*Sk_inv
1089 
1090 
1091  // [8] Updated state estimate
1092  // Xkk = Xkk_1 + K*yk
1095 
1096 
1097  // [9] Updated covariance estimate:
1098  // Pkk = (I - K*H)*Pkk_1
1099  float tmp[EKF_NUM_STATES][EKF_NUM_STATES];
1100  MAKE_MATRIX_PTR(tmp_, tmp, EKF_NUM_STATES);
1101 
1102  // tmp = K*H
1104 
1105  // tmp *= -1
1107 
1108  // tmp += I
1109  int i;
1110  for (i = 0; i < EKF_NUM_STATES; i++) {
1111  tmp_[i][i] += 1;
1112  }
1113  // P = tmp*Pkk_1
1114  float_mat_mul(ekf_P_, tmp_, Pkk_1_, EKF_NUM_STATES, EKF_NUM_STATES, EKF_NUM_STATES);
1115 }
1116 
1117 
1118 
1119 
1120 
1121 static inline void ekf_run(void)
1122 {
1123  static bool start = false;
1124 
1125 
1126  // Time
1127  t1 = get_sys_time_float();
1128  float dt = t1 - t0;
1129  t0 = t1;
1130 
1131  // Only Start If External Pose is Available
1132  if (!start) {
1133  // ekf starts at the first ev update
1135  start = true;
1136 
1137  // initial guess
1138  ekf_X[0] = ins_ext_pos.ev_pos.x;
1139  ekf_X[1] = ins_ext_pos.ev_pos.y;
1140  ekf_X[2] = ins_ext_pos.ev_pos.z;
1141  ekf_X[6] = ins_ext_pos.ev_att.phi;
1143  ekf_X[8] = ins_ext_pos.ev_att.psi;
1144  }
1145  }
1146 
1147  // set input values
1148  if (ins_ext_pos.has_new_acc) {
1149  ekf_U[0] = ins_ext_pos.accels_f.x;
1150  ekf_U[1] = ins_ext_pos.accels_f.y;
1151  ekf_U[2] = ins_ext_pos.accels_f.z;
1152  ins_ext_pos.has_new_acc = false;
1153  } else {
1154  DEBUG_PRINT("ekf missing acc\n");
1155  }
1156  if (ins_ext_pos.has_new_gyro) {
1157  ekf_U[3] = ins_ext_pos.gyros_f.p;
1158  ekf_U[4] = ins_ext_pos.gyros_f.q;
1159  ekf_U[5] = ins_ext_pos.gyros_f.r;
1160  ins_ext_pos.has_new_gyro = false;
1161  } else {
1162  DEBUG_PRINT("ekf missing gyro\n");
1163  }
1164 
1165  if (start) {
1166 
1167  // prediction step
1168  DEBUG_PRINT("ekf prediction step U = %f, %f, %f, %f, %f, %f dt = %f \n", ekf_U[0], ekf_U[1], ekf_U[2], ekf_U[3],
1169  ekf_U[4], ekf_U[5], dt);
1171 
1172  // measurement step
1174 
1175  //fix psi
1176  static float last_psi = 0;
1177  float delta_psi = ins_ext_pos.ev_att.psi - last_psi;
1178  last_psi = ins_ext_pos.ev_att.psi;
1179 
1180  if (delta_psi > M_PI) {
1181  delta_psi -= 2 * M_PI;
1182  } else if (delta_psi < -M_PI) {
1183  delta_psi += 2 * M_PI;
1184  }
1185 
1186 
1187  ekf_Z[0] = ins_ext_pos.ev_pos.x;
1188  ekf_Z[1] = ins_ext_pos.ev_pos.y;
1189  ekf_Z[2] = ins_ext_pos.ev_pos.z;
1190  ekf_Z[3] = ins_ext_pos.ev_att.phi;
1192  ekf_Z[5] += delta_psi;
1193  ins_ext_pos.has_new_ext_pose = false;
1194 
1195  DEBUG_PRINT("ekf measurement step Z = %f, %f, %f, %f \n", ekf_Z[0], ekf_Z[1], ekf_Z[2], ekf_Z[3]);
1197  }
1198  }
1199 
1200  // Export Results
1201  struct NedCoor_f ned_pos;
1202  ned_pos.x = ekf_X[0];
1203  ned_pos.y = ekf_X[1];
1204  ned_pos.z = ekf_X[2];
1205 
1206  struct NedCoor_f ned_speed;
1207  ned_speed.x = ekf_X[3];
1208  ned_speed.y = ekf_X[4];
1209  ned_speed.z = ekf_X[5];
1210 
1211  struct FloatEulers ned_to_body_eulers;
1212  ned_to_body_eulers.phi = ekf_X[6];
1213  ned_to_body_eulers.theta = ekf_X[7];
1214  ned_to_body_eulers.psi = ekf_X[8];
1215 
1216  struct FloatRates rates = { ekf_U[3] - ekf_X[12], ekf_U[4] - ekf_X[13], ekf_U[5] - ekf_X[14] };
1217 
1218  struct FloatVect3 accel;
1219  struct FloatVect3 accel_ned_f;
1220  accel.x = ekf_U[0] - ekf_X[9];
1221  accel.y = ekf_U[1] - ekf_X[10];
1222  accel.z = ekf_U[2] - ekf_X[11];
1223 
1224  // Export Body Accelerations (without bias)
1225  struct Int32Vect3 accel_i;
1226  ACCELS_BFP_OF_REAL(accel_i, accel);
1227  stateSetAccelBody_i(&accel_i);
1228 
1229 
1230  struct FloatRMat *ned_to_body_rmat_f = stateGetNedToBodyRMat_f();
1231  float_rmat_transp_vmult(&accel_ned_f, ned_to_body_rmat_f, &accel);
1232  accel_ned_f.z += 9.81;
1233 
1234  stateSetPositionNed_f(&ned_pos);
1235  stateSetSpeedNed_f(&ned_speed);
1236  stateSetNedToBodyEulers_f(&ned_to_body_eulers);
1237  stateSetBodyRates_f(&rates);
1238  stateSetAccelNed_f((struct NedCoor_f *)&accel_ned_f);
1239 
1240 }
1241 
1242 
1243 
1248 void ins_ext_pos_log_header(FILE *file)
1249 {
1250  fprintf(file,
1251  "ekf_X1,ekf_X2,ekf_X3,ekf_X4,ekf_X5,ekf_X6,ekf_X7,ekf_X8,ekf_X9,ekf_X10,ekf_X11,ekf_X12,ekf_X13,ekf_X14,ekf_X15,");
1252  fprintf(file, "ekf_U1,ekf_U2,ekf_U3,ekf_U4,ekf_U5,ekf_U6,");
1253  fprintf(file, "ekf_Z1,ekf_Z2,ekf_Z3,ekf_Z4,");
1254 }
1255 
1256 void ins_ext_pos_log_data(FILE *file)
1257 {
1258  fprintf(file, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,", ekf_X[0], ekf_X[1], ekf_X[2], ekf_X[3], ekf_X[4],
1259  ekf_X[5], ekf_X[6], ekf_X[7], ekf_X[8], ekf_X[9], ekf_X[10], ekf_X[11], ekf_X[12], ekf_X[13], ekf_X[14]);
1260  fprintf(file, "%f,%f,%f,%f,%f,%f,", ekf_U[0], ekf_U[1], ekf_U[2], ekf_U[3], ekf_U[4], ekf_U[5]);
1261  fprintf(file, "%f,%f,%f,%f,", ekf_Z[0], ekf_Z[1], ekf_Z[2], ekf_Z[3]);
1262 }
Main include for ABI (AirBorneInterface).
Event structure to store callbacks in a linked list.
Definition: abi_common.h:66
float q
in rad/s
float phi
in radians
float p
in rad/s
float r
in rad/s
float theta
in radians
float psi
in radians
static void float_vect_add(float *a, const float *b, const int n)
a += b
static void float_vect_sum(float *o, const float *a, const float *b, const int n)
o = a + b
static void float_vect_smul(float *o, const float *a, const float s, const int n)
o = a * s
static void float_mat_vect_mul(float *o, float **a, float *b, int m, int n)
o = a * b
static void float_vect_scale(float *a, const float s, const int n)
a *= s
static void float_mat_sum_scaled(float **a, float **b, float k, int m, int n)
a += k*b, where k is a scalar value
static void float_mat_mul_copy(float **o, float **a, float **b, int m, int n, int l)
o = a * b
static void float_mat_copy(float **a, float **b, int m, int n)
a = b
static void float_vect_copy(float *a, const float *b, const int n)
a = b
static void float_mat_mul(float **o, float **a, float **b, int m, int n, int l)
o = a * b
static void float_mat_transpose(float **o, float **a, int n, int m)
transpose non-square matrix
#define MAKE_MATRIX_PTR(_ptr, _mat, _rows)
Make a pointer to a matrix of _rows lines.
void float_mat_invert(float **o, float **mat, int n)
Calculate inverse of any n x n matrix (passed as C array) o = mat^-1 Algorithm verified with Matlab.
void float_rmat_transp_vmult(struct FloatVect3 *vb, struct FloatRMat *m_b2a, struct FloatVect3 *va)
rotate 3D vector by transposed rotation matrix.
void float_eulers_of_quat(struct FloatEulers *e, struct FloatQuat *q)
euler rotation 'ZYX'
static void float_mat_transpose_square(float **a, int n)
transpose square matrix
static void float_mat_diagonal_scal(float **o, float v, int n)
Make an n x n identity matrix (for matrix passed as array)
static void float_mat_scale(float **a, float k, int m, int n)
a *= k, where k is a scalar value
euler angles
Roation quaternion.
rotation matrix
angular rates
#define ACCELS_BFP_OF_REAL(_ef, _ei)
Definition: pprz_algebra.h:801
#define RATES_FLOAT_OF_BFP(_rf, _ri)
Definition: pprz_algebra.h:759
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:795
angular rates
int32_t lat
in degrees*1e7
int32_t hmsl
Height above mean sea level in mm.
int32_t alt
in millimeters above WGS84 reference ellipsoid
int32_t z
Down.
int32_t z
in centimeters
struct LlaCoor_i lla
Reference point in lla.
int32_t x
in centimeters
int32_t y
East.
struct EcefCoor_i ecef
Reference point in ecef.
int32_t y
in centimeters
int32_t lon
in degrees*1e7
int32_t x
North.
void ecef_of_lla_i(struct EcefCoor_i *out, struct LlaCoor_i *in)
Convert a LLA to ECEF.
void ltp_def_from_ecef_i(struct LtpDef_i *def, struct EcefCoor_i *ecef)
vector in EarthCenteredEarthFixed coordinates
vector in Latitude, Longitude and Altitude
definition of the local (flat earth) coordinate system
vector in North East Down coordinates
static void stateSetAccelNed_f(struct NedCoor_f *ned_accel)
Set acceleration in NED coordinates (float).
Definition: state.h:1002
static struct FloatRMat * stateGetNedToBodyRMat_f(void)
Get vehicle body attitude rotation matrix (float).
Definition: state.h:1137
static void stateSetNedToBodyEulers_f(struct FloatEulers *ned_to_body_eulers)
Set vehicle body attitude from euler angles (float).
Definition: state.h:1105
static void stateSetPositionNed_f(struct NedCoor_f *ned_pos)
Set position from local NED coordinates (float).
Definition: state.h:598
static void stateSetLocalOrigin_i(struct LtpDef_i *ltp_def)
Set the local (flat earth) coordinate frame origin (int).
Definition: state.h:457
static void stateSetBodyRates_f(struct FloatRates *body_rate)
Set vehicle body angular rate (float).
Definition: state.h:1181
static void stateSetAccelBody_i(struct Int32Vect3 *body_accel)
Set acceleration in Body coordinates (int).
Definition: state.h:855
static void stateSetSpeedNed_f(struct NedCoor_f *ned_speed)
Set ground speed in local NED coordinates (float).
Definition: state.h:809
Inertial Measurement Unit interface.
Integrated Navigation System interface.
static void ekf_L(const float X[EKF_NUM_STATES], const float U[EKF_NUM_INPUTS], float out[EKF_NUM_STATES][EKF_NUM_INPUTS])
Definition: ins_ext_pose.c:642
struct InsExtPose ins_ext_pos
Definition: ins_ext_pose.c:71
struct FloatRates gyros_f
Definition: ins_ext_pose.c:53
struct FloatEulers ev_att
Definition: ins_ext_pose.c:59
void ins_reset_local_origin(void)
INS local origin reset.
Definition: ins_ext_pose.c:201
static void ekf_f_rk4(const float X[EKF_NUM_STATES], const float U[EKF_NUM_INPUTS], const float dt, float out[EKF_NUM_STATES])
Definition: ins_ext_pose.c:751
static void send_ins(struct transport_tx *trans, struct link_device *dev)
Provide telemetry.
Definition: ins_ext_pose.c:98
void ins_ext_pose_msg_update(uint8_t *buf)
Import External Pose Message.
Definition: ins_ext_pose.c:163
static void ekf_F(const float X[EKF_NUM_STATES], const float U[EKF_NUM_INPUTS], float out[EKF_NUM_STATES][EKF_NUM_STATES])
Definition: ins_ext_pose.c:373
void ins_ext_pos_log_data(FILE *file)
float ekf_P[EKF_NUM_STATES][EKF_NUM_STATES]
Definition: ins_ext_pose.c:282
float ekf_R[EKF_NUM_OUTPUTS][EKF_NUM_OUTPUTS]
Definition: ins_ext_pose.c:284
struct LtpDef_i ltp_def
Definition: ins_ext_pose.c:63
static abi_event accel_ev
Definition: ins_ext_pose.c:134
static void gyro_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro)
Definition: ins_ext_pose.c:142
struct NedCoor_i ltp_pos
Definition: ins_ext_pose.c:66
static void ekf_init(void)
EKF protos.
Definition: ins_ext_pose.c:309
struct FloatVect3 accels_f
Definition: ins_ext_pose.c:54
static void ins_ext_pose_init_from_flightplan(void)
Definition: ins_ext_pose.c:74
bool has_new_gyro
Definition: ins_ext_pose.c:55
float ekf_U[EKF_NUM_INPUTS]
Definition: ins_ext_pose.c:280
void ekf_set_diag(float **a, float *b, int n)
Definition: ins_ext_pose.c:293
float t0
Definition: ins_ext_pose.c:289
void ins_ext_pose_init(void)
Module.
Definition: ins_ext_pose.c:222
struct NedCoor_i ltp_speed
Definition: ins_ext_pose.c:67
static void ekf_measurement_step(const float Z[EKF_NUM_OUTPUTS])
static abi_event gyro_ev
Definition: ins_ext_pose.c:135
static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
Definition: ins_ext_pose.c:114
bool has_new_ext_pose
Definition: ins_ext_pose.c:60
struct FloatVect3 ev_pos
Definition: ins_ext_pose.c:58
struct NedCoor_i ltp_accel
Definition: ins_ext_pose.c:68
static void send_ins_z(struct transport_tx *trans, struct link_device *dev)
Definition: ins_ext_pose.c:106
float ekf_Q[EKF_NUM_INPUTS][EKF_NUM_INPUTS]
Definition: ins_ext_pose.c:283
bool has_new_acc
Definition: ins_ext_pose.c:56
#define DEBUG_PRINT(...)
Definition: ins_ext_pose.c:43
void ins_ext_pose_run(void)
Definition: ins_ext_pose.c:250
void ins_reset_altitude_ref(void)
INS altitude reference reset.
Definition: ins_ext_pose.c:206
#define INS_EXT_POSE_IMU_ID
Import Gyro and Acc from ABI.
Definition: ins_ext_pose.c:130
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
Definition: ins_ext_pose.c:150
float t1
Definition: ins_ext_pose.c:290
static void ekf_step(const float U[EKF_NUM_INPUTS], const float Z[EKF_NUM_OUTPUTS], const float dt)
Definition: ins_ext_pose.c:800
void ins_ext_pos_log_header(FILE *file)
Logging.
float ekf_Z[EKF_NUM_OUTPUTS]
Definition: ins_ext_pose.c:281
static void ekf_prediction_step(const float U[EKF_NUM_INPUTS], const float dt)
Definition: ins_ext_pose.c:952
static void ekf_f(const float X[EKF_NUM_STATES], const float U[EKF_NUM_INPUTS], float out[EKF_NUM_STATES])
Definition: ins_ext_pose.c:334
static void ekf_run(void)
float ekf_X[EKF_NUM_STATES]
Definition: ins_ext_pose.c:279
float ekf_H[EKF_NUM_OUTPUTS][EKF_NUM_STATES]
Definition: ins_ext_pose.c:286
Data for telemetry and LTP origin.
Definition: ins_ext_pose.c:51
Integrated Navigation System interface.
#define EKF_NUM_OUTPUTS
Definition: ins_ext_pose.h:37
#define EKF_NUM_INPUTS
Definition: ins_ext_pose.h:36
#define EKF_NUM_STATES
Definition: ins_ext_pose.h:35
Paparazzi floating point algebra.
float z
in meters
float x
in meters
float y
in meters
vector in North East Down coordinates Units: meters
API to get/set the generic vehicle states.
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
static float get_sys_time_float(void)
Get the time in seconds since startup.
Definition: sys_time.h:138
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:51
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
static float K[9]
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
float b
Definition: wedgebug.c:202