[INS_EXT_POSE] Kalman Filter based on external attitude measurements (#2993)

conf/modules/ins_ext_pose.xml

@@ -0,0 +1,35 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="ins_ext_pose" dir="ins">
+  <doc>
+    <description>
+      Extened Kalman filter based on https://en.wikipedia.org/wiki/Extended_Kalman_filter
+      
+      Designed to merge only IMU and OptiTrack/VICON (no baro, sonar, GPS, ...)
+      
+       - predict attitude and path with gyroscopes and accelerometers
+       - correct attitude and path with external pose and position message
+       - estimate gyro and accelerometer biases
+       
+      Bypassed when simulating.
+    </description>
+  </doc>
+  <dep>
+    <depends>@gps,@datalink,@imu</depends>
+    <provides>ahrs,ins</provides>
+  </dep>
+  <autoload name="ins_sim"/>
+  <autoload name="ins_nps"/>
+  <header>
+    <file name="ins_ext_pose.h"/>
+  </header>
+  <init fun="ins_ext_pose_init()"/>
+  <periodic fun="ins_ext_pose_run()" freq="512" />
+  <datalink message="EXTERNAL_POSE" fun="ins_ext_pose_msg_update(buf)"/>
+
+  <makefile target="ap">
+    <define name="INS_TYPE_H" value="modules/ins/ins_ext_pose.h" type="string"/>
+    <file name="ins_ext_pose.c"/>
+    <file name="ins.c"/>
+  </makefile>
+</module>

sw/airborne/modules/ins/ins_ext_pose.h

@@ -0,0 +1,56 @@
+/*
+ * Copyright (C) 2023 MAVLab
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file modules/ins/ins_ext_pose.h
+ * Integrated Navigation System interface.
+ */
+
+/*
+Extened kalman filter based on https://en.wikipedia.org/wiki/Extended_Kalman_filter
+Section 5.3: Non-additive noise formulation and equations
+*/
+
+#ifndef INS_EXT_POSE_H
+#define INS_EXT_POSE_H
+
+#define EKF_NUM_STATES 15
+#define EKF_NUM_INPUTS 6
+#define EKF_NUM_OUTPUTS 6
+
+#include "std.h"
+
+#include <stdio.h>
+
+
+extern float ekf_X[EKF_NUM_STATES];
+
+extern void ins_ext_pose_init(void);
+extern void ins_ext_pose_run(void);
+
+extern void ins_ext_pose_msg_update(uint8_t *buf);
+
+// Logging
+extern void ins_ext_pos_log_header(FILE *file);
+extern void ins_ext_pos_log_data(FILE *file);
+
+
+#endif

sw/airborne/modules/loggers/logger_file.c

@@ -43,6 +43,7 @@
 #include "firmwares/fixedwing/stabilization/stabilization_adaptive.h"
 #endif
 
+#include "generated/modules.h"
 
 /** Set the default File logger path to the USB drive */
 #ifndef LOGGER_FILE_PATH
@@ -67,6 +68,13 @@ static void logger_file_write_header(FILE *file) {
   fprintf(file, "vel_x,vel_y,vel_z,");
   fprintf(file, "att_phi,att_theta,att_psi,");
   fprintf(file, "rate_p,rate_q,rate_r,");
+#ifdef BOARD_BEBOP
+  fprintf(file, "rpm_obs_1,rpm_obs_2,rpm_obs_3,rpm_obs_4,");
+  fprintf(file, "rpm_ref_1,rpm_ref_2,rpm_ref_3,rpm_ref_4,");
+#endif
+#ifdef INS_EXT_POSE_H
+  ins_ext_pos_log_header(file);
+#endif
 #ifdef COMMAND_THRUST
   fprintf(file, "cmd_thrust,cmd_roll,cmd_pitch,cmd_yaw\n");
 #else
@@ -91,6 +99,13 @@ static void logger_file_write_row(FILE *file) {
   fprintf(file, "%f,%f,%f,", vel->x, vel->y, vel->z);
   fprintf(file, "%f,%f,%f,", att->phi, att->theta, att->psi);
   fprintf(file, "%f,%f,%f,", rates->p, rates->q, rates->r);
+#ifdef BOARD_BEBOP
+  fprintf(file, "%d,%d,%d,%d,",actuators_bebop.rpm_obs[0],actuators_bebop.rpm_obs[1],actuators_bebop.rpm_obs[2],actuators_bebop.rpm_obs[3]);
+  fprintf(file, "%d,%d,%d,%d,",actuators_bebop.rpm_ref[0],actuators_bebop.rpm_ref[1],actuators_bebop.rpm_ref[2],actuators_bebop.rpm_ref[3]);
+#endif
+#ifdef INS_EXT_POSE_H
+  ins_ext_pos_log_data(file);
+#endif
 #ifdef COMMAND_THRUST
   fprintf(file, "%d,%d,%d,%d\n",
       stabilization_cmd[COMMAND_THRUST], stabilization_cmd[COMMAND_ROLL],
@@ -104,6 +119,9 @@ static void logger_file_write_row(FILE *file) {
 /** Start the file logger and open a new file */
 void logger_file_start(void)
 {
+  // Ensure that the module is running when started with this function
+  logger_file_logger_file_periodic_status = MODULES_RUN;
+  
   // Create output folder if necessary
   if (access(STRINGIFY(LOGGER_FILE_PATH), F_OK)) {
     char save_dir_cmd[256];