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"
32#include "math/pprz_algebra_float.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
46struct FloatQuat ins_ext_vision_rot;
47#endif
48
51struct InsExtPose {
52 /* Inputs */
53 struct FloatRates gyros_f;
54 struct FloatVect3 accels_f;
55 bool has_new_gyro;
56 bool has_new_acc;
57
58 struct FloatVect3 ev_pos;
59 struct FloatVect3 ev_vel;
60 struct FloatEulers ev_att;
61 struct FloatQuat ev_quat;
62 bool has_new_ext_pose;
63 float ev_time;
64
65 /* Origin */
66 struct LtpDef_i ltp_def;
67
68 /* output LTP NED */
69 struct NedCoor_i ltp_pos;
70 struct NedCoor_i ltp_speed;
71 struct NedCoor_i ltp_accel;
72};
73struct InsExtPose ins_ext_pos;
74
75
76static void ins_ext_pose_init_from_flightplan(void)
77{
78
79 struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
80 llh_nav0.lat = NAV_LAT0;
81 llh_nav0.lon = NAV_LON0;
82 /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
83 llh_nav0.alt = NAV_ALT0 + NAV_MSL0;
84
85 struct EcefCoor_i ecef_nav0;
86 ecef_of_lla_i(&ecef_nav0, &llh_nav0);
87
88 ltp_def_from_ecef_i(&ins_ext_pos.ltp_def, &ecef_nav0);
89 ins_ext_pos.ltp_def.hmsl = NAV_ALT0;
90 stateSetLocalOrigin_i(MODULE_INS_EXT_POSE_ID, &ins_ext_pos.ltp_def);
91 /* update local ENU coordinates of global waypoints */
92 waypoints_localize_all();
93}
94
95
99#if PERIODIC_TELEMETRY
100#include "modules/datalink/telemetry.h"
101
109
110static void send_ins_z(struct transport_tx *trans, struct link_device *dev)
111{
112 static float fake_baro_z = 0.0;
113 pprz_msg_send_INS_Z(trans, dev, AC_ID,
114 (float *)&fake_baro_z, &ins_ext_pos.ltp_pos.z,
115 &ins_ext_pos.ltp_speed.z, &ins_ext_pos.ltp_accel.z);
116}
117
126
142static void send_ahrs_bias(struct transport_tx *trans, struct link_device *dev)
143{
144 float dummy0 = 0.0;
145 pprz_msg_send_AHRS_BIAS(trans, dev, AC_ID,
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
166PRINT_CONFIG_VAR(INS_EXT_POSE_IMU_ID)
167
168static abi_event accel_ev;
169static abi_event gyro_ev;
170
171static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel);
172static void gyro_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro);
173
174
175
176static void gyro_cb(uint8_t sender_id __attribute__((unused)),
177 uint32_t stamp __attribute__((unused)),
178 struct Int32Rates *gyro)
179{
180 RATES_FLOAT_OF_BFP(ins_ext_pos.gyros_f, *gyro);
181 ins_ext_pos.has_new_gyro = true;
182}
183
184static void accel_cb(uint8_t sender_id __attribute__((unused)),
185 uint32_t stamp __attribute__((unused)),
186 struct Int32Vect3 *accel)
187{
188 ACCELS_FLOAT_OF_BFP(ins_ext_pos.accels_f, *accel);
189 ins_ext_pos.has_new_acc = true;
190}
191
192
197void ins_ext_pose_msg_update(uint8_t *buf)
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;
215 struct FloatEulers orient_eulers;
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;
226 float_quat_comp(&rot_q, &orient, &ins_ext_vision_rot);
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
233 float_eulers_of_quat(&orient_eulers, &orient);
234
235 ins_ext_pos.ev_time = get_sys_time_usec();
236 ins_ext_pos.ev_pos.x = enu_y;
237 ins_ext_pos.ev_pos.y = enu_x;
238 ins_ext_pos.ev_pos.z = -enu_z;
239 ins_ext_pos.ev_vel.x = enu_yd;
240 ins_ext_pos.ev_vel.y = enu_xd;
241 ins_ext_pos.ev_vel.z = -enu_zd;
242 ins_ext_pos.ev_att.phi = orient_eulers.phi;
243 ins_ext_pos.ev_att.theta = orient_eulers.theta;
244 ins_ext_pos.ev_att.psi = orient_eulers.psi;
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
250 ins_ext_pos.has_new_ext_pose = true;
251
252 DEBUG_PRINT("Att = %f %f %f \n", ins_ext_pos.ev_att.phi, ins_ext_pos.ev_att.theta, ins_ext_pos.ev_att.psi);
253}
254
258static inline void ekf_init(void);
259static inline void ekf_run(void);
260
265void ins_ext_pose_init(void)
266{
267
268 // Initialize inputs
269 ins_ext_pos.has_new_acc = false;
270 ins_ext_pos.has_new_gyro = false;
271 ins_ext_pos.has_new_ext_pose = false;
272
273 // Get External Pose Origin From Flightplan
274 ins_ext_pose_init_from_flightplan();
275
276 // Provide telemetry
277#if PERIODIC_TELEMETRY
278 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_INS, send_ins);
279 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_INS_Z, send_ins_z);
280 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_INS_REF, send_ins_ref);
281 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_EXTERNAL_POSE_DOWN, send_external_pose_down);
282 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AHRS_BIAS, send_ahrs_bias);
283#endif
284
285 // Get IMU through ABI
286 AbiBindMsgIMU_ACCEL(INS_EXT_POSE_IMU_ID, &accel_ev, accel_cb);
287 AbiBindMsgIMU_GYRO(INS_EXT_POSE_IMU_ID, &gyro_ev, gyro_cb);
288 // Get External Pose through datalink message: setup in xml
289
290 // Initialize EKF
291 ekf_init();
292}
293
294void ins_ext_pose_run(void)
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
323float ekf_X[EKF_NUM_STATES];
324float ekf_U[EKF_NUM_INPUTS];
325float ekf_Z[EKF_NUM_OUTPUTS];
326float ekf_P[EKF_NUM_STATES][EKF_NUM_STATES];
327float ekf_Q[EKF_NUM_INPUTS][EKF_NUM_INPUTS];
328float ekf_R[EKF_NUM_OUTPUTS][EKF_NUM_OUTPUTS];
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
366 MAKE_MATRIX_PTR(ekf_P_, ekf_P, EKF_NUM_STATES);
367 MAKE_MATRIX_PTR(ekf_Q_, ekf_Q, EKF_NUM_INPUTS);
368 MAKE_MATRIX_PTR(ekf_R_, ekf_R, EKF_NUM_OUTPUTS);
369
370 ekf_set_diag(ekf_P_, Pdiag, EKF_NUM_STATES);
371 ekf_set_diag(ekf_Q_, Qdiag, EKF_NUM_INPUTS);
372 ekf_set_diag(ekf_R_, Rdiag, EKF_NUM_OUTPUTS);
373 float_vect_copy(ekf_X, X0, EKF_NUM_STATES);
374 float_vect_copy(ekf_U, U0, EKF_NUM_INPUTS);
375 float_vect_copy(ekf_Z, Z0, EKF_NUM_OUTPUTS);
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);
810 float_vect_add(Xtmp, X, 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);
817 float_vect_add(Xtmp, X, EKF_NUM_STATES);
818
819 // k3 = f(Xtmp,U)
820 ekf_f(Xtmp, U, k3);
821
822 // Xtmp = X+dt*k3
823 float_vect_smul(Xtmp, k3, dt, EKF_NUM_STATES);
824 float_vect_add(Xtmp, X, EKF_NUM_STATES);
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
832 float_vect_scale(out, 2, EKF_NUM_STATES);
833 // out += k1
834 float_vect_add(out, k1, EKF_NUM_STATES);
835 // out += k4
836 float_vect_add(out, k4, EKF_NUM_STATES);
837 // out *= dt/6
838 float_vect_scale(out, dt / 6, EKF_NUM_STATES);
839 // out += X
840 float_vect_add(out, X, EKF_NUM_STATES);
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
852 float F[EKF_NUM_STATES][EKF_NUM_STATES];
853 float L[EKF_NUM_STATES][EKF_NUM_INPUTS];
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
861 float Fd[EKF_NUM_STATES][EKF_NUM_STATES];
862 float Ld[EKF_NUM_STATES][EKF_NUM_INPUTS];
863 float tmp[EKF_NUM_STATES][EKF_NUM_STATES];
864
865 MAKE_MATRIX_PTR(F_, F, EKF_NUM_STATES);
866 MAKE_MATRIX_PTR(L_, L, EKF_NUM_STATES);
867 MAKE_MATRIX_PTR(Fd_, Fd, EKF_NUM_STATES);
868 MAKE_MATRIX_PTR(Ld_, Ld, EKF_NUM_STATES);
869 MAKE_MATRIX_PTR(tmp_, tmp, EKF_NUM_STATES);
870
871 // tmp = I+F*dt/2
872 float_mat_diagonal_scal(tmp_, 1, EKF_NUM_STATES);
873 float_mat_sum_scaled(tmp_, F_, dt / 2, EKF_NUM_STATES, EKF_NUM_STATES);
874
875 // Ld = tmp*L*dt
876 float_mat_mul(Ld_, tmp_, L_, EKF_NUM_STATES, EKF_NUM_STATES, EKF_NUM_INPUTS);
877 float_mat_scale(Ld_, dt, EKF_NUM_STATES, EKF_NUM_INPUTS);
878
879 // Fd = tmp*F*dt
880 float_mat_mul(Fd_, tmp_, F_, EKF_NUM_STATES, EKF_NUM_STATES, EKF_NUM_STATES);
881 float_mat_scale(Fd_, dt, EKF_NUM_STATES, EKF_NUM_STATES);
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
892 float Pkk_1[EKF_NUM_STATES][EKF_NUM_STATES];
893 float LdT[EKF_NUM_INPUTS][EKF_NUM_STATES];
894 float QLdT[EKF_NUM_INPUTS][EKF_NUM_STATES];
895
896 MAKE_MATRIX_PTR(Pkk_1_, Pkk_1, EKF_NUM_STATES);
897 MAKE_MATRIX_PTR(ekf_P_, ekf_P, EKF_NUM_STATES);
898 MAKE_MATRIX_PTR(ekf_Q_, ekf_Q, EKF_NUM_STATES);
899 MAKE_MATRIX_PTR(LdT_, LdT, EKF_NUM_INPUTS);
900 MAKE_MATRIX_PTR(QLdT_, QLdT, EKF_NUM_INPUTS);
901
902 // Fd = Fd.T
903 float_mat_transpose_square(Fd_, EKF_NUM_STATES);
904
905 // tmp = P*Fd
906 float_mat_mul(tmp_, ekf_P_, Fd_, EKF_NUM_STATES, EKF_NUM_STATES, EKF_NUM_STATES);
907
908 // Fd = Fd.T
909 float_mat_transpose_square(Fd_, EKF_NUM_STATES);
910
911 // Pkk_1 = Fd*tmp
912 float_mat_mul(Pkk_1_, Fd_, tmp_, EKF_NUM_STATES, EKF_NUM_STATES, EKF_NUM_STATES);
913
914 // LdT = Ld.T
915 float_mat_transpose(LdT_, Ld_, EKF_NUM_STATES, EKF_NUM_INPUTS);
916
917 // QLdT = Q*LdT
918 float_mat_mul(QLdT_, ekf_Q_, LdT_, EKF_NUM_INPUTS, EKF_NUM_INPUTS, EKF_NUM_STATES);
919
920 // tmp = Ld*QLdT
921 float_mat_mul(tmp_, Ld_, QLdT_, EKF_NUM_STATES, EKF_NUM_INPUTS, EKF_NUM_STATES);
922
923 // Pkk_1 += tmp
924 float_mat_sum_scaled(Pkk_1_, tmp_, 1, EKF_NUM_STATES, EKF_NUM_STATES);
925
926
927 // [5] Measurement residual:
928 // yk = Z - H*Xkk_1
929 float yk[EKF_NUM_OUTPUTS];
930
931 MAKE_MATRIX_PTR(ekf_H_, ekf_H, EKF_NUM_OUTPUTS);
932
933 float_mat_vect_mul(yk, ekf_H_, Xkk_1, EKF_NUM_OUTPUTS, EKF_NUM_STATES);
934 float_vect_scale(yk, -1, EKF_NUM_OUTPUTS);
935 float_vect_add(yk, Z, EKF_NUM_OUTPUTS);
936
937
938 // [6] Residual covariance:
939 // Sk = H*Pkk_1*H.T + R
940 float Sk[EKF_NUM_OUTPUTS][EKF_NUM_OUTPUTS];
941 float PHT[EKF_NUM_STATES][EKF_NUM_OUTPUTS];
942
943 MAKE_MATRIX_PTR(Sk_, Sk, EKF_NUM_OUTPUTS);
944 MAKE_MATRIX_PTR(PHT_, PHT, EKF_NUM_STATES);
945 MAKE_MATRIX_PTR(ekf_R_, ekf_R, EKF_NUM_OUTPUTS);
946
947 // PHT = Pkk_1*H.T
948 float_mat_transpose(PHT_, ekf_H_, EKF_NUM_OUTPUTS, EKF_NUM_STATES);
949 float_mat_mul_copy(PHT_, Pkk_1_, PHT_, EKF_NUM_STATES, EKF_NUM_STATES, EKF_NUM_OUTPUTS);
950
951 // Sk = H*PHT
952 float_mat_mul(Sk_, ekf_H_, PHT_, EKF_NUM_OUTPUTS, EKF_NUM_STATES, EKF_NUM_OUTPUTS);
953
954 // Sk += R
955 float_mat_sum_scaled(Sk_, ekf_R_, 1, EKF_NUM_OUTPUTS, EKF_NUM_OUTPUTS);
956
957
958 // [7] Near-optimal Kalman gain:
959 // K = Pkk_1*H.T*inv(Sk)
960 float Sk_inv[EKF_NUM_OUTPUTS][EKF_NUM_OUTPUTS];
961 float K[EKF_NUM_STATES][EKF_NUM_OUTPUTS];
962
963 MAKE_MATRIX_PTR(Sk_inv_, Sk_inv, EKF_NUM_OUTPUTS);
964 MAKE_MATRIX_PTR(K_, K, EKF_NUM_STATES);
965
966 // Sk_inv = inv(Sk)
967 float_mat_invert(Sk_inv_, Sk_, EKF_NUM_OUTPUTS);
968
969 // K = PHT*Sk_inv
970 float_mat_mul(K_, PHT_, Sk_inv_, EKF_NUM_STATES, EKF_NUM_OUTPUTS, EKF_NUM_OUTPUTS);
971
972
973 // [8] Updated state estimate
974 // Xkk = Xkk_1 + K*yk
975 float_mat_vect_mul(ekf_X, K_, yk, EKF_NUM_STATES, EKF_NUM_OUTPUTS);
976 float_vect_add(ekf_X, Xkk_1, EKF_NUM_STATES);
977
978
979 // [9] Updated covariance estimate:
980 // Pkk = (I - K*H)*Pkk_1
981
982 // tmp = K*H
983 float_mat_mul(tmp_, K_, ekf_H_, EKF_NUM_STATES, EKF_NUM_OUTPUTS, EKF_NUM_STATES);
984
985 // tmp *= -1
986 float_mat_scale(tmp_, -1, EKF_NUM_STATES, EKF_NUM_STATES);
987
988 // tmp += I
989 for (i = 0; i < EKF_NUM_STATES; i++) {
990 tmp_[i][i] += 1;
991 }
992 // P = tmp*Pkk_1
993 float_mat_mul(ekf_P_, tmp_, Pkk_1_, EKF_NUM_STATES, EKF_NUM_STATES, EKF_NUM_STATES);
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
1003 float_vect_scale(Xkk_1, dt, EKF_NUM_STATES);
1004 // Xkk_1 += X
1005 float_vect_add(Xkk_1, ekf_X, EKF_NUM_STATES);
1006
1007
1008 // [2] Get matrices
1009 float F[EKF_NUM_STATES][EKF_NUM_STATES];
1010 float Ld[EKF_NUM_STATES][EKF_NUM_INPUTS];
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
1018 float Fd[EKF_NUM_STATES][EKF_NUM_STATES];
1019
1020 MAKE_MATRIX_PTR(F_, F, EKF_NUM_STATES);
1021 MAKE_MATRIX_PTR(Fd_, Fd, EKF_NUM_STATES);
1022 MAKE_MATRIX_PTR(Ld_, Ld, EKF_NUM_STATES);
1023
1024 // Fd = I+F*dt/2
1025 float_mat_diagonal_scal(Fd_, 1, EKF_NUM_STATES);
1026 float_mat_sum_scaled(Fd_, F_, dt, EKF_NUM_STATES, EKF_NUM_STATES);
1027
1028 // Ld = Ld*dt
1029 float_mat_scale(Ld_, dt, EKF_NUM_STATES, EKF_NUM_INPUTS);
1030
1031
1032 // [4] Predicted covariance estimate:
1033 // Pkk_1 = Fd*P*Fd.T + Ld*Q*Ld.T
1034 float Pkk_1[EKF_NUM_STATES][EKF_NUM_STATES];
1035 float LdT[EKF_NUM_INPUTS][EKF_NUM_STATES];
1036 float QLdT[EKF_NUM_INPUTS][EKF_NUM_STATES];
1037 float tmp[EKF_NUM_STATES][EKF_NUM_STATES];
1038
1039 MAKE_MATRIX_PTR(Pkk_1_, Pkk_1, EKF_NUM_STATES);
1040 MAKE_MATRIX_PTR(ekf_P_, ekf_P, EKF_NUM_STATES);
1041 MAKE_MATRIX_PTR(ekf_Q_, ekf_Q, EKF_NUM_STATES);
1042 MAKE_MATRIX_PTR(LdT_, LdT, EKF_NUM_INPUTS);
1043 MAKE_MATRIX_PTR(QLdT_, QLdT, EKF_NUM_INPUTS);
1044 MAKE_MATRIX_PTR(tmp_, tmp, EKF_NUM_STATES);
1045
1046 // Fd = Fd.T
1047 float_mat_transpose_square(Fd_, EKF_NUM_STATES);
1048
1049 // tmp = P*Fd
1050 float_mat_mul(tmp_, ekf_P_, Fd_, EKF_NUM_STATES, EKF_NUM_STATES, EKF_NUM_STATES);
1051
1052 // Fd = Fd.T
1053 float_mat_transpose_square(Fd_, EKF_NUM_STATES);
1054
1055 // Pkk_1 = Fd*tmp
1056 float_mat_mul(Pkk_1_, Fd_, tmp_, EKF_NUM_STATES, EKF_NUM_STATES, EKF_NUM_STATES);
1057
1058 // LdT = Ld.T
1059 float_mat_transpose(LdT_, Ld_, EKF_NUM_STATES, EKF_NUM_INPUTS);
1060
1061 // QLdT = Q*LdT
1062 float_mat_mul(QLdT_, ekf_Q_, LdT_, EKF_NUM_INPUTS, EKF_NUM_INPUTS, EKF_NUM_STATES);
1063
1064 // tmp = Ld*QLdT
1065 float_mat_mul(tmp_, Ld_, QLdT_, EKF_NUM_STATES, EKF_NUM_INPUTS, EKF_NUM_STATES);
1066
1067 // Pkk_1 += tmp
1068 float_mat_sum_scaled(Pkk_1_, tmp_, 1, EKF_NUM_STATES, EKF_NUM_STATES);
1069
1070 // X = Xkk_1
1071 float_vect_copy(ekf_X, Xkk_1, EKF_NUM_STATES);
1072
1073 // P = Pkk_1
1074 float_mat_copy(ekf_P_, Pkk_1_, EKF_NUM_STATES, EKF_NUM_STATES);
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];
1081 float_vect_copy(Xkk_1, ekf_X, EKF_NUM_STATES);
1082
1083 // Pkk_1 = P
1084 float Pkk_1[EKF_NUM_STATES][EKF_NUM_STATES];
1085 MAKE_MATRIX_PTR(Pkk_1_, Pkk_1, EKF_NUM_STATES);
1086 MAKE_MATRIX_PTR(ekf_P_, ekf_P, EKF_NUM_STATES);
1087 float_mat_copy(Pkk_1_, ekf_P_, EKF_NUM_STATES, EKF_NUM_STATES);
1088
1089 // [5] Measurement residual:
1090 // yk = Z - H*Xkk_1
1091 float yk[EKF_NUM_OUTPUTS];
1092
1093 MAKE_MATRIX_PTR(ekf_H_, ekf_H, EKF_NUM_OUTPUTS);
1094
1095 float_mat_vect_mul(yk, ekf_H_, Xkk_1, EKF_NUM_OUTPUTS, EKF_NUM_STATES);
1096 float_vect_scale(yk, -1, EKF_NUM_OUTPUTS);
1097 float_vect_add(yk, Z, EKF_NUM_OUTPUTS);
1098
1099
1100 // [6] Residual covariance:
1101 // Sk = H*Pkk_1*H.T + R
1102 float Sk[EKF_NUM_OUTPUTS][EKF_NUM_OUTPUTS];
1103 float PHT[EKF_NUM_STATES][EKF_NUM_OUTPUTS];
1104
1105 MAKE_MATRIX_PTR(Sk_, Sk, EKF_NUM_OUTPUTS);
1106 MAKE_MATRIX_PTR(PHT_, PHT, EKF_NUM_STATES);
1107 MAKE_MATRIX_PTR(ekf_R_, ekf_R, EKF_NUM_OUTPUTS);
1108
1109 // PHT = Pkk_1*H.T
1110 float_mat_transpose(PHT_, ekf_H_, EKF_NUM_OUTPUTS, EKF_NUM_STATES);
1111 float_mat_mul_copy(PHT_, Pkk_1_, PHT_, EKF_NUM_STATES, EKF_NUM_STATES, EKF_NUM_OUTPUTS);
1112
1113 // Sk = H*PHT
1114 float_mat_mul(Sk_, ekf_H_, PHT_, EKF_NUM_OUTPUTS, EKF_NUM_STATES, EKF_NUM_OUTPUTS);
1115
1116 // Sk += R
1117 float_mat_sum_scaled(Sk_, ekf_R_, 1, EKF_NUM_OUTPUTS, EKF_NUM_OUTPUTS);
1118
1119
1120 // [7] Near-optimal Kalman gain:
1121 // K = Pkk_1*H.T*inv(Sk)
1122 float Sk_inv[EKF_NUM_OUTPUTS][EKF_NUM_OUTPUTS];
1123 float K[EKF_NUM_STATES][EKF_NUM_OUTPUTS];
1124
1125 MAKE_MATRIX_PTR(Sk_inv_, Sk_inv, EKF_NUM_OUTPUTS);
1126 MAKE_MATRIX_PTR(K_, K, EKF_NUM_STATES);
1127
1128 // Sk_inv = inv(Sk)
1129 float_mat_invert(Sk_inv_, Sk_, EKF_NUM_OUTPUTS);
1130
1131 // K = PHT*Sk_inv
1132 float_mat_mul(K_, PHT_, Sk_inv_, EKF_NUM_STATES, EKF_NUM_OUTPUTS, EKF_NUM_OUTPUTS);
1133
1134
1135 // [8] Updated state estimate
1136 // Xkk = Xkk_1 + K*yk
1137 float_mat_vect_mul(ekf_X, K_, yk, EKF_NUM_STATES, EKF_NUM_OUTPUTS);
1138 float_vect_add(ekf_X, Xkk_1, EKF_NUM_STATES);
1139
1140
1141 // [9] Updated covariance estimate:
1142 // Pkk = (I - K*H)*Pkk_1
1143 float tmp[EKF_NUM_STATES][EKF_NUM_STATES];
1144 MAKE_MATRIX_PTR(tmp_, tmp, EKF_NUM_STATES);
1145
1146 // tmp = K*H
1147 float_mat_mul(tmp_, K_, ekf_H_, EKF_NUM_STATES, EKF_NUM_OUTPUTS, EKF_NUM_STATES);
1148
1149 // tmp *= -1
1150 float_mat_scale(tmp_, -1, EKF_NUM_STATES, EKF_NUM_STATES);
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
1158 float_mat_mul(ekf_P_, tmp_, Pkk_1_, EKF_NUM_STATES, EKF_NUM_STATES, EKF_NUM_STATES);
1159}
1160
1161
1162
1163
1164
1165static inline void ekf_run(void)
1166{
1167 static bool start = false;
1168
1169
1170 // Time
1171 t1 = get_sys_time_float();
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
1178 if (ins_ext_pos.has_new_ext_pose) {
1179 start = true;
1180
1181 // initial guess
1182 ekf_X[0] = ins_ext_pos.ev_pos.x;
1183 ekf_X[1] = ins_ext_pos.ev_pos.y;
1184 ekf_X[2] = ins_ext_pos.ev_pos.z;
1185 ekf_X[6] = ins_ext_pos.ev_att.phi;
1186 ekf_X[7] = ins_ext_pos.ev_att.theta;
1187 ekf_X[8] = ins_ext_pos.ev_att.psi;
1188 }
1189 }
1190
1191 // set input values
1192 if (ins_ext_pos.has_new_acc) {
1193 ekf_U[0] = ins_ext_pos.accels_f.x;
1194 ekf_U[1] = ins_ext_pos.accels_f.y;
1195 ekf_U[2] = ins_ext_pos.accels_f.z;
1196 ins_ext_pos.has_new_acc = false;
1197 } else {
1198 DEBUG_PRINT("ekf missing acc\n");
1199 }
1200 if (ins_ext_pos.has_new_gyro) {
1201 ekf_U[3] = ins_ext_pos.gyros_f.p;
1202 ekf_U[4] = ins_ext_pos.gyros_f.q;
1203 ekf_U[5] = ins_ext_pos.gyros_f.r;
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);
1214 ekf_prediction_step(ekf_U, dt);
1215
1216 // measurement step
1217 if (ins_ext_pos.has_new_ext_pose) {
1218
1219 //fix psi
1220 static float last_psi = 0;
1221 float delta_psi = ins_ext_pos.ev_att.psi - last_psi;
1222 last_psi = ins_ext_pos.ev_att.psi;
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
1231 ekf_Z[0] = ins_ext_pos.ev_pos.x;
1232 ekf_Z[1] = ins_ext_pos.ev_pos.y;
1233 ekf_Z[2] = ins_ext_pos.ev_pos.z;
1234 ekf_Z[3] = ins_ext_pos.ev_att.phi;
1235 ekf_Z[4] = ins_ext_pos.ev_att.theta;
1236 ekf_Z[5] += delta_psi;
1237 ins_ext_pos.has_new_ext_pose = false;
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]);
1240 ekf_measurement_step(ekf_Z);
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
1255 struct FloatEulers ned_to_body_eulers;
1256 ned_to_body_eulers.phi = ekf_X[6];
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);
1271 stateSetAccelBody_i(MODULE_INS_EXT_POSE_ID, &accel_i);
1272
1273
1274 struct FloatRMat *ned_to_body_rmat_f = stateGetNedToBodyRMat_f();
1275 float_rmat_transp_vmult(&accel_ned_f, ned_to_body_rmat_f, &accel);
1276 accel_ned_f.z += 9.81;
1277
1278 stateSetPositionNed_f(MODULE_INS_EXT_POSE_ID, &ned_pos);
1279 stateSetSpeedNed_f(MODULE_INS_EXT_POSE_ID, &ned_speed);
1280 stateSetNedToBodyEulers_f(MODULE_INS_EXT_POSE_ID, &ned_to_body_eulers);
1281 stateSetBodyRates_f(MODULE_INS_EXT_POSE_ID, &rates);
1282 stateSetAccelNed_f(MODULE_INS_EXT_POSE_ID, (struct NedCoor_f *)&accel_ned_f);
1283
1284}
1285
1286
1287
1292void ins_ext_pos_log_header(FILE *file)
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
1300void ins_ext_pos_log_data(FILE *file)
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}
abi.h
Main include for ABI (AirBorneInterface).
abi_struct
Event structure to store callbacks in a linked list.
Definition abi_common.h:67
get_sys_time_usec
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition sys_time_arch.c:71
x12
@ x12
Definition discrete_ekf_no_north.c:34
FloatQuat::qx
float qx
Definition pprz_algebra_float.h:65
FloatRates::q
float q
in rad/s
Definition pprz_algebra_float.h:95
FloatEulers::phi
float phi
in radians
Definition pprz_algebra_float.h:85
FloatRates::p
float p
in rad/s
Definition pprz_algebra_float.h:94
FloatQuat::qi
float qi
Definition pprz_algebra_float.h:64
FloatQuat::qy
float qy
Definition pprz_algebra_float.h:66
FloatRates::r
float r
in rad/s
Definition pprz_algebra_float.h:96
FloatEulers::theta
float theta
in radians
Definition pprz_algebra_float.h:86
FloatVect3::x
float x
Definition pprz_algebra_float.h:55
FloatVect3::y
float y
Definition pprz_algebra_float.h:56
FloatQuat::qz
float qz
Definition pprz_algebra_float.h:67
FloatEulers::psi
float psi
in radians
Definition pprz_algebra_float.h:87
FloatVect3::z
float z
Definition pprz_algebra_float.h:57
float_vect_add
static void float_vect_add(float *a, const float *b, const int n)
a += b
Definition pprz_algebra_float.h:577
float_vect_sum
static void float_vect_sum(float *o, const float *a, const float *b, const int n)
o = a + b
Definition pprz_algebra_float.h:556
float_vect_smul
static void float_vect_smul(float *o, const float *a, const float s, const int n)
o = a * s
Definition pprz_algebra_float.h:591
float_mat_vect_mul
static void float_mat_vect_mul(float *o, float **a, float *b, int m, int n)
o = a * b
Definition pprz_algebra_float.h:785
float_vect_scale
static void float_vect_scale(float *a, const float s, const int n)
a *= s
Definition pprz_algebra_float.h:616
float_mat_sum_scaled
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
Definition pprz_algebra_float.h:817
float_mat_mul_copy
static void float_mat_mul_copy(float **o, float **a, float **b, int m, int n, int l)
o = a * b
Definition pprz_algebra_float.h:761
float_mat_copy
static void float_mat_copy(float **a, float **b, int m, int n)
a = b
Definition pprz_algebra_float.h:659
float_vect_copy
static void float_vect_copy(float *a, const float *b, const int n)
a = b
Definition pprz_algebra_float.h:549
float_mat_mul
static void float_mat_mul(float **o, float **a, float **b, int m, int n, int l)
o = a * b
Definition pprz_algebra_float.h:718
float_mat_transpose
static void float_mat_transpose(float **o, float **a, int n, int m)
transpose non-square matrix
Definition pprz_algebra_float.h:702
MAKE_MATRIX_PTR
#define MAKE_MATRIX_PTR(_ptr, _mat, _rows)
Make a pointer to a matrix of _rows lines.
Definition pprz_algebra_float.h:638
float_mat_invert
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.
Definition pprz_algebra_float.c:935
float_rmat_transp_vmult
void float_rmat_transp_vmult(struct FloatVect3 *vb, struct FloatRMat *m_b2a, struct FloatVect3 *va)
rotate 3D vector by transposed rotation matrix.
Definition pprz_algebra_float.c:120
float_eulers_of_quat
void float_eulers_of_quat(struct FloatEulers *e, struct FloatQuat *q)
euler rotation 'ZYX'
Definition pprz_algebra_float.c:688
float_mat_transpose_square
static void float_mat_transpose_square(float **a, int n)
transpose square matrix
Definition pprz_algebra_float.h:688
float_quat_comp
void float_quat_comp(struct FloatQuat *a2c, struct FloatQuat *a2b, struct FloatQuat *b2c)
Composition (multiplication) of two quaternions.
Definition pprz_algebra_float.c:320
float_mat_diagonal_scal
static void float_mat_diagonal_scal(float **o, float v, int n)
Make an n x n identity matrix (for matrix passed as array)
Definition pprz_algebra_float.h:870
float_mat_scale
static void float_mat_scale(float **a, float k, int m, int n)
a *= k, where k is a scalar value
Definition pprz_algebra_float.h:801
FloatEulers
euler angles
Definition pprz_algebra_float.h:84
FloatQuat
Roation quaternion.
Definition pprz_algebra_float.h:63
FloatRMat
rotation matrix
Definition pprz_algebra_float.h:77
FloatRates
angular rates
Definition pprz_algebra_float.h:93
FloatVect3
Definition pprz_algebra_float.h:54
ACCELS_BFP_OF_REAL
#define ACCELS_BFP_OF_REAL(_ef, _ei)
Definition pprz_algebra.h:801
RATES_FLOAT_OF_BFP
#define RATES_FLOAT_OF_BFP(_rf, _ri)
Definition pprz_algebra.h:759
ACCELS_FLOAT_OF_BFP
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
Definition pprz_algebra.h:795
Int32Rates
angular rates
Definition pprz_algebra_int.h:179
Int32Vect3
Definition pprz_algebra_int.h:88
LlaCoor_i::lat
int32_t lat
in degrees*1e7
Definition pprz_geodetic_int.h:60
LtpDef_i::hmsl
int32_t hmsl
Height above mean sea level in mm.
Definition pprz_geodetic_int.h:102
LlaCoor_i::alt
int32_t alt
in millimeters above WGS84 reference ellipsoid
Definition pprz_geodetic_int.h:62
NedCoor_i::z
int32_t z
Down.
Definition pprz_geodetic_int.h:71
EcefCoor_i::z
int32_t z
in centimeters
Definition pprz_geodetic_int.h:53
LtpDef_i::lla
struct LlaCoor_i lla
Reference point in lla.
Definition pprz_geodetic_int.h:100
EcefCoor_i::x
int32_t x
in centimeters
Definition pprz_geodetic_int.h:51
NedCoor_i::y
int32_t y
East.
Definition pprz_geodetic_int.h:70
LtpDef_i::ecef
struct EcefCoor_i ecef
Reference point in ecef.
Definition pprz_geodetic_int.h:99
EcefCoor_i::y
int32_t y
in centimeters
Definition pprz_geodetic_int.h:52
LlaCoor_i::lon
int32_t lon
in degrees*1e7
Definition pprz_geodetic_int.h:61
NedCoor_i::x
int32_t x
North.
Definition pprz_geodetic_int.h:69
ecef_of_lla_i
void ecef_of_lla_i(struct EcefCoor_i *out, struct LlaCoor_i *in)
Convert a LLA to ECEF.
Definition pprz_geodetic_int.c:392
ltp_def_from_ecef_i
void ltp_def_from_ecef_i(struct LtpDef_i *def, struct EcefCoor_i *ecef)
Definition pprz_geodetic_int.c:60
EcefCoor_i
vector in EarthCenteredEarthFixed coordinates
Definition pprz_geodetic_int.h:50
LlaCoor_i
vector in Latitude, Longitude and Altitude
Definition pprz_geodetic_int.h:59
LtpDef_i
definition of the local (flat earth) coordinate system
Definition pprz_geodetic_int.h:98
NedCoor_i
vector in North East Down coordinates
Definition pprz_geodetic_int.h:68
stateSetAccelNed_f
static void stateSetAccelNed_f(uint16_t id, struct NedCoor_f *ned_accel)
Set acceleration in NED coordinates (float).
Definition state.h:1147
stateSetAccelBody_i
static void stateSetAccelBody_i(uint16_t id, struct Int32Vect3 *body_accel)
Set acceleration in Body coordinates (int).
Definition state.h:1167
stateGetNedToBodyRMat_f
static struct FloatRMat * stateGetNedToBodyRMat_f(void)
Get vehicle body attitude rotation matrix (float).
Definition state.h:1300
stateSetNedToBodyEulers_f
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
stateSetPositionNed_f
static void stateSetPositionNed_f(uint16_t id, struct NedCoor_f *ned_pos)
Set position from local NED coordinates (float).
Definition state.h:718
stateSetLocalOrigin_i
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
stateSetBodyRates_f
static void stateSetBodyRates_f(uint16_t id, struct FloatRates *body_rate)
Set vehicle body angular rate (float).
Definition state.h:1346
stateSetSpeedNed_f
static void stateSetSpeedNed_f(uint16_t id, struct NedCoor_f *ned_speed)
Set ground speed in local NED coordinates (float).
Definition state.h:947
imu.h
Inertial Measurement Unit interface.
ins.h
Integrated Navigation System interface.
ekf_L
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:686
ins_ext_pos
struct InsExtPose ins_ext_pos
Definition ins_ext_pose.c:73
InsExtPose::gyros_f
struct FloatRates gyros_f
Definition ins_ext_pose.c:53
InsExtPose::ev_att
struct FloatEulers ev_att
Definition ins_ext_pose.c:60
ekf_f_rk4
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:795
InsExtPose::ev_vel
struct FloatVect3 ev_vel
Definition ins_ext_pose.c:59
send_ins
static void send_ins(struct transport_tx *trans, struct link_device *dev)
Provide telemetry.
Definition ins_ext_pose.c:102
ins_ext_pose_msg_update
void ins_ext_pose_msg_update(uint8_t *buf)
Import External Pose Message.
Definition ins_ext_pose.c:197
ekf_F
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:417
ins_ext_pos_log_data
void ins_ext_pos_log_data(FILE *file)
Definition ins_ext_pose.c:1300
ekf_P
float ekf_P[EKF_NUM_STATES][EKF_NUM_STATES]
Definition ins_ext_pose.c:326
ekf_R
float ekf_R[EKF_NUM_OUTPUTS][EKF_NUM_OUTPUTS]
Definition ins_ext_pose.c:328
InsExtPose::ltp_def
struct LtpDef_i ltp_def
Definition ins_ext_pose.c:66
accel_ev
static abi_event accel_ev
Definition ins_ext_pose.c:168
gyro_cb
static void gyro_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro)
Definition ins_ext_pose.c:176
InsExtPose::ev_quat
struct FloatQuat ev_quat
Definition ins_ext_pose.c:61
InsExtPose::ltp_pos
struct NedCoor_i ltp_pos
Definition ins_ext_pose.c:69
ekf_init
static void ekf_init(void)
EKF protos.
Definition ins_ext_pose.c:353
InsExtPose::accels_f
struct FloatVect3 accels_f
Definition ins_ext_pose.c:54
ins_ext_pose_init_from_flightplan
static void ins_ext_pose_init_from_flightplan(void)
Definition ins_ext_pose.c:76
InsExtPose::has_new_gyro
bool has_new_gyro
Definition ins_ext_pose.c:55
ekf_U
float ekf_U[EKF_NUM_INPUTS]
Definition ins_ext_pose.c:324
ekf_set_diag
void ekf_set_diag(float **a, float *b, int n)
Definition ins_ext_pose.c:337
t0
float t0
Definition ins_ext_pose.c:333
ins_ext_pose_init
void ins_ext_pose_init(void)
Module.
Definition ins_ext_pose.c:265
InsExtPose::ltp_speed
struct NedCoor_i ltp_speed
Definition ins_ext_pose.c:70
ekf_measurement_step
static void ekf_measurement_step(const float Z[EKF_NUM_OUTPUTS])
Definition ins_ext_pose.c:1077
gyro_ev
static abi_event gyro_ev
Definition ins_ext_pose.c:169
send_ins_ref
static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
Definition ins_ext_pose.c:118
InsExtPose::has_new_ext_pose
bool has_new_ext_pose
Definition ins_ext_pose.c:62
InsExtPose::ev_pos
struct FloatVect3 ev_pos
Definition ins_ext_pose.c:58
InsExtPose::ltp_accel
struct NedCoor_i ltp_accel
Definition ins_ext_pose.c:71
send_ins_z
static void send_ins_z(struct transport_tx *trans, struct link_device *dev)
Definition ins_ext_pose.c:110
ekf_Q
float ekf_Q[EKF_NUM_INPUTS][EKF_NUM_INPUTS]
Definition ins_ext_pose.c:327
InsExtPose::has_new_acc
bool has_new_acc
Definition ins_ext_pose.c:56
DEBUG_PRINT
#define DEBUG_PRINT(...)
Definition ins_ext_pose.c:42
ins_ext_pose_run
void ins_ext_pose_run(void)
Definition ins_ext_pose.c:294
INS_EXT_POSE_IMU_ID
#define INS_EXT_POSE_IMU_ID
Import Gyro and Acc from ABI.
Definition ins_ext_pose.c:164
InsExtPose::ev_time
float ev_time
Definition ins_ext_pose.c:63
accel_cb
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
Definition ins_ext_pose.c:184
t1
float t1
Definition ins_ext_pose.c:334
ekf_step
static void ekf_step(const float U[EKF_NUM_INPUTS], const float Z[EKF_NUM_OUTPUTS], const float dt)
Definition ins_ext_pose.c:844
ins_ext_pos_log_header
void ins_ext_pos_log_header(FILE *file)
Logging.
Definition ins_ext_pose.c:1292
ekf_Z
float ekf_Z[EKF_NUM_OUTPUTS]
Definition ins_ext_pose.c:325
ekf_prediction_step
static void ekf_prediction_step(const float U[EKF_NUM_INPUTS], const float dt)
Definition ins_ext_pose.c:996
send_external_pose_down
static void send_external_pose_down(struct transport_tx *trans, struct link_device *dev)
Definition ins_ext_pose.c:127
ekf_f
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:378
ekf_run
static void ekf_run(void)
Definition ins_ext_pose.c:1165
ekf_X
float ekf_X[EKF_NUM_STATES]
Definition ins_ext_pose.c:323
ekf_H
float ekf_H[EKF_NUM_OUTPUTS][EKF_NUM_STATES]
Definition ins_ext_pose.c:330
send_ahrs_bias
static void send_ahrs_bias(struct transport_tx *trans, struct link_device *dev)
Definition ins_ext_pose.c:142
InsExtPose
Data for telemetry and LTP origin.
Definition ins_ext_pose.c:51
ins_ext_pose.h
Integrated Navigation System interface.
EKF_NUM_OUTPUTS
#define EKF_NUM_OUTPUTS
Definition ins_ext_pose.h:37
EKF_NUM_INPUTS
#define EKF_NUM_INPUTS
Definition ins_ext_pose.h:36
EKF_NUM_STATES
#define EKF_NUM_STATES
Definition ins_ext_pose.h:35
foo
uint16_t foo
Definition main_demo5.c:58
waypoints_localize_all
void waypoints_localize_all(void)
update local ENU coordinates of global waypoints
Definition waypoints.c:357
PRINT_CONFIG_VAR
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF)
pprz_algebra_float.h
Paparazzi floating point algebra.
NedCoor_f::x
float x
in meters
Definition pprz_geodetic_float.h:64
NedCoor_f
vector in North East Down coordinates Units: meters
Definition pprz_geodetic_float.h:63
state.h
API to get/set the generic vehicle states.
dev
static const struct usb_device_descriptor dev
Definition usb_ser_hw.c:74
get_sys_time_float
static float get_sys_time_float(void)
Get the time in seconds since startup.
Definition sys_time.h:138
register_periodic_telemetry
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition telemetry.c:51
telemetry.h
Periodic telemetry system header (includes downlink utility and generated code).
DefaultPeriodic
#define DefaultPeriodic
Set default periodic telemetry.
Definition telemetry.h:66
K
static float K[9]
Definition undistort_image.c:41
uint32_t
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition vl53l1_types.h:78
uint8_t
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition vl53l1_types.h:98
b
float b
Definition wedgebug.c:202