Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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#include "modules/core/abi.h"
37
38#if 0
39#include <stdio.h>
40#define DEBUG_PRINT(...) printf(__VA_ARGS__)
41#else
42#define DEBUG_PRINT(...) {}
43#endif
44
45#ifdef INS_EXT_VISION_ROTATION
47#endif
48
51struct InsExtPose {
52 /* Inputs */
57
63 float ev_time;
64
65 /* Origin */
67
68 /* output LTP NED */
72};
74
75
77{
78
79 struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
81 llh_nav0.lon = NAV_LON0;
82 /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
84
85 struct EcefCoor_i ecef_nav0;
87
91 /* update local ENU coordinates of global waypoints */
93}
94
95
99#if PERIODIC_TELEMETRY
101
109
110static void send_ins_z(struct transport_tx *trans, struct link_device *dev)
111{
112 static float fake_baro_z = 0.0;
114 (float *)&fake_baro_z, &ins_ext_pos.ltp_pos.z,
116}
117
126
142static void send_ahrs_bias(struct transport_tx *trans, struct link_device *dev)
143{
144 float dummy0 = 0.0;
146 &ekf_X[9],
147 &ekf_X[10],
148 &ekf_X[11],
149 &ekf_X[12],
150 &ekf_X[13],
151 &ekf_X[14],
152 &dummy0,
153 &dummy0,
154 &dummy0);
155}
156#endif
157
158
163#ifndef INS_EXT_POSE_IMU_ID
164#define INS_EXT_POSE_IMU_ID ABI_BROADCAST
165#endif
167
170
173
174
175
177 uint32_t stamp __attribute__((unused)),
178 struct Int32Rates *gyro)
179{
182}
183
185 uint32_t stamp __attribute__((unused)),
186 struct Int32Vect3 *accel)
187{
190}
191
192
198{
199 if (DL_EXTERNAL_POSE_ac_id(buf) != AC_ID) { return; } // not for this aircraft
200
201 float enu_x = DL_EXTERNAL_POSE_enu_x(buf);
202 float enu_y = DL_EXTERNAL_POSE_enu_y(buf);
203 float enu_z = DL_EXTERNAL_POSE_enu_z(buf);
204 float enu_xd = DL_EXTERNAL_POSE_enu_xd(buf);
205 float enu_yd = DL_EXTERNAL_POSE_enu_yd(buf);
206 float enu_zd = DL_EXTERNAL_POSE_enu_zd(buf);
207 float quat_i = DL_EXTERNAL_POSE_body_qi(buf);
208 float quat_x = DL_EXTERNAL_POSE_body_qx(buf);
209 float quat_y = DL_EXTERNAL_POSE_body_qy(buf);
210 float quat_z = DL_EXTERNAL_POSE_body_qz(buf);
211
212 DEBUG_PRINT("EXT_UPDATE\n");
213
214 struct FloatQuat orient;
216
217 // Transformation of External Pose. Optitrack motive 2.X Yup
218 orient.qi = quat_i ;
219 orient.qx = quat_y ;
220 orient.qy = quat_x ;
221 orient.qz = -quat_z;
222
223#ifdef INS_EXT_VISION_ROTATION
224 // Rotate the quaternion
225 struct FloatQuat rot_q;
227 orient.qi = rot_q.qi;
228 orient.qx = rot_q.qx;
229 orient.qy = rot_q.qy;
230 orient.qz = rot_q.qz;
231#endif
232
234
245 ins_ext_pos.ev_quat.qi = orient.qi;
246 ins_ext_pos.ev_quat.qx = orient.qx;
247 ins_ext_pos.ev_quat.qy = orient.qy;
248 ins_ext_pos.ev_quat.qz = orient.qz;
249
251
253}
254
258static inline void ekf_init(void);
259static inline void ekf_run(void);
260
266{
267
268 // Initialize inputs
269 ins_ext_pos.has_new_acc = false;
272
273 // Get External Pose Origin From Flightplan
275
276 // Provide telemetry
277#if PERIODIC_TELEMETRY
283#endif
284
285 // Get IMU through ABI
288 // Get External Pose through datalink message: setup in xml
289
290 // Initialize EKF
291 ekf_init();
292}
293
295{
296 ekf_run();
297}
298
299
300
301
302/***************************************************
303 * Kalman Filter.
304 */
305
306
307
308static inline void ekf_f(const float X[EKF_NUM_STATES], const float U[EKF_NUM_INPUTS], float out[EKF_NUM_STATES]);
309static inline void ekf_F(const float X[EKF_NUM_STATES], const float U[EKF_NUM_INPUTS],
310 float out[EKF_NUM_STATES][EKF_NUM_STATES]);
311static inline void ekf_L(const float X[EKF_NUM_STATES], const float U[EKF_NUM_INPUTS],
312 float out[EKF_NUM_STATES][EKF_NUM_INPUTS]);
313
314static inline void ekf_f_rk4(const float X[EKF_NUM_STATES], const float U[EKF_NUM_INPUTS], const float dt,
315 float out[EKF_NUM_STATES]);
316
317static inline void ekf_step(const float U[EKF_NUM_INPUTS], const float Z[EKF_NUM_OUTPUTS], const float dt);
318static inline void ekf_prediction_step(const float U[EKF_NUM_INPUTS], const float dt);
319static inline void ekf_measurement_step(const float Z[EKF_NUM_OUTPUTS]);
320
321
322
329
330float 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}};
331
332
333float t0;
334float t1;
335
336void ekf_set_diag(float **a, float *b, int n);
337void ekf_set_diag(float **a, float *b, int n)
338{
339 int i, j;
340 for (i = 0 ; i < n; i++) {
341 for (j = 0 ; j < n; j++) {
342 if (i == j) {
343 a[i][j] = b[i];
344 } else {
345 a[i][j] = 0.0;
346 }
347 }
348 }
349}
350
351
352
353static inline void ekf_init(void)
354{
355
356 DEBUG_PRINT("ekf init");
357 float X0[EKF_NUM_STATES] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
358 float U0[EKF_NUM_INPUTS] = {0, 0, 0, 0, 0, 0};
359 float Z0[EKF_NUM_OUTPUTS] = {0, 0, 0, 0, 0, 0};
360
361 float Pdiag[EKF_NUM_STATES] = {1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1.};
362 float Qdiag[EKF_NUM_INPUTS] = {1.0, 1.0, 1.0, 0.0173, 4.878e-4, 3.547e-4};//{0.0325, 0.4494, 0.5087, 0.0173, 4.878e-4, 3.547e-4};
363
364 float Rdiag[EKF_NUM_OUTPUTS] = {8.372e-6, 3.832e-6, 4.761e-6, 2.830e-4, 8.684e-6, 7.013e-6};
365
369
376}
377
378static inline void ekf_f(const float X[EKF_NUM_STATES], const float U[EKF_NUM_INPUTS], float out[EKF_NUM_STATES])
379{
380 float x0 = cos(X[8]);
381 float x1 = U[0] - X[9];
382 float x2 = cos(X[7]);
383 float x3 = x1 * x2;
384 float x4 = U[2] - X[11];
385 float x5 = sin(X[6]);
386 float x6 = sin(X[8]);
387 float x7 = x5 * x6;
388 float x8 = sin(X[7]);
389 float x9 = cos(X[6]);
390 float x10 = x0 * x9;
391 float x11 = U[1] - X[10];
392 float x12 = x6 * x9;
393 float x13 = x0 * x5;
394 float x14 = tan(X[7]);
395 float x15 = U[4] - X[13];
396 float x16 = x15 * x5;
397 float x17 = U[5] - X[14];
398 float x18 = x17 * x9;
399 float x19 = 1.0 / x2;
400 out[0] = X[3];
401 out[1] = X[4];
402 out[2] = X[5];
403 out[3] = x0 * x3 + x11 * (-x12 + x13 * x8) + x4 * (x10 * x8 + x7);
404 out[4] = x11 * (x10 + x7 * x8) + x3 * x6 + x4 * (x12 * x8 - x13);
405 out[5] = -x1 * x8 + x11 * x2 * x5 + x2 * x4 * x9 + 9.8100000000000005;
406 out[6] = U[3] - X[12] + x14 * x16 + x14 * x18;
407 out[7] = x15 * x9 - x17 * x5;
408 out[8] = x16 * x19 + x18 * x19;
409 out[9] = 0;
410 out[10] = 0;
411 out[11] = 0;
412 out[12] = 0;
413 out[13] = 0;
414 out[14] = 0;
415}
416
417static inline void ekf_F(const float X[EKF_NUM_STATES], const float U[EKF_NUM_INPUTS],
418 float out[EKF_NUM_STATES][EKF_NUM_STATES])
419{
420 float x0 = U[1] - X[10];
421 float x1 = sin(X[6]);
422 float x2 = sin(X[8]);
423 float x3 = x1 * x2;
424 float x4 = sin(X[7]);
425 float x5 = cos(X[6]);
426 float x6 = cos(X[8]);
427 float x7 = x5 * x6;
428 float x8 = x4 * x7;
429 float x9 = x3 + x8;
430 float x10 = U[2] - X[11];
431 float x11 = x2 * x5;
432 float x12 = x1 * x6;
433 float x13 = x12 * x4;
434 float x14 = x11 - x13;
435 float x15 = U[0] - X[9];
436 float x16 = x15 * x4;
437 float x17 = cos(X[7]);
438 float x18 = x0 * x17;
439 float x19 = x10 * x17;
440 float x20 = x17 * x2;
441 float x21 = x11 * x4;
442 float x22 = x12 - x21;
443 float x23 = -x3 * x4 - x7;
444 float x24 = x17 * x6;
445 float x25 = x17 * x5;
446 float x26 = x1 * x17;
447 float x27 = x4 * x5;
448 float x28 = U[4] - X[13];
449 float x29 = tan(X[7]);
450 float x30 = x29 * x5;
451 float x31 = U[5] - X[14];
452 float x32 = x1 * x29;
453 float x33 = pow(x29, 2) + 1;
454 float x34 = x1 * x28;
455 float x35 = 1.0 / x17;
456 float x36 = x35 * x5;
457 float x37 = x1 * x35;
458 float x38 = pow(x17, -2);
459 out[0][0] = 0;
460 out[0][1] = 0;
461 out[0][2] = 0;
462 out[0][3] = 1;
463 out[0][4] = 0;
464 out[0][5] = 0;
465 out[0][6] = 0;
466 out[0][7] = 0;
467 out[0][8] = 0;
468 out[0][9] = 0;
469 out[0][10] = 0;
470 out[0][11] = 0;
471 out[0][12] = 0;
472 out[0][13] = 0;
473 out[0][14] = 0;
474 out[1][0] = 0;
475 out[1][1] = 0;
476 out[1][2] = 0;
477 out[1][3] = 0;
478 out[1][4] = 1;
479 out[1][5] = 0;
480 out[1][6] = 0;
481 out[1][7] = 0;
482 out[1][8] = 0;
483 out[1][9] = 0;
484 out[1][10] = 0;
485 out[1][11] = 0;
486 out[1][12] = 0;
487 out[1][13] = 0;
488 out[1][14] = 0;
489 out[2][0] = 0;
490 out[2][1] = 0;
491 out[2][2] = 0;
492 out[2][3] = 0;
493 out[2][4] = 0;
494 out[2][5] = 1;
495 out[2][6] = 0;
496 out[2][7] = 0;
497 out[2][8] = 0;
498 out[2][9] = 0;
499 out[2][10] = 0;
500 out[2][11] = 0;
501 out[2][12] = 0;
502 out[2][13] = 0;
503 out[2][14] = 0;
504 out[3][0] = 0;
505 out[3][1] = 0;
506 out[3][2] = 0;
507 out[3][3] = 0;
508 out[3][4] = 0;
509 out[3][5] = 0;
510 out[3][6] = x0 * x9 + x10 * x14;
511 out[3][7] = x12 * x18 - x16 * x6 + x19 * x7;
512 out[3][8] = x0 * x23 + x10 * x22 - x15 * x20;
513 out[3][9] = -x24;
514 out[3][10] = x14;
515 out[3][11] = -x3 - x8;
516 out[3][12] = 0;
517 out[3][13] = 0;
518 out[3][14] = 0;
519 out[4][0] = 0;
520 out[4][1] = 0;
521 out[4][2] = 0;
522 out[4][3] = 0;
523 out[4][4] = 0;
524 out[4][5] = 0;
525 out[4][6] = x0 * (-x12 + x21) + x10 * x23;
526 out[4][7] = x11 * x19 - x16 * x2 + x18 * x3;
527 out[4][8] = x0 * (-x11 + x13) + x10 * x9 + x15 * x24;
528 out[4][9] = -x20;
529 out[4][10] = x23;
530 out[4][11] = x22;
531 out[4][12] = 0;
532 out[4][13] = 0;
533 out[4][14] = 0;
534 out[5][0] = 0;
535 out[5][1] = 0;
536 out[5][2] = 0;
537 out[5][3] = 0;
538 out[5][4] = 0;
539 out[5][5] = 0;
540 out[5][6] = x0 * x25 - x10 * x26;
541 out[5][7] = -x0 * x1 * x4 - x10 * x27 + x17 * (-U[0] + X[9]);
542 out[5][8] = 0;
543 out[5][9] = x4;
544 out[5][10] = -x26;
545 out[5][11] = -x25;
546 out[5][12] = 0;
547 out[5][13] = 0;
548 out[5][14] = 0;
549 out[6][0] = 0;
550 out[6][1] = 0;
551 out[6][2] = 0;
552 out[6][3] = 0;
553 out[6][4] = 0;
554 out[6][5] = 0;
555 out[6][6] = x28 * x30 - x31 * x32;
556 out[6][7] = x31 * x33 * x5 + x33 * x34;
557 out[6][8] = 0;
558 out[6][9] = 0;
559 out[6][10] = 0;
560 out[6][11] = 0;
561 out[6][12] = -1;
562 out[6][13] = -x32;
563 out[6][14] = -x30;
564 out[7][0] = 0;
565 out[7][1] = 0;
566 out[7][2] = 0;
567 out[7][3] = 0;
568 out[7][4] = 0;
569 out[7][5] = 0;
570 out[7][6] = -x34 + x5 * (-U[5] + X[14]);
571 out[7][7] = 0;
572 out[7][8] = 0;
573 out[7][9] = 0;
574 out[7][10] = 0;
575 out[7][11] = 0;
576 out[7][12] = 0;
577 out[7][13] = -x5;
578 out[7][14] = x1;
579 out[8][0] = 0;
580 out[8][1] = 0;
581 out[8][2] = 0;
582 out[8][3] = 0;
583 out[8][4] = 0;
584 out[8][5] = 0;
585 out[8][6] = x28 * x36 - x31 * x37;
586 out[8][7] = x27 * x31 * x38 + x34 * x38 * x4;
587 out[8][8] = 0;
588 out[8][9] = 0;
589 out[8][10] = 0;
590 out[8][11] = 0;
591 out[8][12] = 0;
592 out[8][13] = -x37;
593 out[8][14] = -x36;
594 out[9][0] = 0;
595 out[9][1] = 0;
596 out[9][2] = 0;
597 out[9][3] = 0;
598 out[9][4] = 0;
599 out[9][5] = 0;
600 out[9][6] = 0;
601 out[9][7] = 0;
602 out[9][8] = 0;
603 out[9][9] = 0;
604 out[9][10] = 0;
605 out[9][11] = 0;
606 out[9][12] = 0;
607 out[9][13] = 0;
608 out[9][14] = 0;
609 out[10][0] = 0;
610 out[10][1] = 0;
611 out[10][2] = 0;
612 out[10][3] = 0;
613 out[10][4] = 0;
614 out[10][5] = 0;
615 out[10][6] = 0;
616 out[10][7] = 0;
617 out[10][8] = 0;
618 out[10][9] = 0;
619 out[10][10] = 0;
620 out[10][11] = 0;
621 out[10][12] = 0;
622 out[10][13] = 0;
623 out[10][14] = 0;
624 out[11][0] = 0;
625 out[11][1] = 0;
626 out[11][2] = 0;
627 out[11][3] = 0;
628 out[11][4] = 0;
629 out[11][5] = 0;
630 out[11][6] = 0;
631 out[11][7] = 0;
632 out[11][8] = 0;
633 out[11][9] = 0;
634 out[11][10] = 0;
635 out[11][11] = 0;
636 out[11][12] = 0;
637 out[11][13] = 0;
638 out[11][14] = 0;
639 out[12][0] = 0;
640 out[12][1] = 0;
641 out[12][2] = 0;
642 out[12][3] = 0;
643 out[12][4] = 0;
644 out[12][5] = 0;
645 out[12][6] = 0;
646 out[12][7] = 0;
647 out[12][8] = 0;
648 out[12][9] = 0;
649 out[12][10] = 0;
650 out[12][11] = 0;
651 out[12][12] = 0;
652 out[12][13] = 0;
653 out[12][14] = 0;
654 out[13][0] = 0;
655 out[13][1] = 0;
656 out[13][2] = 0;
657 out[13][3] = 0;
658 out[13][4] = 0;
659 out[13][5] = 0;
660 out[13][6] = 0;
661 out[13][7] = 0;
662 out[13][8] = 0;
663 out[13][9] = 0;
664 out[13][10] = 0;
665 out[13][11] = 0;
666 out[13][12] = 0;
667 out[13][13] = 0;
668 out[13][14] = 0;
669 out[14][0] = 0;
670 out[14][1] = 0;
671 out[14][2] = 0;
672 out[14][3] = 0;
673 out[14][4] = 0;
674 out[14][5] = 0;
675 out[14][6] = 0;
676 out[14][7] = 0;
677 out[14][8] = 0;
678 out[14][9] = 0;
679 out[14][10] = 0;
680 out[14][11] = 0;
681 out[14][12] = 0;
682 out[14][13] = 0;
683 out[14][14] = 0;
684}
685
686static inline void ekf_L(const float X[EKF_NUM_STATES], __attribute__((unused)) const float U[EKF_NUM_INPUTS],
687 float out[EKF_NUM_STATES][EKF_NUM_INPUTS])
688{
689 float x0 = cos(X[7]);
690 float x1 = cos(X[8]);
691 float x2 = sin(X[8]);
692 float x3 = cos(X[6]);
693 float x4 = x2 * x3;
694 float x5 = sin(X[7]);
695 float x6 = sin(X[6]);
696 float x7 = x1 * x6;
697 float x8 = x2 * x6;
698 float x9 = x1 * x3;
699 float x10 = tan(X[7]);
700 float x11 = 1.0 / x0;
701 out[0][0] = 0;
702 out[0][1] = 0;
703 out[0][2] = 0;
704 out[0][3] = 0;
705 out[0][4] = 0;
706 out[0][5] = 0;
707 out[1][0] = 0;
708 out[1][1] = 0;
709 out[1][2] = 0;
710 out[1][3] = 0;
711 out[1][4] = 0;
712 out[1][5] = 0;
713 out[2][0] = 0;
714 out[2][1] = 0;
715 out[2][2] = 0;
716 out[2][3] = 0;
717 out[2][4] = 0;
718 out[2][5] = 0;
719 out[3][0] = -x0 * x1;
720 out[3][1] = x4 - x5 * x7;
721 out[3][2] = -x5 * x9 - x8;
722 out[3][3] = 0;
723 out[3][4] = 0;
724 out[3][5] = 0;
725 out[4][0] = -x0 * x2;
726 out[4][1] = -x5 * x8 - x9;
727 out[4][2] = -x4 * x5 + x7;
728 out[4][3] = 0;
729 out[4][4] = 0;
730 out[4][5] = 0;
731 out[5][0] = x5;
732 out[5][1] = -x0 * x6;
733 out[5][2] = -x0 * x3;
734 out[5][3] = 0;
735 out[5][4] = 0;
736 out[5][5] = 0;
737 out[6][0] = 0;
738 out[6][1] = 0;
739 out[6][2] = 0;
740 out[6][3] = -1;
741 out[6][4] = -x10 * x6;
742 out[6][5] = -x10 * x3;
743 out[7][0] = 0;
744 out[7][1] = 0;
745 out[7][2] = 0;
746 out[7][3] = 0;
747 out[7][4] = -x3;
748 out[7][5] = x6;
749 out[8][0] = 0;
750 out[8][1] = 0;
751 out[8][2] = 0;
752 out[8][3] = 0;
753 out[8][4] = -x11 * x6;
754 out[8][5] = -x11 * x3;
755 out[9][0] = 0;
756 out[9][1] = 0;
757 out[9][2] = 0;
758 out[9][3] = 0;
759 out[9][4] = 0;
760 out[9][5] = 0;
761 out[10][0] = 0;
762 out[10][1] = 0;
763 out[10][2] = 0;
764 out[10][3] = 0;
765 out[10][4] = 0;
766 out[10][5] = 0;
767 out[11][0] = 0;
768 out[11][1] = 0;
769 out[11][2] = 0;
770 out[11][3] = 0;
771 out[11][4] = 0;
772 out[11][5] = 0;
773 out[12][0] = 0;
774 out[12][1] = 0;
775 out[12][2] = 0;
776 out[12][3] = 0;
777 out[12][4] = 0;
778 out[12][5] = 0;
779 out[13][0] = 0;
780 out[13][1] = 0;
781 out[13][2] = 0;
782 out[13][3] = 0;
783 out[13][4] = 0;
784 out[13][5] = 0;
785 out[14][0] = 0;
786 out[14][1] = 0;
787 out[14][2] = 0;
788 out[14][3] = 0;
789 out[14][4] = 0;
790 out[14][5] = 0;
791}
792
793
794
795static inline void ekf_f_rk4(const float X[EKF_NUM_STATES], const float U[EKF_NUM_INPUTS], const float dt,
796 float out[EKF_NUM_STATES])
797{
798 float k1[EKF_NUM_STATES];
799 float k2[EKF_NUM_STATES];
800 float k3[EKF_NUM_STATES];
801 float k4[EKF_NUM_STATES];
802
803 float Xtmp[EKF_NUM_STATES];
804
805 // k1 = f(X,U)
806 ekf_f(X, U, k1);
807
808 // Xtmp = X+dt*k1/2
809 float_vect_smul(Xtmp, k1, dt / 2, EKF_NUM_STATES);
811
812 // k2 = f(Xtmp,U)
813 ekf_f(Xtmp, U, k2);
814
815 // Xtmp = X+dt*k2/2
816 float_vect_smul(Xtmp, k2, dt / 2, EKF_NUM_STATES);
818
819 // k3 = f(Xtmp,U)
820 ekf_f(Xtmp, U, k3);
821
822 // Xtmp = X+dt*k3
825
826 // k4 = f(Xtmp,U)
827 ekf_f(Xtmp, U, k4);
828
829 // out = k2+k3
830 float_vect_sum(out, k2, k3, EKF_NUM_STATES);
831 // out *= 2
833 // out += k1
835 // out += k4
837 // out *= dt/6
838 float_vect_scale(out, dt / 6, EKF_NUM_STATES);
839 // out += X
841}
842
843
844static inline void ekf_step(const float U[EKF_NUM_INPUTS], const float Z[EKF_NUM_OUTPUTS], const float dt)
845{
846 // [1] Predicted (a priori) state estimate:
847 float Xkk_1[EKF_NUM_STATES];
848 ekf_f_rk4(ekf_X, U, dt, Xkk_1);
849
850
851 // [2] Get matrices
854 ekf_F(ekf_X, U, F);
855 ekf_L(ekf_X, U, L);
856
857
858 // [3] Continuous to discrete
859 // Fd = eye(N) + F*dt + F*F*dt**2/2 = I + [I+F*dt/2]*F*dt
860 // Ld = L*dt+F*L*dt**2/2 = [I+F*dt/2]*L*dt
864
870
871 // tmp = I+F*dt/2
874
875 // Ld = tmp*L*dt
878
879 // Fd = tmp*F*dt
882
883 // Fd += I
884 int i;
885 for (i = 0; i < EKF_NUM_STATES; i++) {
886 Fd[i][i] += 1;
887 }
888
889
890 // [4] Predicted covariance estimate:
891 // Pkk_1 = Fd*P*Fd.T + Ld*Q*Ld.T
895
901
902 // Fd = Fd.T
904
905 // tmp = P*Fd
907
908 // Fd = Fd.T
910
911 // Pkk_1 = Fd*tmp
913
914 // LdT = Ld.T
916
917 // QLdT = Q*LdT
919
920 // tmp = Ld*QLdT
922
923 // Pkk_1 += tmp
925
926
927 // [5] Measurement residual:
928 // yk = Z - H*Xkk_1
929 float yk[EKF_NUM_OUTPUTS];
930
932
936
937
938 // [6] Residual covariance:
939 // Sk = H*Pkk_1*H.T + R
942
946
947 // PHT = Pkk_1*H.T
950
951 // Sk = H*PHT
953
954 // Sk += R
956
957
958 // [7] Near-optimal Kalman gain:
959 // K = Pkk_1*H.T*inv(Sk)
962
965
966 // Sk_inv = inv(Sk)
968
969 // K = PHT*Sk_inv
971
972
973 // [8] Updated state estimate
974 // Xkk = Xkk_1 + K*yk
977
978
979 // [9] Updated covariance estimate:
980 // Pkk = (I - K*H)*Pkk_1
981
982 // tmp = K*H
984
985 // tmp *= -1
987
988 // tmp += I
989 for (i = 0; i < EKF_NUM_STATES; i++) {
990 tmp_[i][i] += 1;
991 }
992 // P = tmp*Pkk_1
994}
995
996static inline void ekf_prediction_step(const float U[EKF_NUM_INPUTS], const float dt)
997{
998 // [1] Predicted (a priori) state estimate:
999 float Xkk_1[EKF_NUM_STATES];
1000 // Xkk_1 = f(X,U)
1001 ekf_f(ekf_X, U, Xkk_1);
1002 // Xkk_1 *= dt
1004 // Xkk_1 += X
1006
1007
1008 // [2] Get matrices
1011 ekf_F(ekf_X, U, F);
1012 ekf_L(ekf_X, U, Ld);
1013
1014
1015 // [3] Continuous to discrete
1016 // Fd = eye(N) + F*dt
1017 // Ld = L*dt
1019
1023
1024 // Fd = I+F*dt/2
1027
1028 // Ld = Ld*dt
1030
1031
1032 // [4] Predicted covariance estimate:
1033 // Pkk_1 = Fd*P*Fd.T + Ld*Q*Ld.T
1038
1045
1046 // Fd = Fd.T
1048
1049 // tmp = P*Fd
1051
1052 // Fd = Fd.T
1054
1055 // Pkk_1 = Fd*tmp
1057
1058 // LdT = Ld.T
1060
1061 // QLdT = Q*LdT
1063
1064 // tmp = Ld*QLdT
1066
1067 // Pkk_1 += tmp
1069
1070 // X = Xkk_1
1072
1073 // P = Pkk_1
1075}
1076
1077static inline void ekf_measurement_step(const float Z[EKF_NUM_OUTPUTS])
1078{
1079 // Xkk_1 = X
1080 float Xkk_1[EKF_NUM_STATES];
1082
1083 // Pkk_1 = P
1088
1089 // [5] Measurement residual:
1090 // yk = Z - H*Xkk_1
1091 float yk[EKF_NUM_OUTPUTS];
1092
1094
1098
1099
1100 // [6] Residual covariance:
1101 // Sk = H*Pkk_1*H.T + R
1104
1108
1109 // PHT = Pkk_1*H.T
1112
1113 // Sk = H*PHT
1115
1116 // Sk += R
1118
1119
1120 // [7] Near-optimal Kalman gain:
1121 // K = Pkk_1*H.T*inv(Sk)
1124
1127
1128 // Sk_inv = inv(Sk)
1130
1131 // K = PHT*Sk_inv
1133
1134
1135 // [8] Updated state estimate
1136 // Xkk = Xkk_1 + K*yk
1139
1140
1141 // [9] Updated covariance estimate:
1142 // Pkk = (I - K*H)*Pkk_1
1145
1146 // tmp = K*H
1148
1149 // tmp *= -1
1151
1152 // tmp += I
1153 int i;
1154 for (i = 0; i < EKF_NUM_STATES; i++) {
1155 tmp_[i][i] += 1;
1156 }
1157 // P = tmp*Pkk_1
1159}
1160
1161
1162
1163
1164
1165static inline void ekf_run(void)
1166{
1167 static bool start = false;
1168
1169
1170 // Time
1172 float dt = t1 - t0;
1173 t0 = t1;
1174
1175 // Only Start If External Pose is Available
1176 if (!start) {
1177 // ekf starts at the first ev update
1179 start = true;
1180
1181 // initial guess
1188 }
1189 }
1190
1191 // set input values
1196 ins_ext_pos.has_new_acc = false;
1197 } else {
1198 DEBUG_PRINT("ekf missing acc\n");
1199 }
1204 ins_ext_pos.has_new_gyro = false;
1205 } else {
1206 DEBUG_PRINT("ekf missing gyro\n");
1207 }
1208
1209 if (start) {
1210
1211 // prediction step
1212 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],
1213 ekf_U[4], ekf_U[5], dt);
1215
1216 // measurement step
1218
1219 //fix psi
1220 static float last_psi = 0;
1223
1224 if (delta_psi > M_PI) {
1225 delta_psi -= 2 * M_PI;
1226 } else if (delta_psi < -M_PI) {
1227 delta_psi += 2 * M_PI;
1228 }
1229
1230
1236 ekf_Z[5] += delta_psi;
1238
1239 DEBUG_PRINT("ekf measurement step Z = %f, %f, %f, %f \n", ekf_Z[0], ekf_Z[1], ekf_Z[2], ekf_Z[3]);
1241 }
1242 }
1243
1244 // Export Results
1245 struct NedCoor_f ned_pos;
1246 ned_pos.x = ekf_X[0];
1247 ned_pos.y = ekf_X[1];
1248 ned_pos.z = ekf_X[2];
1249
1250 struct NedCoor_f ned_speed;
1251 ned_speed.x = ekf_X[3];
1252 ned_speed.y = ekf_X[4];
1253 ned_speed.z = ekf_X[5];
1254
1257 ned_to_body_eulers.theta = ekf_X[7];
1258 ned_to_body_eulers.psi = ekf_X[8];
1259
1260 struct FloatRates rates = { ekf_U[3] - ekf_X[12], ekf_U[4] - ekf_X[13], ekf_U[5] - ekf_X[14] };
1261
1262 struct FloatVect3 accel;
1263 struct FloatVect3 accel_ned_f;
1264 accel.x = ekf_U[0] - ekf_X[9];
1265 accel.y = ekf_U[1] - ekf_X[10];
1266 accel.z = ekf_U[2] - ekf_X[11];
1267
1268 // Export Body Accelerations (without bias)
1269 struct Int32Vect3 accel_i;
1270 ACCELS_BFP_OF_REAL(accel_i, accel);
1272
1273
1276 accel_ned_f.z += 9.81;
1277
1283
1284}
1285
1286
1287
1293{
1294 fprintf(file,
1295 "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,");
1296 fprintf(file, "ekf_U1,ekf_U2,ekf_U3,ekf_U4,ekf_U5,ekf_U6,");
1297 fprintf(file, "ekf_Z1,ekf_Z2,ekf_Z3,ekf_Z4,");
1298}
1299
1301{
1302 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],
1303 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]);
1304 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]);
1305 fprintf(file, "%f,%f,%f,%f,", ekf_Z[0], ekf_Z[1], ekf_Z[2], ekf_Z[3]);
1306}
Main include for ABI (AirBorneInterface).
Event structure to store callbacks in a linked list.
Definition abi_common.h:67
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
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
void float_quat_comp(struct FloatQuat *a2c, struct FloatQuat *a2b, struct FloatQuat *b2c)
Composition (multiplication) of two quaternions.
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)
#define RATES_FLOAT_OF_BFP(_rf, _ri)
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
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(uint16_t id, struct NedCoor_f *ned_accel)
Set acceleration in NED coordinates (float).
Definition state.h:1147
static void stateSetAccelBody_i(uint16_t id, struct Int32Vect3 *body_accel)
Set acceleration in Body coordinates (int).
Definition state.h:1167
static struct FloatRMat * stateGetNedToBodyRMat_f(void)
Get vehicle body attitude rotation matrix (float).
Definition state.h:1300
static void stateSetNedToBodyEulers_f(uint16_t id, struct FloatEulers *ned_to_body_eulers)
Set vehicle body attitude from euler angles (float).
Definition state.h:1267
static void stateSetPositionNed_f(uint16_t id, struct NedCoor_f *ned_pos)
Set position from local NED coordinates (float).
Definition state.h:718
static void stateSetLocalOrigin_i(uint16_t id, struct LtpDef_i *ltp_def)
Set the local (flat earth) coordinate frame origin (int).
Definition state.h:519
static void stateSetBodyRates_f(uint16_t id, struct FloatRates *body_rate)
Set vehicle body angular rate (float).
Definition state.h:1346
static void stateSetSpeedNed_f(uint16_t id, struct NedCoor_f *ned_speed)
Set ground speed in local NED coordinates (float).
Definition state.h:947
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])
struct InsExtPose ins_ext_pos
struct FloatRates gyros_f
struct FloatEulers ev_att
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])
struct FloatVect3 ev_vel
static void send_ins(struct transport_tx *trans, struct link_device *dev)
Provide telemetry.
void ins_ext_pose_msg_update(uint8_t *buf)
Import External Pose Message.
static void ekf_F(const float X[EKF_NUM_STATES], const float U[EKF_NUM_INPUTS], float out[EKF_NUM_STATES][EKF_NUM_STATES])
void ins_ext_pos_log_data(FILE *file)
float ekf_P[EKF_NUM_STATES][EKF_NUM_STATES]
float ekf_R[EKF_NUM_OUTPUTS][EKF_NUM_OUTPUTS]
struct LtpDef_i ltp_def
static abi_event accel_ev
static void gyro_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro)
struct FloatQuat ev_quat
struct NedCoor_i ltp_pos
static void ekf_init(void)
EKF protos.
struct FloatVect3 accels_f
static void ins_ext_pose_init_from_flightplan(void)
bool has_new_gyro
float ekf_U[EKF_NUM_INPUTS]
void ekf_set_diag(float **a, float *b, int n)
float t0
void ins_ext_pose_init(void)
Module.
struct NedCoor_i ltp_speed
static void ekf_measurement_step(const float Z[EKF_NUM_OUTPUTS])
static abi_event gyro_ev
static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
bool has_new_ext_pose
struct FloatVect3 ev_pos
struct NedCoor_i ltp_accel
static void send_ins_z(struct transport_tx *trans, struct link_device *dev)
float ekf_Q[EKF_NUM_INPUTS][EKF_NUM_INPUTS]
bool has_new_acc
#define DEBUG_PRINT(...)
void ins_ext_pose_run(void)
#define INS_EXT_POSE_IMU_ID
Import Gyro and Acc from ABI.
float ev_time
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
float t1
static void ekf_step(const float U[EKF_NUM_INPUTS], const float Z[EKF_NUM_OUTPUTS], const float dt)
void ins_ext_pos_log_header(FILE *file)
Logging.
float ekf_Z[EKF_NUM_OUTPUTS]
static void ekf_prediction_step(const float U[EKF_NUM_INPUTS], const float dt)
static void send_external_pose_down(struct transport_tx *trans, struct link_device *dev)
static void ekf_f(const float X[EKF_NUM_STATES], const float U[EKF_NUM_INPUTS], float out[EKF_NUM_STATES])
static void ekf_run(void)
float ekf_X[EKF_NUM_STATES]
float ekf_H[EKF_NUM_OUTPUTS][EKF_NUM_STATES]
static void send_ahrs_bias(struct transport_tx *trans, struct link_device *dev)
Data for telemetry and LTP origin.
Integrated Navigation System interface.
#define EKF_NUM_OUTPUTS
#define EKF_NUM_INPUTS
#define EKF_NUM_STATES
uint16_t foo
Definition main_demo5.c:58
void waypoints_localize_all(void)
update local ENU coordinates of global waypoints
Definition waypoints.c:357
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF)
Paparazzi floating point algebra.
float x
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.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
float b
Definition wedgebug.c:202