updated hff to accept velocity as only input and add bias

conf/abi.xml

@@ -97,6 +97,14 @@
       <field name="relative_heading" type="float" unit="rad"/>
     </message>
 
+    <message name="POSITION_ESTIMATE" id="18">
+      <field name="stamp" type="uint32_t" unit="us"/>
+      <field name="x" type="float" unit="m"/>
+      <field name="y" type="float" unit="m"/>
+      <field name="z" type="float" unit="m"/>
+      <field name="noise" type="float"/>
+    </message>
+
   </msg_class>
 
 </protocol>

conf/airframes/TUDELFT/tudelft_ardrone2_opticflow.xml

@@ -23,10 +23,8 @@
       <configure name="USE_MAGNETOMETER" value="TRUE"/>
       <define name="AHRS_USE_GPS_HEADING" value="FALSE"/>
     </module>
-    <module name="ins" type="extended"/>
-  </firmware>
-
-  <modules main_freq="512">
+    <module name="ins" type="hff_extended"/>
+    
     <module name="bat_voltage_ardrone2"/>
     <module name="gps" type="ubx_ucenter"/>
     <module name="send_imu_mag_current"/>
@@ -43,9 +41,7 @@
       <define name="VIDEO_CAPTURE_CAMERA" value="bottom_camera"/>
       <define name="VIDEO_CAPTURE_PATH" value="/data/ftp/internal_000/images"/>
     </module>
-
-    <!--module name="opticflow_hover"/-->
-  </modules>
+  </firmware>
 
   <commands>
     <axis name="PITCH" failsafe_value="0"/>

conf/airframes/TUDELFT/tudelft_ardrone2_opticflow_indi.xml

@@ -11,7 +11,7 @@
 
     <define name="USE_SONAR" value="TRUE"/>
 
-	<!-- Subsystem section -->
+    <!-- Subsystem section -->
     <module name="telemetry" type="transparent_udp"/>
     <module name="radio_control" type="datalink"/>
     <module name="motor_mixing"/>
@@ -24,9 +24,7 @@
       <define name="AHRS_USE_GPS_HEADING" value="FALSE"/>
     </module>
     <module name="ins" type="extended"/>
-  </firmware>
-
-  <modules main_freq="512">
+    
     <module name="bat_voltage_ardrone2"/>
     <module name="gps" type="ubx_ucenter"/>
     <module name="send_imu_mag_current"/>
@@ -43,9 +41,7 @@
       <define name="VIDEO_CAPTURE_CAMERA" value="bottom_camera"/>
       <define name="VIDEO_CAPTURE_PATH" value="/data/ftp/internal_000/images"/>
     </module>
-
-    <!--module name="opticflow_hover"/-->
-  </modules>
+  </firmware>
 
   <commands>
     <axis name="PITCH" failsafe_value="0"/>

conf/airframes/TUDELFT/tudelft_bebop2_opticflow.xml

@@ -18,14 +18,11 @@
       <define name="AHRS_USE_GPS_HEADING" value="TRUE"/>
       <define name="AHRS_HEADING_UPDATE_GPS_MIN_SPEED" value="0"/>
     </module>
-    <module name="ins" type="extended"/>
+    <module name="ins" type="hff_extended"/>
     <!-- Disable use of GPS: -->
     <!--    <define name="INS_INT_GPS_ID" value="ABI_DISABLE"/>
     </module> -->
     
-  </firmware>
-
-  <modules main_freq="512">
     <!-- <module name="geo_mag"/>
     <module name="send_imu_mag_current"/> -->
     <module name="air_data"/>
@@ -35,10 +32,9 @@
     </module>
 
     <module name="video_thread"/>
-
     <module name="pose_history"/>
 
-  <module name="cv_opticflow">
+    <module name="cv_opticflow">
       <define name="OPTICFLOW_CAMERA" value="bottom_camera"/>
       <define name="MAX_HORIZON" value="10"/>
       <define name="OPTICFLOW_FX" value="347.22222222"/> <!-- 2.5 / (3.6 * 2.0) * 1000 -->
