Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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"
32#include "math/pprz_algebra.h"
34#include "modules/imu/imu.h"
35#include "modules/ahrs/ahrs.h"
36#include "modules/ins/ins.h"
37#include "generated/flight_plan.h"
38#include "modules/core/abi.h"
39
40#ifndef INS_EXT_POSE_P0
41#define INS_EXT_POSE_P0 {1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1., 1.}
42#endif
43
44#ifndef INS_EXT_POSE_Q_NOISE
45#define INS_EXT_POSE_Q_NOISE {1.0, 1.0, 1.0, 0.0173, 4.878e-4, 3.547e-4}
46#endif
47
48#ifndef INS_EXT_POSE_R_NOISE
49#define INS_EXT_POSE_R_NOISE {8.372e-6, 3.832e-6, 4.761e-6, 2.830e-4, 8.684e-6, 7.013e-6}
50#endif
51
52#if INS_EXT_POSE_DEBUG_PRINT
53#include <stdio.h>
54#define DEBUG_PRINT(...) printf(__VA_ARGS__)
55#else
56#define DEBUG_PRINT(...) {}
57#endif
58
59#ifdef INS_EXT_VISION_ROTATION
61#endif
62
65struct InsExtPose {
66 /* Inputs */
71
77 float ev_time;
78
79 /* Origin */
81
82 /* output LTP NED */
86
87 /* status */
88 bool started;
89};
90
92
93
95{
96 struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
98 llh_nav0.lon = NAV_LON0;
99 /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
100 llh_nav0.alt = NAV_ALT0 + NAV_MSL0;
101
102 struct EcefCoor_i ecef_nav0;
104
108 /* update local ENU coordinates of global waypoints */
110}
111
112
116#if PERIODIC_TELEMETRY
118
126
127static void send_ins_z(struct transport_tx *trans, struct link_device *dev)
128{
129 static float fake_baro_z = 0.0;
131 (float *)&fake_baro_z, &ins_ext_pose.ltp_pos.z,
133}
134
143
144static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
145{
148 uint16_t value = 0;
149
150 if (ins_ext_pose.started) {
151 state_filter_mode = 3; // OK
152 }
154}
155
171
172static void send_ahrs_bias(struct transport_tx *trans, struct link_device *dev)
173{
174 float dummy0 = 0.0;
176 &ekf_X[9],
177 &ekf_X[10],
178 &ekf_X[11],
179 &ekf_X[12],
180 &ekf_X[13],
181 &ekf_X[14],
182 &dummy0,
183 &dummy0,
184 &dummy0);
185}
186#endif
187
188
193#ifndef INS_EXT_POSE_IMU_ID
194#define INS_EXT_POSE_IMU_ID ABI_BROADCAST
195#endif
197
200
203
204
206 uint32_t stamp __attribute__((unused)),
207 struct Int32Rates *gyro)
208{
211}
212
214 uint32_t stamp __attribute__((unused)),
215 struct Int32Vect3 *accel)
216{
219}
220
221
227{
228 if (DL_EXTERNAL_POSE_ac_id(buf) != AC_ID) { return; } // not for this aircraft
229
230 struct EnuCoor_f enu_pos, enu_vel;
237 float quat_i = DL_EXTERNAL_POSE_body_qi(buf);
238 float quat_x = DL_EXTERNAL_POSE_body_qx(buf);
239 float quat_y = DL_EXTERNAL_POSE_body_qy(buf);
240 float quat_z = DL_EXTERNAL_POSE_body_qz(buf);
241
242 DEBUG_PRINT("EXT_UPDATE\n");
243
244 struct FloatQuat orient;
246
247 // Transformation of External Pose. Optitrack motive 2.X Yup
248 orient.qi = quat_i ;
249 orient.qx = quat_y ;
250 orient.qy = quat_x ;
251 orient.qz = -quat_z;
252
253#ifdef INS_EXT_VISION_ROTATION
254 // Rotate the quaternion
255 struct FloatQuat rot_q;
257 QUAT_COPY(orient, rot_q);
258#endif
259
261
267
269
271}
272
276static void ekf_init(void);
277static void ekf_run(void);
278
313
315{
316 ekf_run();
317}
318
319
320
321
322/***************************************************
323 * Kalman Filter.
324 */
325static void ekf_f(const float X[EKF_NUM_STATES],
326 const float U[EKF_NUM_INPUTS],
327 float out[EKF_NUM_STATES]);
328static void ekf_F(const float X[EKF_NUM_STATES],
329 const float U[EKF_NUM_INPUTS],
330 float out[EKF_NUM_STATES][EKF_NUM_STATES]);
331static void ekf_L(const float X[EKF_NUM_STATES],
332 const float U[EKF_NUM_INPUTS],
333 float out[EKF_NUM_STATES][EKF_NUM_INPUTS]);
334
335static void ekf_prediction_step(const float U[EKF_NUM_INPUTS], const float dt);
336static void ekf_measurement_step(const float Z[EKF_NUM_OUTPUTS]);
337
338
339
341static float ekf_U[EKF_NUM_INPUTS];
342static float ekf_Z[EKF_NUM_OUTPUTS];
346
348 {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
349 {0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
350 {0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
351 {0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0},
352 {0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0},
353 {0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0}};
354
355
356static float t0;
357static float t1;
358
359static void ekf_set_diag(float **a, float *b, int n)
360{
361 int i, j;
362 for (i = 0 ; i < n; i++) {
363 for (j = 0 ; j < n; j++) {
364 if (i == j) {
365 a[i][j] = b[i];
366 } else {
367 a[i][j] = 0.0;
368 }
369 }
370 }
371}
372
373
374
375static void ekf_init(void)
376{
377
378 DEBUG_PRINT("ekf init");
379 float X0[EKF_NUM_STATES] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
380 float U0[EKF_NUM_INPUTS] = {0, 0, 0, 0, 0, 0};
381 float Z0[EKF_NUM_OUTPUTS] = {0, 0, 0, 0, 0, 0};
382
386
390
397}
398
399static void ekf_f(const float X[EKF_NUM_STATES], const float U[EKF_NUM_INPUTS], float out[EKF_NUM_STATES])
400{
401 const float x0 = cosf(X[EKF_X_PSI]);
402 const float x1 = U[EKF_U_ACC_X] - X[EKF_X_A_BIAS_X];
403 const float x2 = cosf(X[EKF_X_THETA]);
404 const float x3 = x1 * x2;
405 const float x4 = U[EKF_U_ACC_Z] - X[EKF_X_A_BIAS_Z];
406 const float x5 = sinf(X[EKF_X_PHI]);
407 const float x6 = sinf(X[EKF_X_PSI]);
408 const float x7 = x5 * x6;
409 const float x8 = sinf(X[EKF_X_THETA]);
410 const float x9 = cosf(X[EKF_X_PHI]);
411 const float x10 = x0 * x9;
412 const float x11 = U[EKF_U_ACC_Y] - X[EKF_X_A_BIAS_Y];
413 const float x12 = x6 * x9;
414 const float x13 = x0 * x5;
415 const float x14 = tanf(X[EKF_X_THETA]);
416 const float x15 = U[EKF_U_GYRO_Q] - X[EKF_X_G_BIAS_Q];
417 const float x16 = x15 * x5;
418 const float x17 = U[EKF_U_GYRO_R] - X[EKF_X_G_BIAS_R];
419 const float x18 = x17 * x9;
420 const float x19 = 1.0f / x2; // FIXME prevent div by zero
421 out[EKF_X_POS_X] = X[EKF_X_VEL_X];
422 out[EKF_X_POS_Y] = X[EKF_X_VEL_Y];
423 out[EKF_X_POS_Z] = X[EKF_X_VEL_Z];
424 out[EKF_X_VEL_X] = x0 * x3 + x11 * (-x12 + x13 * x8) + x4 * (x10 * x8 + x7);
425 out[EKF_X_VEL_Y] = x11 * (x10 + x7 * x8) + x3 * x6 + x4 * (x12 * x8 - x13);
426 out[EKF_X_VEL_Z] = -x1 * x8 + x11 * x2 * x5 + x2 * x4 * x9 + 9.81f;
427 out[EKF_X_PHI] = U[EKF_U_GYRO_P] - X[EKF_X_G_BIAS_P] + x14 * x16 + x14 * x18;
428 out[EKF_X_THETA] = x15 * x9 - x17 * x5;
429 out[EKF_X_PSI] = x16 * x19 + x18 * x19;
430 out[EKF_X_A_BIAS_X] = 0.f;
431 out[EKF_X_A_BIAS_Y] = 0.f;
432 out[EKF_X_A_BIAS_Z] = 0.f;
433 out[EKF_X_G_BIAS_P] = 0.f;
434 out[EKF_X_G_BIAS_Q] = 0.f;
435 out[EKF_X_G_BIAS_R] = 0.f;
436}
437
438static void ekf_F(const float X[EKF_NUM_STATES], const float U[EKF_NUM_INPUTS],
439 float out[EKF_NUM_STATES][EKF_NUM_STATES])
440{
441 const float x0 = U[EKF_U_ACC_Y] - X[EKF_X_A_BIAS_Y];
442 const float x1 = sinf(X[EKF_X_PHI]);
443 const float x2 = sinf(X[EKF_X_PSI]);
444 const float x3 = x1 * x2;
445 const float x4 = sinf(X[EKF_X_THETA]);
446 const float x5 = cosf(X[EKF_X_PHI]);
447 const float x6 = cosf(X[EKF_X_PSI]);
448 const float x7 = x5 * x6;
449 const float x8 = x4 * x7;
450 const float x9 = x3 + x8;
451 const float x10 = U[EKF_U_ACC_Z] - X[EKF_X_A_BIAS_Z];
452 const float x11 = x2 * x5;
453 const float x12 = x1 * x6;
454 const float x13 = x12 * x4;
455 const float x14 = x11 - x13;
456 const float x15 = U[EKF_U_ACC_X] - X[EKF_X_A_BIAS_X];
457 const float x16 = x15 * x4;
458 const float x17 = cosf(X[EKF_X_THETA]);
459 const float x18 = x0 * x17;
460 const float x19 = x10 * x17;
461 const float x20 = x17 * x2;
462 const float x21 = x11 * x4;
463 const float x22 = x12 - x21;
464 const float x23 = -x3 * x4 - x7;
465 const float x24 = x17 * x6;
466 const float x25 = x17 * x5;
467 const float x26 = x1 * x17;
468 const float x27 = x4 * x5;
469 const float x28 = U[EKF_U_GYRO_Q] - X[EKF_X_G_BIAS_Q];
470 const float x29 = tanf(X[EKF_X_THETA]);
471 const float x30 = x29 * x5;
472 const float x31 = U[EKF_U_GYRO_R] - X[EKF_X_G_BIAS_R];
473 const float x32 = x1 * x29;
474 const float x33 = powf(x29, 2.f) + 1.f;
475 const float x34 = x1 * x28;
476 const float x35 = 1.0f / x17;
477 const float x36 = x35 * x5;
478 const float x37 = x1 * x35;
479 const float x38 = powf(x17, -2.f);
480
481 memset(out, 0, (EKF_NUM_STATES*EKF_NUM_STATES) * (sizeof **out));
482 out[EKF_X_POS_X][EKF_X_VEL_X] = 1.f;
483 out[EKF_X_POS_Y][EKF_X_VEL_Y] = 1.f;
484 out[EKF_X_POS_Z][EKF_X_VEL_Z] = 1.f;
485 out[EKF_X_VEL_X][EKF_X_PHI] = x0 * x9 + x10 * x14;
486 out[EKF_X_VEL_X][EKF_X_THETA] = x12 * x18 - x16 * x6 + x19 * x7;
487 out[EKF_X_VEL_X][EKF_X_PSI] = x0 * x23 + x10 * x22 - x15 * x20;
490 out[EKF_X_VEL_X][EKF_X_A_BIAS_Z] = -x3 - x8;
491 out[EKF_X_VEL_Y][EKF_X_PHI] = x0 * (-x12 + x21) + x10 * x23;
492 out[EKF_X_VEL_Y][EKF_X_THETA] = x11 * x19 - x16 * x2 + x18 * x3;
493 out[EKF_X_VEL_Y][EKF_X_PSI] = x0 * (-x11 + x13) + x10 * x9 + x15 * x24;
497 out[EKF_X_VEL_Z][EKF_X_PHI] = x0 * x25 - x10 * x26;
498 out[EKF_X_VEL_Z][EKF_X_THETA] = -x0 * x1 * x4 - x10 * x27 + x17 * (-U[EKF_U_ACC_X] + X[EKF_X_A_BIAS_X]);
502 out[EKF_X_PHI][EKF_X_PHI] = x28 * x30 - x31 * x32;
503 out[EKF_X_PHI][EKF_X_THETA] = x31 * x33 * x5 + x33 * x34;
504 out[EKF_X_PHI][EKF_X_G_BIAS_P] = -1.f;
509 out[EKF_X_THETA][EKF_X_G_BIAS_R] = x1;
510 out[EKF_X_PSI][EKF_X_PHI] = x28 * x36 - x31 * x37;
511 out[EKF_X_PSI][EKF_X_THETA] = x27 * x31 * x38 + x34 * x38 * x4;
514}
515
516static void ekf_L(const float X[EKF_NUM_STATES], __attribute__((unused)) const float U[EKF_NUM_INPUTS],
517 float out[EKF_NUM_STATES][EKF_NUM_INPUTS])
518{
519 const float x0 = cosf(X[EKF_X_THETA]);
520 const float x1 = cosf(X[EKF_X_PSI]);
521 const float x2 = sinf(X[EKF_X_PSI]);
522 const float x3 = cosf(X[EKF_X_PHI]);
523 const float x4 = x2 * x3;
524 const float x5 = sinf(X[EKF_X_THETA]);
525 const float x6 = sinf(X[EKF_X_PHI]);
526 const float x7 = x1 * x6;
527 const float x8 = x2 * x6;
528 const float x9 = x1 * x3;
529 const float x10 = tanf(X[EKF_X_THETA]);
530 const float x11 = 1.f / x0; // FIXME protect against div by 0
531
532 memset(out, 0, (EKF_NUM_STATES*EKF_NUM_INPUTS) * (sizeof **out));
533 out[EKF_X_VEL_X][EKF_Z_POS_X] = -x0 * x1;
534 out[EKF_X_VEL_X][EKF_Z_POS_Y] = x4 - x5 * x7;
535 out[EKF_X_VEL_X][EKF_Z_POS_Z] = -x5 * x9 - x8;
536 out[EKF_X_VEL_Y][EKF_Z_POS_X] = -x0 * x2;
537 out[EKF_X_VEL_Y][EKF_Z_POS_Y] = -x5 * x8 - x9;
538 out[EKF_X_VEL_Y][EKF_Z_POS_Z] = -x4 * x5 + x7;
539 out[EKF_X_VEL_Z][EKF_Z_POS_X] = x5;
540 out[EKF_X_VEL_Z][EKF_Z_POS_Y] = -x0 * x6;
541 out[EKF_X_VEL_Z][EKF_Z_POS_Z] = -x0 * x3;
542 out[EKF_X_PHI][EKF_Z_PHI] = -1.f;
543 out[EKF_X_PHI][EKF_Z_THETA] = -x10 * x6;
544 out[EKF_X_PHI][EKF_Z_PSI] = -x10 * x3;
545 out[EKF_X_THETA][EKF_Z_THETA] = -x3;
546 out[EKF_X_THETA][EKF_Z_PSI] = x6;
547 out[EKF_X_PSI][EKF_Z_THETA] = -x11 * x6;
548 out[EKF_X_PSI][EKF_Z_PSI] = -x11 * x3;
549}
550
551
552#if INS_EXT_POSE_USE_RK4
553
554static void ekf_f_rk4(const float X[EKF_NUM_STATES], const float U[EKF_NUM_INPUTS], const float dt,
555 float out[EKF_NUM_STATES])
556{
557 float k1[EKF_NUM_STATES];
558 float k2[EKF_NUM_STATES];
559 float k3[EKF_NUM_STATES];
560 float k4[EKF_NUM_STATES];
561
562 float Xtmp[EKF_NUM_STATES];
563
564 // k1 = f(X,U)
565 ekf_f(X, U, k1);
566
567 // Xtmp = X+dt*k1/2
568 float_vect_smul(Xtmp, k1, dt / 2.f, EKF_NUM_STATES);
570
571 // k2 = f(Xtmp,U)
572 ekf_f(Xtmp, U, k2);
573
574 // Xtmp = X+dt*k2/2
575 float_vect_smul(Xtmp, k2, dt / 2.f, EKF_NUM_STATES);
577
578 // k3 = f(Xtmp,U)
579 ekf_f(Xtmp, U, k3);
580
581 // Xtmp = X+dt*k3
584
585 // k4 = f(Xtmp,U)
586 ekf_f(Xtmp, U, k4);
587
588 // out = k2+k3
589 float_vect_sum(out, k2, k3, EKF_NUM_STATES);
590 // out *= 2
592 // out += k1
594 // out += k4
596 // out *= dt/6
597 float_vect_scale(out, dt / 6.f, EKF_NUM_STATES);
598 // out += X
600}
601
602#endif
603
604static void ekf_prediction_step(const float U[EKF_NUM_INPUTS], const float dt)
605{
608
609#if INS_EXT_POSE_USE_RK4
610 // [1] Predicted (a priori) state estimate:
611 float Xkk_1[EKF_NUM_STATES];
612 ekf_f_rk4(ekf_X, U, dt, Xkk_1);
613
614 // [2] Get matrices
617 ekf_F(ekf_X, U, F);
618 ekf_L(ekf_X, U, L);
619
620
621 // [3] Continuous to discrete
622 // Fd = eye(N) + F*dt + F*F*dt**2/2 = I + [I+F*dt/2]*F*dt
623 // Ld = L*dt+F*L*dt**2/2 = [I+F*dt/2]*L*dt
626
631
632 // tmp = I+F*dt/2
635
636 // Ld = tmp*L*dt
639
640 // Fd = tmp*F*dt
643
644 // Fd += I
645 int i;
646 for (i = 0; i < EKF_NUM_STATES; i++) {
647 Fd[i][i] += 1.f;
648 }
649
650#else
651
652 // [1] Predicted (a priori) state estimate:
653 float Xkk_1[EKF_NUM_STATES];
654 // Xkk_1 = f(X,U)
655 ekf_f(ekf_X, U, Xkk_1);
656 // Xkk_1 *= dt
658 // Xkk_1 += X
660
661
662 // [2] Get matrices
665 ekf_F(ekf_X, U, F);
666 ekf_L(ekf_X, U, Ld);
667
668
669 // [3] Continuous to discrete
670 // Fd = eye(N) + F*dt
671 // Ld = L*dt
673
677
678 // Fd = I+F*dt
681
682 // Ld = Ld*dt
684#endif
685
686 // [4] Predicted covariance estimate:
687 // Pkk_1 = Fd*P*Fd.T + Ld*Q*Ld.T
691
697
698 // Fd = Fd.T
700
701 // tmp = P*Fd
703
704 // Fd = Fd.T
706
707 // Pkk_1 = Fd*tmp
709
710 // LdT = Ld.T
712
713 // QLdT = Q*LdT
715
716 // tmp = Ld*QLdT
718
719 // Pkk_1 += tmp
721
722 // Store state and covariance
723
724 // X = Xkk_1
726
727 // P = Pkk_1
729}
730
731static void ekf_measurement_step(const float Z[EKF_NUM_OUTPUTS])
732{
733 // Xkk_1 = X
734 float Xkk_1[EKF_NUM_STATES];
736
737 // Pkk_1 = P
742
743 // [5] Measurement residual:
744 // yk = Z - H*Xkk_1
745 float yk[EKF_NUM_OUTPUTS];
746
748
752
753
754 // [6] Residual covariance:
755 // Sk = H*Pkk_1*H.T + R
758
762
763 // PHT = Pkk_1*H.T
766
767 // Sk = H*PHT
769
770 // Sk += R
772
773
774 // [7] Near-optimal Kalman gain:
775 // K = Pkk_1*H.T*inv(Sk)
778
781
782 // Sk_inv = inv(Sk)
784
785 // K = PHT*Sk_inv
787
788
789 // [8] Updated state estimate
790 // Xkk = Xkk_1 + K*yk
793
794
795 // [9] Updated covariance estimate:
796 // Pkk = (I - K*H)*Pkk_1
799
800 // tmp = K*H
802
803 // tmp *= -1
805
806 // tmp += I
807 int i;
808 for (i = 0; i < EKF_NUM_STATES; i++) {
809 tmp_[i][i] += 1.f;
810 }
811 // P = tmp*Pkk_1
813}
814
815
816static void ekf_run(void)
817{
818 // Time
820 float dt = t1 - t0;
821 t0 = t1;
822
823 // Only Start If External Pose is Available
824 if (!ins_ext_pose.started) {
825 // ekf starts at the first ev update
827 ins_ext_pose.started = true;
828
829 // initial guess
836 }
837 }
838
839 // set input values
845 } else {
846 DEBUG_PRINT("ekf missing acc\n");
847 }
853 } else {
854 DEBUG_PRINT("ekf missing gyro\n");
855 }
856
857 if (ins_ext_pose.started) {
858
859 // prediction step
861
862 DEBUG_PRINT("ekf prediction step U = %f, %f, %f, %f, %f, %f dt = %f \n",
863 ekf_U[0], ekf_U[1], ekf_U[2], ekf_U[3], ekf_U[4], ekf_U[5], dt);
864
865 // measurement step
867
868 //fix psi
869 static float last_psi = 0;
872
873 if (delta_psi > M_PI) {
874 delta_psi -= 2.f * M_PI;
875 } else if (delta_psi < -M_PI) {
876 delta_psi += 2.f * M_PI;
877 }
878
886
888
889 DEBUG_PRINT("ekf measurement step Z = %f, %f, %f, %f \n",
890 ekf_Z[0], ekf_Z[1], ekf_Z[2], ekf_Z[3]);
891 }
892 }
893
894 // Export Results
895 struct NedCoor_f ned_pos;
900
901 struct NedCoor_f ned_speed;
906
911
912 struct FloatRates rates;
916
917 struct FloatVect3 body_accel;
918 body_accel.x = ekf_U[EKF_U_ACC_X] - ekf_X[EKF_X_A_BIAS_X];
919 body_accel.y = ekf_U[EKF_U_ACC_Y] - ekf_X[EKF_X_A_BIAS_Y];
920 body_accel.z = ekf_U[EKF_U_ACC_Z] - ekf_X[EKF_X_A_BIAS_Z];
921 struct Int32Vect3 body_accel_i;
922 ACCELS_BFP_OF_REAL(body_accel_i, body_accel);
923
924 struct FloatVect3 ned_accel_v;
928 ned_accel_v.z += 9.81f;
930 struct NedCoor_f ned_accel;
932
939
940}
941
942
943
949{
950 fprintf(file,
951 "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,");
952 fprintf(file, "ekf_U1,ekf_U2,ekf_U3,ekf_U4,ekf_U5,ekf_U6,");
953 fprintf(file, "ekf_Z1,ekf_Z2,ekf_Z3,ekf_Z4,");
954}
955
957{
958 fprintf(file, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,",
959 ekf_X[0], ekf_X[1], ekf_X[2], ekf_X[3], ekf_X[4],
960 ekf_X[5], ekf_X[6], ekf_X[7], ekf_X[8], ekf_X[9],
961 ekf_X[10], ekf_X[11], ekf_X[12], ekf_X[13], ekf_X[14]);
962 fprintf(file, "%f,%f,%f,%f,%f,%f,",
963 ekf_U[0], ekf_U[1], ekf_U[2], ekf_U[3], ekf_U[4], ekf_U[5]);
964 fprintf(file, "%f,%f,%f,%f,", ekf_Z[0], ekf_Z[1], ekf_Z[2], ekf_Z[3]);
965}
Main include for ABI (AirBorneInterface).
Event structure to store callbacks in a linked list.
Definition abi_common.h:67
#define AHRS_COMP_ID_GENERIC
Definition ahrs.h:36
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
#define float_rmat_of_eulers
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 POSITIONS_BFP_OF_REAL(_ef, _ei)
#define EULERS_COPY(_a, _b)
#define QUAT_COPY(_qo, _qi)
#define SPEEDS_BFP_OF_REAL(_ef, _ei)
#define VECT3_COPY(_a, _b)
#define ACCELS_BFP_OF_REAL(_ef, _ei)
#define RATES_FLOAT_OF_BFP(_rf, _ri)
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
angular rates
#define ENU_OF_TO_NED(_po, _pi)
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 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 FloatRates gyros_f
struct FloatEulers ev_att
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])
struct InsExtPose ins_ext_pose
static float ekf_P[EKF_NUM_STATES][EKF_NUM_STATES]
static float ekf_R[EKF_NUM_OUTPUTS][EKF_NUM_OUTPUTS]
#define INS_EXT_POSE_R_NOISE
struct LtpDef_i ltp_def
#define INS_EXT_POSE_P0
static abi_event accel_ev
static void gyro_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro)
void ins_ext_pose_log_data(FILE *file)
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
static float ekf_U[EKF_NUM_INPUTS]
static 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)
static float ekf_Q[EKF_NUM_INPUTS][EKF_NUM_INPUTS]
#define INS_EXT_POSE_Q_NOISE
bool has_new_acc
void ins_ext_pose_log_header(FILE *file)
Logging.
static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
#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)
static float t1
static void ekf_set_diag(float **a, float *b, int n)
static 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]
static 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.
@ EKF_X_VEL_Z
@ EKF_X_VEL_X
@ EKF_X_VEL_Y
@ EKF_X_A_BIAS_Z
@ EKF_X_PSI
@ EKF_X_A_BIAS_X
@ EKF_X_G_BIAS_R
@ EKF_X_G_BIAS_Q
@ EKF_X_G_BIAS_P
@ EKF_NUM_STATES
@ EKF_X_THETA
@ EKF_X_POS_Y
@ EKF_X_POS_X
@ EKF_X_A_BIAS_Y
@ EKF_X_POS_Z
@ EKF_X_PHI
@ EKF_U_ACC_Z
@ EKF_NUM_INPUTS
@ EKF_U_GYRO_R
@ EKF_U_ACC_Y
@ EKF_U_GYRO_P
@ EKF_U_ACC_X
@ EKF_U_GYRO_Q
@ EKF_Z_POS_Z
@ EKF_Z_PHI
@ EKF_Z_POS_X
@ EKF_Z_PSI
@ EKF_NUM_OUTPUTS
@ EKF_Z_THETA
@ EKF_Z_POS_Y
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 generic algebra macros.
Paparazzi floating point algebra.
float x
in meters
float x
in meters
vector in East North Up coordinates Units: 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
int16_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint16_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 short uint16_t
Typedef defining 16 bit unsigned short type.
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