ins_ext_pose module

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
• drop delayed poses using the capture time in the EXTERNAL_POSE timestamp field (ms)
• drop poses whose attitude innovation is too large (reset after repeated rejections)

Bypassed when simulating.

Example for airframe file

Add to your firmware section: This example contains all possible configuration options, not all of them are mandatory!

<module name="ins_ext_pose">
  b'<define name="INS_EXT_POSE_MAX_AGE" value="30." />\n    '
  b'<define name="INS_EXT_POSE_GATE_PSI" value="RadOfDeg(40.)" />\n    '
  b'<define name="INS_EXT_POSE_GATE_TILT" value="RadOfDeg(15.)" />\n    '
  b'<define name="INS_EXT_POSE_GATE_RESET_NB" value="10" />\n    '
  b'<define name="INS_EXT_POSE_TIMEOUT" value="0.5" />\n  '
</module>

Module configuration options

Define Options

• name: INS_EXT_POSE_MAX_AGE value: 30.
  Description: maximum pose age above the lowest receive delay [ms], 0 to disable
• name: INS_EXT_POSE_GATE_PSI value: RadOfDeg(40.)
  Description: heading innovation gate [rad], 0 to disable
• name: INS_EXT_POSE_GATE_TILT value: RadOfDeg(15.)
  Description: roll/pitch innovation gate [rad], 0 to disable
• name: INS_EXT_POSE_GATE_RESET_NB value: 10
  Description: consecutive gated poses before resetting the filter on the pose
• name: INS_EXT_POSE_TIMEOUT value: 0.5
  Description: time without fused pose before the filter status is reported lost [s]

Module functions

Init Functions

These initialization functions are called once on startup.

• ins_ext_pose_init()

Periodic Functions

These functions are called periodically at the specified frequency from the module periodic loop.

• ins_ext_pose_run()
  • Running at maximum module frequency.
  • Autorun: LOCK
    Periodic function automatically starts after init and can't be stopped.

Datalink Functions

Whenever the specified datalink message is received, the corresponing handler function is called.

• on message EXTERNAL_POSE call ins_ext_pose_msg_update(buf)

Files

Header Files

The following headers are automatically included in modules.h

• ins_ext_pose.h

Source Files

• modules/ins/ins_ext_pose.c
• modules/ins/ins.c

Raw ins_ext_pose.xml file:

<!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
       - drop delayed poses using the capture time in the EXTERNAL_POSE timestamp field (ms)
       - drop poses whose attitude innovation is too large (reset after repeated rejections)
       
      Bypassed when simulating.
    </description>
    <define name="INS_EXT_POSE_MAX_AGE" value="30." description="maximum pose age above the lowest receive delay [ms], 0 to disable"/>
    <define name="INS_EXT_POSE_GATE_PSI" value="RadOfDeg(40.)" description="heading innovation gate [rad], 0 to disable"/>
    <define name="INS_EXT_POSE_GATE_TILT" value="RadOfDeg(15.)" description="roll/pitch innovation gate [rad], 0 to disable"/>
    <define name="INS_EXT_POSE_GATE_RESET_NB" value="10" description="consecutive gated poses before resetting the filter on the pose"/>
    <define name="INS_EXT_POSE_TIMEOUT" value="0.5" description="time without fused pose before the filter status is reported lost [s]"/>
  </doc>
  <settings>
    <dl_settings>
      <dl_settings name="ins_ext_pose">
        <dl_setting var="ins_ext_pose_max_age" min="0" step="1" max="200" shortname="max_age" unit="ms"/>
        <dl_setting var="ins_ext_pose_gate_psi" min="0" step="0.0175" max="3.15" shortname="gate_psi" unit="rad" alt_unit="deg"/>
        <dl_setting var="ins_ext_pose_gate_tilt" min="0" step="0.0175" max="1.58" shortname="gate_tilt" unit="rad" alt_unit="deg"/>
      </dl_settings>
    </dl_settings>
  </settings>
  <dep>
    <depends>@gps,@datalink,@imu</depends>
    <provides>ahrs,ins</provides>
  </dep>
  <header>
    <file name="ins_ext_pose.h"/>
  </header>
  <init fun="ins_ext_pose_init()"/>
  <periodic fun="ins_ext_pose_run()"/>
  <datalink message="EXTERNAL_POSE" fun="ins_ext_pose_msg_update(buf)"/>
 
  <makefile target="ap">
    <define name="INS_EXT_POSE" value="TRUE"/>
    <file name="ins_ext_pose.c"/>
    <file name="ins.c"/>
  </makefile>
</module>