@@ -62,8 +58,7 @@
       <define name="VIEWVIDEO_DOWNSIZE_FACTOR" value="1"/>
       <define name="VIEWVIDEO_QUALITY_FACTOR" value="60"/>
     </module> -->
-
-  </modules>
+  </firmware>
 
   <commands>
     <axis name="PITCH" failsafe_value="0"/>

conf/airframes/TUDELFT/tudelft_bebop_opticflow.xml

@@ -12,7 +12,7 @@
 
     <define name="USE_SONAR"/>
 
-	<!-- Subsystem section -->
+    <!-- Subsystem section -->
     <module name="telemetry" type="transparent_udp"/>
     <module name="radio_control" type="datalink"/>
     <module name="motor_mixing"/>
@@ -24,10 +24,8 @@
       <configure name="USE_MAGNETOMETER" value="TRUE"/>
       <define name="AHRS_USE_GPS_HEADING" value="FALSE"/>
     </module>
-    <module name="ins" type="extended"/>
-  </firmware>
-
-  <modules main_freq="512">
+    <module name="ins" type="hff_extended"/>
+    
     <module name="geo_mag"/>
     <module name="air_data"/>
     <module name="send_imu_mag_current"/>
@@ -55,9 +53,7 @@
       <define name="VIEWVIDEO_DOWNSIZE_FACTOR" value="1"/>
       <define name="VIEWVIDEO_QUALITY_FACTOR" value="60"/>
     </module-->
-
-
-  </modules>
+  </firmware>
 
   <commands>
     <axis name="PITCH" failsafe_value="0"/>

conf/conf_example.xml

@@ -292,7 +292,7 @@
    radio="radios/dummy.xml"
    telemetry="telemetry/default_rotorcraft.xml"
    flight_plan="flight_plans/rotorcraft_optitrack.xml"
-   settings=""
+   settings="settings/rotorcraft_basic.xml"
    settings_modules="modules/cv_opticflow.xml modules/ins_extended.xml modules/ahrs_int_cmpl_quat.xml modules/stabilization_indi_simple.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml"
    gui_color="blue"
   />

conf/modules/ins_hff_extended.xml

@@ -0,0 +1,43 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="ins" dir="ins">
+  <doc>
+    <description>
+      extended INS with vertical filter using sonar.
+    </description>
+    <define name="USE_INS_NAV_INIT" value="TRUE|FALSE" description="Initialize the origin of the local coordinate system from flight plan. (Default: TRUE)"/>
+    <define name="INS_INT_SONAR_ID" value="ABI_BROADCAST" description="The ABI sender id of the sonar to use"/>
+    <define name="INS_INT_BARO_ID" value="BARO_BOARD_SENDER_ID" description="The ABI sender id of the baro to use"/>
+    <define name="INS_INT_GPS_ID" value="GPS_MULTI_ID" description="The ABI sender id of the GPS to use"/>
+    <define name="INS_INT_IMU_ID" value="ABI_BROADCAST" description="The ABI sender id of the IMU to use"/>
+    <define name="INS_INT_VEL_ID" value="ABI_BROADCAST" description="The ABI sender id of the VELOCITY_ESTIMATE (e.g. from opticflow"/>
+    <define name="INS_SONAR_MIN_RANGE" value="0.001" description="min sonar range in meters"/>
+    <define name="INS_SONAR_MAX_RANGE" value="4.0" description="max sonar range in meters"/>
+    <define name="INS_SONAR_UPDATE_ON_AGL" value="FALSE" description="assume flat ground and use sonar for height"/>
+  </doc>
+
+  <settings>
+    <dl_settings NAME="Ins Extended">
+      <dl_settings NAME="INS">
+        <dl_setting MAX="10" MIN="0" STEP="0.1" VAR="vff.accel_noise" shortname="accel_noise" module="subsystems/ins/vf_extended_float"/>
+        <dl_setting MAX="10" MIN="0" STEP="0.1" VAR="vff.r_baro" shortname="r_baro"/>
+        <dl_setting MAX="10" MIN="0" STEP="0.1" VAR="vff.r_alt" shortname="r_alt"/>
+        <dl_setting MAX="10" MIN="0" STEP="0.1" VAR="vff.r_offset" shortname="r_offset"/>
+      </dl_settings>
+    </dl_settings>
+  </settings>
+
+  <header>
+    <file name="ins_int.h" dir="subsystems/ins"/>
+  </header>
+  <init fun="ins_int_init()"/>
+  <makefile target="ap|nps">
+    <define name="INS_TYPE_H" value="subsystems/ins/ins_int.h" type="string"/>
+    <file name="ins.c" dir="subsystems"/>
+    <file name="ins_int.c" dir="subsystems/ins"/>
+    <file name="vf_extended_float.c" dir="subsystems/ins"/>
+    <define name="USE_VFF_EXTENDED"/>
+    <file name="hf_float.c" dir="subsystems/ins"/>
+    <define name="USE_HFF"/>
+  </makefile>
+</module>

