Merge pull request #1189 from hmllr/flow_understandability_fixes

flow sensor eKF update minor fixes

src/deck/drivers/src/flowdeck_v1v2.c

@@ -120,7 +120,13 @@ static void flowdeckTask(void *param)
     flowMeasurement_t flowData;
     flowData.stdDevX = stdFlow;
     flowData.stdDevY = stdFlow;
-    flowData.dt = 0.01;
+    flowData.dt = (float)(usecTimestamp()-lastTime)/1000000.0f;
+    // we do want to update dt every measurement and not only in the ones with detected motion, 
+    // as we work with instantaneous gyro and velocity values in the update function 
+    // (meaning assuming the current measurements over all of dt)
+    lastTime = usecTimestamp(); 
+
+
 
 #if defined(USE_MA_SMOOTHING)
       // Use MA Smoothing
@@ -151,8 +157,6 @@ static void flowdeckTask(void *param)
       // Push measurements into the estimator if flow is not disabled
       //    and the PMW flow sensor indicates motion detection
       if (!useFlowDisabled && currentMotion.motion == 0xB0) {
-        flowData.dt = (float)(usecTimestamp()-lastTime)/1000000.0f;
-        lastTime = usecTimestamp();
         estimatorEnqueueFlow(&flowData);
       }
     } else {

src/modules/src/kalman_core/mm_flow.c

@@ -26,6 +26,8 @@
 #include "mm_flow.h"
 #include "log.h"
 
+#define FLOW_RESOLUTION 0.1f //We do get the measurements in 10x the motion pixels (experimentally measured)
+
 // TODO remove the temporary test variables (used for logging)
 static float predictedNX;
 static float predictedNY;
@@ -38,9 +40,9 @@ void kalmanCoreUpdateWithFlow(kalmanCoreData_t* this, const flowMeasurement_t *f
 
   // ~~~ 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 Npix = 35.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.2f;
+  float thetapix = 0.71674f;// 2*sin(42/2); 42degree is the agnle of aperture, here we computed the corresponding ground length
   //~~~ Body rates ~~~
   // TODO check if this is feasible or if some filtering has to be done
   float omegax_b = gyro->x * DEG_TO_RAD;
@@ -71,32 +73,31 @@ void kalmanCoreUpdateWithFlow(kalmanCoreData_t* this, const flowMeasurement_t *f
   }
 
   // ~~~ X velocity prediction and update ~~~
-  // predics the number of accumulated pixels in the x-direction
-  float omegaFactor = 1.25f;
+  // predicts the number of accumulated pixels in the x-direction
   float hx[KC_STATE_DIM] = {0};
   arm_matrix_instance_f32 Hx = {1, KC_STATE_DIM, hx};
-  predictedNX = (flow->dt * Npix / thetapix ) * ((dx_g * this->R[2][2] / z_g) - omegaFactor * omegay_b);
-  measuredNX = flow->dpixelx;
+  predictedNX = (flow->dt * Npix / thetapix ) * ((dx_g * this->R[2][2] / z_g) - omegay_b);
+  measuredNX = flow->dpixelx*FLOW_RESOLUTION;
 
   // derive measurement equation with respect to dx (and z?)
   hx[KC_STATE_Z] = (Npix * flow->dt / thetapix) * ((this->R[2][2] * dx_g) / (-z_g * z_g));
   hx[KC_STATE_PX] = (Npix * flow->dt / thetapix) * (this->R[2][2] / z_g);
 
   //First update
-  kalmanCoreScalarUpdate(this, &Hx, measuredNX-predictedNX, flow->stdDevX);
+  kalmanCoreScalarUpdate(this, &Hx, (measuredNX-predictedNX), flow->stdDevX*FLOW_RESOLUTION);
 
   // ~~~ Y velocity prediction and update ~~~
   float hy[KC_STATE_DIM] = {0};
   arm_matrix_instance_f32 Hy = {1, KC_STATE_DIM, hy};
-  predictedNY = (flow->dt * Npix / thetapix ) * ((dy_g * this->R[2][2] / z_g) + omegaFactor * omegax_b);
-  measuredNY = flow->dpixely;
+  predictedNY = (flow->dt * Npix / thetapix ) * ((dy_g * this->R[2][2] / z_g) + omegax_b);
+  measuredNY = flow->dpixely*FLOW_RESOLUTION;
 
   // derive measurement equation with respect to dy (and z?)
   hy[KC_STATE_Z] = (Npix * flow->dt / thetapix) * ((this->R[2][2] * dy_g) / (-z_g * z_g));
   hy[KC_STATE_PY] = (Npix * flow->dt / thetapix) * (this->R[2][2] / z_g);
 
   // Second update
-  kalmanCoreScalarUpdate(this, &Hy, measuredNY-predictedNY, flow->stdDevY);
+  kalmanCoreScalarUpdate(this, &Hy, (measuredNY-predictedNY), flow->stdDevY*FLOW_RESOLUTION);
 }
 
 /**