#239: Fixed flow behaviour when body is rotating

Thanks @mgreiff for the implementation

src/modules/src/estimator_kalman.c

@@ -1010,8 +1010,8 @@ static void stateEstimatorUpdateWithFlow(flowMeasurement_t *flow, sensorData_t *
   // ~~~ Camera constants ~~~
   // The angle of aperture is guessed from the raw data register and thankfully look to be symmetric
   float Npix = 30.0;                      // [pixels] (same in x and y)
-  float thetapix = DEG_TO_RAD * 4.0f;    // [rad]    (same in x and y)
-
+  //float thetapix = DEG_TO_RAD * 4.0f;     // [rad]    (same in x and y)
+  float thetapix = DEG_TO_RAD * 4.2f;
   //~~~ Body rates ~~~
   // TODO check if this is feasible or if some filtering has to be done
   omegax_b = sensors->gyro.x * DEG_TO_RAD;
@@ -1025,8 +1025,11 @@ static void stateEstimatorUpdateWithFlow(flowMeasurement_t *flow, sensorData_t *
   //
   // where \hat{} denotes a basis vector, \dot{} denotes a derivative and
   // _G and _B refer to the global/body coordinate systems.
-  // dx_g = R[0][0] * S[STATE_PX] + R[1][0] * S[STATE_PY] + R[2][0] * S[STATE_PZ];
-  // dy_g = R[0][1] * S[STATE_PX] + R[1][1] * S[STATE_PY] + R[2][1] * S[STATE_PZ];
+
+  // Modification 1
+  //dx_g = R[0][0] * S[STATE_PX] + R[0][1] * S[STATE_PY] + R[0][2] * S[STATE_PZ];
+  //dy_g = R[1][0] * S[STATE_PX] + R[1][1] * S[STATE_PY] + R[1][2] * S[STATE_PZ];
+
 
   dx_g = S[STATE_PX];
   dy_g = S[STATE_PY];
@@ -1039,9 +1042,10 @@ static void stateEstimatorUpdateWithFlow(flowMeasurement_t *flow, sensorData_t *
 
   // ~~~ X velocity prediction and update ~~~
   // predics the number of accumulated pixels in the x-direction
+  float omegaFactor = 1.25f;
   float hx[STATE_DIM] = {0};
   arm_matrix_instance_f32 Hx = {1, STATE_DIM, hx};
-  predictedNX = (flow->dt * Npix / thetapix ) * ((dx_g * R[2][2] / z_g) - omegay_b);
+  predictedNX = (flow->dt * Npix / thetapix ) * ((dx_g * R[2][2] / z_g) - omegaFactor * omegay_b);
   measuredNX = flow->dpixelx;
 
   // derive measurement equation with respect to dx (and z?)
@@ -1054,7 +1058,7 @@ static void stateEstimatorUpdateWithFlow(flowMeasurement_t *flow, sensorData_t *
   // ~~~ Y velocity prediction and update ~~~
   float hy[STATE_DIM] = {0};
   arm_matrix_instance_f32 Hy = {1, STATE_DIM, hy};
-  predictedNY = (flow->dt * Npix / thetapix ) * ((dy_g * R[2][2] / z_g) + omegax_b);
+  predictedNY = (flow->dt * Npix / thetapix ) * ((dy_g * R[2][2] / z_g) + omegaFactor * omegax_b);
   measuredNY = flow->dpixely;
 
   // derive measurement equation with respect to dy (and z?)
@@ -1073,13 +1077,19 @@ static void stateEstimatorUpdateWithTof(tofMeasurement_t *tof)
 
   // Only update the filter if the measurement is reliable (\hat{h} -> infty when R[2][2] -> 0)
   if (fabs(R[2][2]) > 0.1 && R[2][2] > 0){
+    float angle = fabsf(acosf(R[2][2])) - DEG_TO_RAD * (15.0f / 2.0f);
+    if (angle < 0.0f) {
+      angle = 0.0f;
+    }
+    //float predictedDistance = S[STATE_Z] / cosf(angle);
     float predictedDistance = S[STATE_Z] / R[2][2];
     float measuredDistance = tof->distance; // [m]
 
     //Measurement equation
     //
     // h = z/((R*z_b)\dot z_b) = z/cos(alpha)
     h[STATE_Z] = 1 / R[2][2];
+    //h[STATE_Z] = 1 / cosf(angle);
 
     // Scalar update
     stateEstimatorScalarUpdate(&H, measuredDistance-predictedDistance, tof->stdDev);