Line data    Source code

       1             : #include "PosVelEKF.h"
       2             : #include <math.h>
       3             : #include <string.h>
       4             : 
       5             : // Initialize the covariance and state matrix
       6             : // This is called when the landing target is located for the first time or it was lost, then relocated
       7          22 : void PosVelEKF::init(float pos, float posVar, float vel, float velVar)
       8             : {
       9          22 :     _state[0] = pos;
      10          22 :     _state[1] = vel;
      11          22 :     _cov[0] = posVar;
      12          22 :     _cov[1] = 0.0f;
      13          22 :     _cov[2] = velVar;
      14          22 : }
      15             : 
      16             : // This functions runs the Prediction Step of the EKF
      17             : // This is called at 400 hz
      18      212080 : void PosVelEKF::predict(float dt, float dVel, float dVelNoise)
      19             : {
      20             :     // Newly predicted state and covariance matrix at next time step
      21             :     float newState[2];
      22             :     float newCov[3];
      23             : 
      24             :     // We assume the following state model for this problem
      25      212080 :     newState[0] = dt*_state[1] + _state[0];
      26      212080 :     newState[1] = dVel + _state[1];
      27             : 
      28             :     /*
      29             :         The above state model is broken down into the needed EKF form:
      30             :         newState = A*OldState + B*u
      31             : 
      32             :         Taking jacobian with respect to state, we derive the A (or F) matrix.
      33             : 
      34             :         A = F = |1 dt|
      35             :                 |0  1|
      36             : 
      37             :         B = |0|
      38             :             |1|
      39             : 
      40             :         u = dVel
      41             : 
      42             :         Covariance Matrix is ALWAYS symmetric, therefore the following matrix is assumed:
      43             :         P = Covariance Matrix = |cov[0]  cov[1]|
      44             :                                 |cov[1]  cov[2]|
      45             : 
      46             :         newCov = F * P * F.transpose + Q
      47             :         Q = |0       0      |
      48             :             |0   dVelNoise^2|
      49             : 
      50             :         Post algebraic operations, and converting it to a upper triangular matrix (because of symmetry)
      51             :         The Updated covariance matrix is of the following form:
      52             :     */
      53             : 
      54      212080 :     newCov[0] = dt*_cov[1] + dt*(dt*_cov[2] + _cov[1]) + _cov[0];
      55      212080 :     newCov[1] = dt*_cov[2] + _cov[1];
      56      212080 :     newCov[2] = ((dVelNoise)*(dVelNoise)) + _cov[2];
      57             : 
      58             :     // store the predicted matrices
      59      212080 :     memcpy(_state,newState,sizeof(_state));
      60      212080 :     memcpy(_cov,newCov,sizeof(_cov));
      61      212080 : }
      62             : 
      63             : // fuse the new sensor measurement into the EKF calculations
      64             : // This is called whenever we have a new measurement available
      65       51722 : void PosVelEKF::fusePos(float pos, float posVar)
      66             : {
      67             :     float newState[2];
      68             :     float newCov[3];
      69             : 
      70             :     // innovation_residual = new_sensor_readings - OldState
      71       51722 :     const float innovation_residual = pos - _state[0];
      72             : 
      73             :     /*
      74             :     Measurement matrix H = [1 0] since we are directly measuring pos only
      75             :     Innovation Covariance = S = H * P * H.Transpose + R
      76             :     Since this is a 1-D measurement, R = posVar, which is expected variance in position sensor reading
      77             :     Post multiplication this becomes:
      78             :     */
      79       51722 :     const float innovation_covariance = _cov[0] + posVar;
      80             : 
      81             :     /*
      82             :     Next step involves calculating the kalman gain "K"
      83             :     K = P * H.transpose * S.inverse
      84             :     After solving, this comes out to be:
      85             :     K = | cov[0]/innovation_covariance |
      86             :         | cov[1]/innovation_covariance |
      87             : 
      88             :     Updated state estimate = OldState + K * innovation residual
      89             :     This is calculated and simplified below
      90             :     */
      91             : 
      92       51722 :     newState[0] = _cov[0]*(innovation_residual)/(innovation_covariance) + _state[0];
      93       51722 :     newState[1] = _cov[1]*(innovation_residual)/(innovation_covariance) + _state[1];
      94             : 
      95             :     /*
      96             :     Updated covariance matrix = (I-K*H)*P
      97             :     This is calculated and simplified below. Again, this is converted to upper triangular matrix (because of symmetry)
      98             :     */
      99             : 
     100       51722 :     newCov[0] = _cov[0] * posVar / innovation_covariance;
     101       51722 :     newCov[1] = _cov[1] * posVar / innovation_covariance;
     102       51722 :     newCov[2] = -_cov[1] * _cov[1] / innovation_covariance + _cov[2];
     103             : 
     104       51722 :     memcpy(_state,newState,sizeof(_state));
     105       51722 :     memcpy(_cov,newCov,sizeof(_cov));
     106       51722 : }
     107             : 
     108             : // Returns normalized innovation squared
     109       53218 : float PosVelEKF::getPosNIS(float pos, float posVar)
     110             : {
     111             :     // NIS = innovation_residual.Transpose * Innovation_Covariance.Inverse * innovation_residual
     112       53218 :     const float innovation_residual = pos - _state[0];
     113       53218 :     const float innovation_covariance = _cov[0] + posVar;
     114             : 
     115       53218 :     const float NIS = (innovation_residual*innovation_residual)/(innovation_covariance);
     116       53218 :     return NIS;
     117             : }