Implement full pose measurement in EKF.

This adds two new messages to the crtp_localization_service to
receive external pose information (compressed and not compressed)
such as information obtained by a motion capture system. (Fixes
issue #203).

It also adds a full pose update in the EKF using a scalar update
for both position and orientation errors. (Fixes issue #246).

Tested with the Crazyswarm using a single Crazyflie and a VICON
motion capture system.

src/modules/interface/crtp_localization_service.h

@@ -38,6 +38,17 @@ struct CrtpExtPosition
   float z; // in m
 } __attribute__((packed));
 
+struct CrtpExtPose
+{
+  float x; // in m
+  float y; // in m
+  float z; // in m
+  float qx;
+  float qy;
+  float qz;
+  float qw;
+} __attribute__((packed));
+
 typedef enum
 {
   RANGE_STREAM_FLOAT      = 0,
@@ -47,6 +58,8 @@ typedef enum
   EMERGENCY_STOP_WATCHDOG = 4,
   COMM_GNSS_NMEA           = 6,
   COMM_GNSS_PROPRIETARY    = 7,
+  EXT_POSE                 = 8,
+  EXT_POSE_PACKED          = 9,
 } locsrv_t;
 
 // Set up the callback for the CRTP_PORT_LOCALIZATION

src/modules/interface/estimator.h

@@ -44,6 +44,7 @@ const char* stateEstimatorGetName();
 // Support to incorporate additional sensors into the state estimate via the following functions:
 bool estimatorEnqueueTDOA(const tdoaMeasurement_t *uwb);
 bool estimatorEnqueuePosition(const positionMeasurement_t *pos);
+bool estimatorEnqueuePose(const poseMeasurement_t *pose);
 bool estimatorEnqueueDistance(const distanceMeasurement_t *dist);
 bool estimatorEnqueueTOF(const tofMeasurement_t *tof);
 bool estimatorEnqueueAbsoluteHeight(const heightMeasurement_t *height);

src/modules/interface/estimator_kalman.h

@@ -67,6 +67,7 @@ void estimatorKalman(state_t *state, sensorData_t *sensors, control_t *control,
  */
 bool estimatorKalmanEnqueueTDOA(const tdoaMeasurement_t *uwb);
 bool estimatorKalmanEnqueuePosition(const positionMeasurement_t *pos);
+bool estimatorKalmanEnqueuePose(const poseMeasurement_t *pose);
 bool estimatorKalmanEnqueueDistance(const distanceMeasurement_t *dist);
 bool estimatorKalmanEnqueueTOF(const tofMeasurement_t *tof);
 bool estimatorKalmanEnqueueAbsoluteHeight(const heightMeasurement_t *height);

src/modules/interface/kalman_core.h

@@ -110,6 +110,9 @@ void kalmanCoreUpdateWithAbsoluteHeight(kalmanCoreData_t* this, heightMeasuremen
 // Direct measurements of Crazyflie position
 void kalmanCoreUpdateWithPosition(kalmanCoreData_t* this, positionMeasurement_t *xyz);
 
+// Direct measurements of Crazyflie pose
+void kalmanCoreUpdateWithPose(kalmanCoreData_t* this, poseMeasurement_t *pose);
+
 // Distance-to-point measurements
 void kalmanCoreUpdateWithDistance(kalmanCoreData_t* this, distanceMeasurement_t *d);
 

src/modules/interface/stabilizer_types.h

@@ -101,6 +101,20 @@ typedef struct positionMeasurement_s {
   float stdDev;
 } positionMeasurement_t;
 
+typedef struct poseMeasurement_s {
+  union {
+    struct {
+      float x;
+      float y;
+      float z;
+    };
+    float pos[3];
+  };
+  quaternion_t quat;
+  float stdDevPos;
+  float stdDevQuat;
+} poseMeasurement_t;
+
 typedef struct distanceMeasurement_s {
   union {
     struct {

src/modules/src/crtp_localization_service.c

@@ -41,6 +41,7 @@
 #include "locodeck.h"
 
 #include "estimator.h"
+#include "quatcompress.h"
 
 #define NBR_OF_RANGES_IN_PACKET   5
 #define DEFAULT_EMERGENCY_STOP_TIMEOUT (1 * RATE_MAIN_LOOP)
@@ -70,12 +71,25 @@ typedef struct {
   int16_t z; // mm
 } __attribute__((packed)) extPositionPackedItem;
 
+// up to 2 items per CRTP packet
+typedef struct {
+  uint8_t id; // last 8 bit of the Crazyflie address
+  int16_t x; // mm
+  int16_t y; // mm
+  int16_t z; // mm
+  uint32_t quat; // compressed quaternion, see quatcompress.h
+} __attribute__((packed)) extPosePackedItem;
+
 // Struct for logging position information
 static positionMeasurement_t ext_pos;
+// Struct for logging pose information
+static poseMeasurement_t ext_pose;
+
 static CRTPPacket pkRange;
 static uint8_t rangeIndex;
 static bool enableRangeStreamFloat = false;
 static float extPosStdDev = 0.01;
+static float extQuatStdDev = 4.5e-3;
 static bool isInit = false;
 static uint8_t my_id;
 
@@ -141,6 +155,33 @@ static void genericLocHandle(CRTPPacket* pk)
     stabilizerSetEmergencyStop();
   } else if (type == EMERGENCY_STOP_WATCHDOG) {
     stabilizerSetEmergencyStopTimeout(DEFAULT_EMERGENCY_STOP_TIMEOUT);
+  } else if (type == EXT_POSE) {
+    const struct CrtpExtPose* data = (const struct CrtpExtPose*)&pk->data[1];
+    ext_pose.x = data->x;
+    ext_pose.y = data->y;
+    ext_pose.z = data->z;
+    ext_pose.quat.x = data->qx;
+    ext_pose.quat.y = data->qy;
+    ext_pose.quat.z = data->qz;
+    ext_pose.quat.w = data->qw;
+    ext_pose.stdDevPos = extPosStdDev;
+    ext_pose.stdDevQuat = extQuatStdDev;
+    estimatorEnqueuePose(&ext_pose);
+  } else if (type == EXT_POSE_PACKED) {
+    uint8_t numItems = (pk->size - 1) / sizeof(extPosePackedItem);
+    for (uint8_t i = 0; i < numItems; ++i) {
+      const extPosePackedItem* item = (const extPosePackedItem*)&pk->data[1 + i * sizeof(extPosePackedItem)];
+      if (item->id == my_id) {
+        ext_pose.x = item->x / 1000.0f;
+        ext_pose.y = item->y / 1000.0f;
+        ext_pose.z = item->z / 1000.0f;
+        quatdecompress(item->quat, (float *)&ext_pose.quat.q0);
+        ext_pose.stdDevPos = extPosStdDev;
+        ext_pose.stdDevQuat = extQuatStdDev;
+        estimatorEnqueuePose(&ext_pose);
+        break;
+      }
+    }
   }
 }
 
@@ -206,4 +247,5 @@ LOG_GROUP_STOP(ext_pos)
 PARAM_GROUP_START(locSrv)
   PARAM_ADD(PARAM_UINT8, enRangeStreamFP32, &enableRangeStreamFloat)
   PARAM_ADD(PARAM_FLOAT, extPosStdDev, &extPosStdDev)
+  PARAM_ADD(PARAM_FLOAT, extQuatStdDev, &extQuatStdDev)
 PARAM_GROUP_STOP(locSrv)

src/modules/src/estimator.c

@@ -18,6 +18,7 @@ typedef struct {
   const char* name;
   bool (*estimatorEnqueueTDOA)(const tdoaMeasurement_t *uwb);
   bool (*estimatorEnqueuePosition)(const positionMeasurement_t *pos);
+  bool (*estimatorEnqueuePose)(const poseMeasurement_t *pose);
   bool (*estimatorEnqueueDistance)(const distanceMeasurement_t *dist);
   bool (*estimatorEnqueueTOF)(const tofMeasurement_t *tof);
   bool (*estimatorEnqueueAbsoluteHeight)(const heightMeasurement_t *height);
@@ -34,6 +35,7 @@ static EstimatorFcns estimatorFunctions[] = {
     .name = "None",
     .estimatorEnqueueTDOA = NOT_IMPLEMENTED,
     .estimatorEnqueuePosition = NOT_IMPLEMENTED,
+    .estimatorEnqueuePose = NOT_IMPLEMENTED,
     .estimatorEnqueueDistance = NOT_IMPLEMENTED,
     .estimatorEnqueueTOF = NOT_IMPLEMENTED,
     .estimatorEnqueueAbsoluteHeight = NOT_IMPLEMENTED,
@@ -46,6 +48,7 @@ static EstimatorFcns estimatorFunctions[] = {
     .name = "Complementary",
     .estimatorEnqueueTDOA = NOT_IMPLEMENTED,
     .estimatorEnqueuePosition = NOT_IMPLEMENTED,
+    .estimatorEnqueuePose = NOT_IMPLEMENTED,
     .estimatorEnqueueDistance = NOT_IMPLEMENTED,
     .estimatorEnqueueTOF = NOT_IMPLEMENTED,
     .estimatorEnqueueAbsoluteHeight = NOT_IMPLEMENTED,
@@ -58,6 +61,7 @@ static EstimatorFcns estimatorFunctions[] = {
     .name = "Kalman",
     .estimatorEnqueueTDOA = estimatorKalmanEnqueueTDOA,
     .estimatorEnqueuePosition = estimatorKalmanEnqueuePosition,
+    .estimatorEnqueuePose = estimatorKalmanEnqueuePose,
     .estimatorEnqueueDistance = estimatorKalmanEnqueueDistance,
     .estimatorEnqueueTOF = estimatorKalmanEnqueueTOF,
     .estimatorEnqueueAbsoluteHeight = estimatorKalmanEnqueueAbsoluteHeight,
@@ -125,6 +129,14 @@ bool estimatorEnqueuePosition(const positionMeasurement_t *pos) {
   return false;
 }
 
+bool estimatorEnqueuePose(const poseMeasurement_t *pose) {
+  if (estimatorFunctions[currentEstimator].estimatorEnqueuePose) {
+    return estimatorFunctions[currentEstimator].estimatorEnqueuePose(pose);
+  }
+
+  return false;
+}
+
 bool estimatorEnqueueDistance(const distanceMeasurement_t *dist) {
   if (estimatorFunctions[currentEstimator].estimatorEnqueueDistance) {
     return estimatorFunctions[currentEstimator].estimatorEnqueueDistance(dist);

src/modules/src/estimator_kalman.c

@@ -102,6 +102,14 @@ static inline bool stateEstimatorHasPositionMeasurement(positionMeasurement_t *p
   return (pdTRUE == xQueueReceive(posDataQueue, pos, 0));
 }
 
+// Direct measurements of Crazyflie pose
+static xQueueHandle poseDataQueue;
+#define POSE_QUEUE_LENGTH (10)
+
+static inline bool stateEstimatorHasPoseMeasurement(poseMeasurement_t *pose) {
+  return (pdTRUE == xQueueReceive(poseDataQueue, pose, 0));
+}
+
 // Measurements of a UWB Tx/Rx
 static xQueueHandle tdoaDataQueue;
 #define UWB_QUEUE_LENGTH (10)
@@ -369,6 +377,13 @@ void estimatorKalman(state_t *state, sensorData_t *sensors, control_t *control,
     doneUpdate = true;
   }
 
+  poseMeasurement_t pose;
+  while (stateEstimatorHasPoseMeasurement(&pose))
+  {
+    kalmanCoreUpdateWithPose(&coreData, &pose);
+    doneUpdate = true;
+  }
+
   tdoaMeasurement_t tdoa;
   while (stateEstimatorHasTDOAPacket(&tdoa))
   {
@@ -423,6 +438,7 @@ void estimatorKalmanInit(void) {
   {
     distDataQueue = xQueueCreate(DIST_QUEUE_LENGTH, sizeof(distanceMeasurement_t));
     posDataQueue = xQueueCreate(POS_QUEUE_LENGTH, sizeof(positionMeasurement_t));
+    poseDataQueue = xQueueCreate(POSE_QUEUE_LENGTH, sizeof(poseMeasurement_t));
     tdoaDataQueue = xQueueCreate(UWB_QUEUE_LENGTH, sizeof(tdoaMeasurement_t));
     flowDataQueue = xQueueCreate(FLOW_QUEUE_LENGTH, sizeof(flowMeasurement_t));
     tofDataQueue = xQueueCreate(TOF_QUEUE_LENGTH, sizeof(tofMeasurement_t));
@@ -432,6 +448,7 @@ void estimatorKalmanInit(void) {
   {
     xQueueReset(distDataQueue);
     xQueueReset(posDataQueue);
+    xQueueReset(poseDataQueue);
     xQueueReset(tdoaDataQueue);
     xQueueReset(flowDataQueue);
     xQueueReset(tofDataQueue);
@@ -487,6 +504,12 @@ bool estimatorKalmanEnqueuePosition(const positionMeasurement_t *pos)
   return stateEstimatorEnqueueExternalMeasurement(posDataQueue, (void *)pos);
 }
 
+bool estimatorKalmanEnqueuePose(const poseMeasurement_t *pose)
+{
+  ASSERT(isInit);
+  return stateEstimatorEnqueueExternalMeasurement(poseDataQueue, (void *)pose);
+}
+
 bool estimatorKalmanEnqueueDistance(const distanceMeasurement_t *dist)
 {
   ASSERT(isInit);

src/modules/src/kalman_core.c

@@ -63,6 +63,7 @@
 
 #include "log.h"
 #include "param.h"
+#include "math3d.h"
 
 // #define DEBUG_STATE_CHECK
 
@@ -328,6 +329,41 @@ void kalmanCoreUpdateWithPosition(kalmanCoreData_t* this, positionMeasurement_t
   }
 }
 
+void kalmanCoreUpdateWithPose(kalmanCoreData_t* this, poseMeasurement_t *pose)
+{
+  // a direct measurement of states x, y, and z, and orientation
+  // do a scalar update for each state, since this should be faster than updating all together
+  for (int i=0; i<3; i++) {
+    float h[KC_STATE_DIM] = {0};
+    arm_matrix_instance_f32 H = {1, KC_STATE_DIM, h};
+    h[KC_STATE_X+i] = 1;
+    scalarUpdate(this, &H, pose->pos[i] - this->S[KC_STATE_X+i], pose->stdDevPos);
+  }
+
+  // compute orientation error
+  struct quat const q_ekf = mkquat(this->q[1], this->q[2], this->q[3], this->q[0]);
+  struct quat const q_measured = mkquat(pose->quat.x, pose->quat.y, pose->quat.z, pose->quat.w);
+  struct quat const q_residual = qqmul(qinv(q_ekf), q_measured);
+  // small angle approximation, see eq. 141 in http://mars.cs.umn.edu/tr/reports/Trawny05b.pdf
+  struct vec const err_quat = vscl(2.0f / q_residual.w, quatimagpart(q_residual));
+
+  // do a scalar update for each state
+  {
+    float h[KC_STATE_DIM] = {0};
+    arm_matrix_instance_f32 H = {1, KC_STATE_DIM, h};
+    h[KC_STATE_D0] = 1;
+    scalarUpdate(this, &H, err_quat.x, pose->stdDevQuat);
+    h[KC_STATE_D0] = 0;
+
+    h[KC_STATE_D1] = 1;
+    scalarUpdate(this, &H, err_quat.y, pose->stdDevQuat);
+    h[KC_STATE_D1] = 0;
+
+    h[KC_STATE_D2] = 1;
+    scalarUpdate(this, &H, err_quat.z, pose->stdDevQuat);
+  }
+}
+
 void kalmanCoreUpdateWithDistance(kalmanCoreData_t* this, distanceMeasurement_t *d)
 {
   // a measurement of distance to point (x, y, z)