AP_NavEKF2: Add methods to output and calculate vertical position derivative

This patch calculates a derivative of the vertical position (positive down).
The derivative is exposed via a public function.

libraries/AP_NavEKF2/AP_NavEKF2.cpp

@@ -460,6 +460,15 @@ void NavEKF2::getVelNED(Vector3f &vel) const
     }
 }
 
+// Return the rate of change of vertical position in the down diection (dPosD/dt) in m/s
+void NavEKF2::getPosDownDerivative(float &ret) const
+{
+    // return the value calculated from a complmentary filer applied to the EKF height and vertical acceleration
+    if (core) {
+        core->getPosDownDerivative(ret);
+    }
+}
+
 // This returns the specific forces in the NED frame
 void NavEKF2::getAccelNED(Vector3f &accelNED) const
 {

libraries/AP_NavEKF2/AP_NavEKF2.h

@@ -65,6 +65,11 @@ public:
     // return NED velocity in m/s
     void getVelNED(Vector3f &vel) const;
 
+    // Return the rate of change of vertical position in the down diection (dPosD/dt) in m/s
+    // This can be different to the z component of the EKF velocity state because it will fluctuate with height errors and corrections in the EKF
+    // but will always be kinematically consistent with the z component of the EKF position state
+    void getPosDownDerivative(float &ret) const;
+
     // This returns the specific forces in the NED frame
     void getAccelNED(Vector3f &accelNED) const;
 

libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp

@@ -149,6 +149,13 @@ void NavEKF2_core::getVelNED(Vector3f &vel) const
     vel = outputDataNew.velocity;
 }
 
+// Return the rate of change of vertical position in the down diection (dPosD/dt) in m/s
+void NavEKF2_core::getPosDownDerivative(float &ret) const
+{
+    // return the value calculated from a complmentary filer applied to the EKF height and vertical acceleration
+    ret = posDownDerivative;
+}
+
 // This returns the specific forces in the NED frame
 void NavEKF2_core::getAccelNED(Vector3f &accelNED) const {
     accelNED = velDotNED;

libraries/AP_NavEKF2/AP_NavEKF2_core.cpp

@@ -186,6 +186,8 @@ void NavEKF2_core::InitialiseVariables()
     gpsVertVelFilt = 0.0f;
     gpsHorizVelFilt = 0.0f;
     memset(&statesArray, 0, sizeof(statesArray));
+    posDownDerivative = 0.0f;
+    posDown = 0.0f;
 }
 
 // Initialise the states from accelerometer and magnetometer data (if present)
@@ -608,6 +610,13 @@ void  NavEKF2_core::calcOutputStatesFast() {
     const float Kpos = 1.0f;
     velCorrection = (stateStruct.position - outputDataDelayed.position) * Kpos;
 
+    // update vertical velocity and position states used to provide a vertical position derivative output
+    // using a simple complementary filter
+    float lastPosDownDerivative = posDownDerivative;
+    posDownDerivative += delVelNav.z;
+    float posDownDerivativeCorrection = 0.2f * (outputDataNew.position.z - posDown);
+    posDown += (posDownDerivative + lastPosDownDerivative) * (imuDataNew.delVelDT*0.5f) + posDownDerivativeCorrection * imuDataNew.delVelDT;
+
 }
 
 // calculate the predicted state covariance matrix
@@ -1169,6 +1178,9 @@ void NavEKF2_core::StoreOutputReset()
         storedOutput[i] = outputDataNew;
     }
     outputDataDelayed = outputDataNew;
+    // reset the states for the complementary filter used to provide a vertical position dervative output
+    posDown = stateStruct.position.z;
+    posDownDerivative = stateStruct.velocity.z;
 }
 
 // Reset the stored output quaternion history to current EKF state

libraries/AP_NavEKF2/AP_NavEKF2_core.h

@@ -68,6 +68,11 @@ public:
     // return NED velocity in m/s
     void getVelNED(Vector3f &vel) const;
 
+    // Return the rate of change of vertical position in the down diection (dPosD/dt) in m/s
+    // This can be different to the z component of the EKF velocity state because it will fluctuate with height errors and corrections in the EKF
+    // but will always be kinematically consistent with the z component of the EKF position state
+    void getPosDownDerivative(float &ret) const;
+
     // This returns the specific forces in the NED frame
     void getAccelNED(Vector3f &accelNED) const;
 
@@ -738,6 +743,10 @@ private:
     bool motorsArmed;               // true when the motors have been armed
     bool prevMotorsArmed;           // value of motorsArmed from previous frame
 
+    // variables used to calulate a vertical velocity that is kinematically consistent with the verical position
+    float posDownDerivative;        // Rate of chage of vertical position (dPosD/dt) in m/s. This is the first time derivative of PosD.
+    float posDown;                  // Down position state used in calculation of posDownRate
+
     // variables used by the pre-initialisation GPS checks
     struct Location gpsloc_prev;    // LLH location of previous GPS measurement
     uint32_t lastPreAlignGpsCheckTime_ms;   // last time in msec the GPS quality was checked during pre alignment checks