sw/airborne/subsystems/ins/hf_float.h

@@ -31,32 +31,34 @@
 
 #include "std.h"
 #include "math/pprz_algebra_float.h"
+#include "math/pprz_algebra_int.h"
 #include "generated/airframe.h"
 
-#define HFF_STATE_SIZE 2
+// X = [ z zdot bias ]
+#define HFF_STATE_SIZE 3
 
 struct HfilterFloat {
   float x;
-  /* float xbias; */
   float xdot;
   float xdotdot;
+  float xbias;
   float y;
-  /* float ybias; */
   float ydot;
   float ydotdot;
+  float ybias;
   float xP[HFF_STATE_SIZE][HFF_STATE_SIZE];
   float yP[HFF_STATE_SIZE][HFF_STATE_SIZE];
   uint16_t lag_counter;
   bool rollback;
 };
 
-extern struct HfilterFloat b2_hff_state;
+extern struct HfilterFloat hff;
 
-extern void b2_hff_init(float init_x, float init_xdot, float init_y, float init_ydot);
-extern void b2_hff_propagate(void);
-extern void b2_hff_update_gps(struct FloatVect2 *pos_ned, struct FloatVect2 *speed_ned);
-extern void b2_hff_update_pos(struct FloatVect2 pos, struct FloatVect2 Rpos);
-extern void b2_hff_update_vel(struct FloatVect2 vel, struct FloatVect2 Rvel);
-extern void b2_hff_realign(struct FloatVect2 pos, struct FloatVect2 vel);
+extern void hff_init(float init_x, float init_xdot, float init_y, float init_ydot);
+extern void hff_propagate(void);
+extern void hff_update_gps(struct FloatVect2 *pos_ned, struct FloatVect2 *speed_ned);
+extern void hff_update_pos(struct FloatVect2 pos, struct FloatVect2 Rpos);
+extern void hff_update_vel(struct FloatVect2 vel, struct FloatVect2 Rvel);
+extern void hff_realign(struct FloatVect2 pos, struct FloatVect2 vel);
 
 #endif /* HF_FLOAT_H */

sw/airborne/subsystems/ins/ins_int.c

@@ -143,7 +143,18 @@ static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s);
 #define INS_INT_VEL_ID ABI_BROADCAST
 #endif
 static abi_event vel_est_ev;
-static void vel_est_cb(uint8_t sender_id, uint32_t stamp, float x, float y, float z, float noise);
+static void vel_est_cb(uint8_t sender_id,
+                       uint32_t stamp,
+                       float x, float y, float z,
+                       float noise);
+#ifndef INS_INT_POS_ID
+#define INS_INT_POS_ID ABI_BROADCAST
+#endif
+static abi_event pos_est_ev;
+static void pos_est_cb(uint8_t sender_id,
+                       uint32_t stamp,
+                       float x, float y, float z,
+                       float noise);
 
 struct InsInt ins_int;
 
