#406 Added initial yaw for the kalman estimator

The initial yaw can be set through the kalman.initialYaw parameter. The initial yaw is used to initialize the estimator when the Crzyflie is not oriented in the positive X direction.

Initial Yaw is set in radians where:
0: facing positive X (default)
PI / 2: facing positive Y
PI: facing negative X
3 * PI / 2: facing negative Y

Note: initial yaw takes effect after the kalman filter is reset.

src/modules/src/kalman_core.c

@@ -155,6 +155,16 @@ static float initialX = 0.0;
 static float initialY = 0.0;
 static float initialZ = 0.0;
 
+// Initial yaw of the Crazyflie in radians.
+// 0 --- facing positive X
+// PI / 2 --- facing positive Y
+// PI --- facing negative X
+// 3 * PI / 2 --- facing negative Y
+static float initialYaw = 0.0;
+
+// Quaternion used for initial yaw
+static float initialQuaternion[4] = {0.0, 0.0, 0.0, 0.0};
+
 static uint32_t tdoaCount;
 
 
@@ -175,7 +185,11 @@ void kalmanCoreInit(kalmanCoreData_t* this) {
 //  this->S[KC_STATE_D2] = 0;
 
   // reset the attitude quaternion
-  this->q[0] = 1; // this->q[1] = 0; this->q[2] = 0; this->q[3] = 0;
+  initialQuaternion[0] = arm_cos_f32(initialYaw / 2);
+  initialQuaternion[1] = 0.0;
+  initialQuaternion[2] = 0.0;
+  initialQuaternion[3] = arm_sin_f32(initialYaw / 2);
+  for (int i = 0; i < 4; i++) { this->q[i] = initialQuaternion[i]; }
 
   // then set the initial rotation matrix to the identity. This only affects
   // the first prediction step, since in the finalization, after shifting
@@ -730,20 +744,20 @@ void kalmanCorePredict(kalmanCoreData_t* this, float cmdThrust, Axis3f *acc, Axi
   float tmpq2;
   float tmpq3;
 
-  if (quadIsFlying) {
-    // rotate the quad's attitude by the delta quaternion vector computed above
-    tmpq0 = (dq[0]*this->q[0] - dq[1]*this->q[1] - dq[2]*this->q[2] - dq[3]*this->q[3]);
-    tmpq1 = (1.0f)*(dq[1]*this->q[0] + dq[0]*this->q[1] + dq[3]*this->q[2] - dq[2]*this->q[3]);
-    tmpq2 = (1.0f)*(dq[2]*this->q[0] - dq[3]*this->q[1] + dq[0]*this->q[2] + dq[1]*this->q[3]);
-    tmpq3 = (1.0f)*(dq[3]*this->q[0] + dq[2]*this->q[1] - dq[1]*this->q[2] + dq[0]*this->q[3]);
-  } else {
-    // rotate the quad's attitude by the delta quaternion vector computed above
-    tmpq0 = (dq[0]*this->q[0] - dq[1]*this->q[1] - dq[2]*this->q[2] - dq[3]*this->q[3]);
-    tmpq1 = (1.0f-ROLLPITCH_ZERO_REVERSION)*(dq[1]*this->q[0] + dq[0]*this->q[1] + dq[3]*this->q[2] - dq[2]*this->q[3]);
-    tmpq2 = (1.0f-ROLLPITCH_ZERO_REVERSION)*(dq[2]*this->q[0] - dq[3]*this->q[1] + dq[0]*this->q[2] + dq[1]*this->q[3]);
-    tmpq3 = (1.0f-ROLLPITCH_ZERO_REVERSION)*(dq[3]*this->q[0] + dq[2]*this->q[1] - dq[1]*this->q[2] + dq[0]*this->q[3]);
-  }
+  // rotate the quad's attitude by the delta quaternion vector computed above
+  tmpq0 = dq[0]*this->q[0] - dq[1]*this->q[1] - dq[2]*this->q[2] - dq[3]*this->q[3];
+  tmpq1 = dq[1]*this->q[0] + dq[0]*this->q[1] + dq[3]*this->q[2] - dq[2]*this->q[3];
+  tmpq2 = dq[2]*this->q[0] - dq[3]*this->q[1] + dq[0]*this->q[2] + dq[1]*this->q[3];
+  tmpq3 = dq[3]*this->q[0] + dq[2]*this->q[1] - dq[1]*this->q[2] + dq[0]*this->q[3];
 
+  if (! quadIsFlying) {
+    float keep = 1.0f - ROLLPITCH_ZERO_REVERSION;
+
+    tmpq0 = keep * tmpq0 + ROLLPITCH_ZERO_REVERSION * initialQuaternion[0];
+    tmpq1 = keep * tmpq1 + ROLLPITCH_ZERO_REVERSION * initialQuaternion[1];
+    tmpq2 = keep * tmpq2 + ROLLPITCH_ZERO_REVERSION * initialQuaternion[2];
+    tmpq3 = keep * tmpq3 + ROLLPITCH_ZERO_REVERSION * initialQuaternion[3];
+  }
 
   // normalize and store the result
   float norm = arm_sqrt(tmpq0*tmpq0 + tmpq1*tmpq1 + tmpq2*tmpq2 + tmpq3*tmpq3);
@@ -1008,4 +1022,5 @@ PARAM_GROUP_START(kalman)
   PARAM_ADD(PARAM_FLOAT, initialX, &initialX)
   PARAM_ADD(PARAM_FLOAT, initialY, &initialY)
   PARAM_ADD(PARAM_FLOAT, initialZ, &initialZ)
+  PARAM_ADD(PARAM_FLOAT, initialYaw, &initialYaw)
 PARAM_GROUP_STOP(kalman)