@@ -211,7 +222,7 @@ void ins_int_init(void)
   /* init vertical and horizontal filters */
   vff_init_zero();
 #if USE_HFF
-  b2_hff_init(0., 0., 0., 0.);
+  hff_init(0., 0., 0., 0.);
 #endif
 
   INT32_VECT3_ZERO(ins_int.ltp_pos);
@@ -230,6 +241,7 @@ void ins_int_init(void)
   AbiBindMsgIMU_ACCEL_INT32(INS_INT_IMU_ID, &accel_ev, accel_cb);
   AbiBindMsgGPS(INS_INT_GPS_ID, &gps_ev, gps_cb);
   AbiBindMsgVELOCITY_ESTIMATE(INS_INT_VEL_ID, &vel_est_ev, vel_est_cb);
+  AbiBindMsgPOSITION_ESTIMATE(INS_INT_POS_ID, &pos_est_ev, pos_est_cb);
 }
 
 void ins_reset_local_origin(void)
@@ -298,7 +310,7 @@ void ins_int_propagate(struct Int32Vect3 *accel, float dt)
 
 #if USE_HFF
   /* propagate horizontal filter */
-  b2_hff_propagate();
+  hff_propagate();
   /* convert and copy result to ins_int */
   ins_update_from_hff();
 #else
@@ -333,11 +345,11 @@ static void baro_cb(uint8_t __attribute__((unused)) sender_id, float pressure)
 
       // Calculate the distance to the origin
       struct EnuCoor_f *enu = stateGetPositionEnu_f();
-      double dist2_to_origin = enu->x*enu->x + enu->y*enu->y;
+      double dist2_to_origin = enu->x * enu->x + enu->y * enu->y;
 
       // correction for the earth's curvature
       const double earth_radius = 6378137.0;
-      float height_correction = (float) (sqrt(earth_radius*earth_radius + dist2_to_origin) - earth_radius);
+      float height_correction = (float)(sqrt(earth_radius * earth_radius + dist2_to_origin) - earth_radius);
 
       // The VFF will update in the NED frame
       ins_int.baro_z = -(baro_up - height_correction);
@@ -406,15 +418,14 @@ void ins_int_update_gps(struct GpsState *gps_s)
 
   struct FloatVect2 gps_speed_m_s_ned;
   VECT2_ASSIGN(gps_speed_m_s_ned, gps_speed_cm_s_ned.x, gps_speed_cm_s_ned.y);
-  VECT2_SDIV(gps_speed_m_s_ned, gps_speed_m_s_ned, 100.);
+  VECT2_SDIV(gps_speed_m_s_ned, gps_speed_m_s_ned, 100.f);
 
   if (ins_int.hf_realign) {
     ins_int.hf_realign = false;
-    const struct FloatVect2 zero = {0.0f, 0.0f};
-    b2_hff_realign(gps_pos_m_ned, zero);
+    hff_realign(gps_pos_m_ned, gps_speed_m_s_ned);
   }
   // run horizontal filter
-  b2_hff_update_gps(&gps_pos_m_ned, &gps_speed_m_s_ned);
+  hff_update_gps(&gps_pos_m_ned, &gps_speed_m_s_ned);
   // convert and copy result to ins_int
   ins_update_from_hff();
 
@@ -489,12 +500,12 @@ static void ins_update_from_vff(void)
 /** update ins state from horizontal filter */
 static void ins_update_from_hff(void)
 {
-  ins_int.ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_state.xdotdot);
-  ins_int.ltp_accel.y = ACCEL_BFP_OF_REAL(b2_hff_state.ydotdot);
-  ins_int.ltp_speed.x = SPEED_BFP_OF_REAL(b2_hff_state.xdot);
-  ins_int.ltp_speed.y = SPEED_BFP_OF_REAL(b2_hff_state.ydot);
-  ins_int.ltp_pos.x   = POS_BFP_OF_REAL(b2_hff_state.x);
-  ins_int.ltp_pos.y   = POS_BFP_OF_REAL(b2_hff_state.y);
+  ins_int.ltp_accel.x = ACCEL_BFP_OF_REAL(hff.xdotdot);
+  ins_int.ltp_accel.y = ACCEL_BFP_OF_REAL(hff.ydotdot);
+  ins_int.ltp_speed.x = SPEED_BFP_OF_REAL(hff.xdot);
+  ins_int.ltp_speed.y = SPEED_BFP_OF_REAL(hff.ydot);
+  ins_int.ltp_pos.x   = POS_BFP_OF_REAL(hff.x);
+  ins_int.ltp_pos.y   = POS_BFP_OF_REAL(hff.y);
 }
 #endif
 
@@ -520,15 +531,15 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)),
   ins_int_update_gps(gps_s);
 }
 
+/* body relative velocity estimate
+ *
+ */
 static void vel_est_cb(uint8_t sender_id __attribute__((unused)),
-                       uint32_t stamp,
+                       uint32_t stamp __attribute__((unused)),
                        float x, float y, float z,
-                       float noise __attribute__((unused)))
+                       float noise)
 {
-
   struct FloatVect3 vel_body = {x, y, z};
-  static uint32_t last_stamp = 0;
-  float dt = 0;
 
   /* rotate velocity estimate to nav/ltp frame */
   struct FloatQuat q_b2n = *stateGetNedToBodyQuat_f();
@@ -536,29 +547,53 @@ static void vel_est_cb(uint8_t sender_id __attribute__((unused)),
   struct FloatVect3 vel_ned;
   float_quat_vmult(&vel_ned, &q_b2n, &vel_body);
 
-  if (last_stamp > 0) {
-    dt = (float)(stamp - last_stamp) * 1e-6;
-  }
-
-  last_stamp = stamp;
-
 #if USE_HFF
-  (void)dt; //dt is unused variable in this define
-
   struct FloatVect2 vel = {vel_ned.x, vel_ned.y};
   struct FloatVect2 Rvel = {noise, noise};
 
-  b2_hff_update_vel(vel,  Rvel);
+  hff_update_vel(vel,  Rvel);
   ins_update_from_hff();
 #else
   ins_int.ltp_speed.x = SPEED_BFP_OF_REAL(vel_ned.x);
   ins_int.ltp_speed.y = SPEED_BFP_OF_REAL(vel_ned.y);
+
+  static uint32_t last_stamp = 0;
   if (last_stamp > 0) {
+    float dt = (float)(stamp - last_stamp) * 1e-6;
     ins_int.ltp_pos.x = ins_int.ltp_pos.x + POS_BFP_OF_REAL(dt * vel_ned.x);
     ins_int.ltp_pos.y = ins_int.ltp_pos.y + POS_BFP_OF_REAL(dt * vel_ned.y);
   }
+  last_stamp = stamp;
 #endif
 
+  vff_update_vz_conf(vel_ned.z, noise);
+
+  ins_ned_to_state();
+
+  /* reset the counter to indicate we just had a measurement update */
+  ins_int.propagation_cnt = 0;
+}
+
+/* NED position estimate relative to ltp origin
+ */
+static void pos_est_cb(uint8_t sender_id __attribute__((unused)),
+                       uint32_t stamp __attribute__((unused)),
+                       float x, float y, float z,
+                       float noise)
+{
+#if USE_HFF
+  struct FloatVect2 pos = {x, y};
+  struct FloatVect2 Rpos = {noise, noise};
+
+  hff_update_pos(pos, Rpos);
+  ins_update_from_hff();
+#else
+  ins_int.ltp_pos.x = POS_BFP_OF_REAL(x);
+  ins_int.ltp_pos.y = POS_BFP_OF_REAL(y);
+#endif
+
+  vff_update_z_conf(z, noise);
+
   ins_ned_to_state();
 
   /* reset the counter to indicate we just had a measurement update */