Line data    Source code

       1             : #pragma once
       2             : 
       3             : /*
       4             : * This class implements a simple 1-D Extended Kalman Filter to estimate the Relative body frame position of the lading target and its relative velocity
       5             : * position and velocity of the target is predicted using delta velocity
       6             : * The predictions are corrected periodically using the landing target sensor(or camera)
       7             : */
       8             : class PosVelEKF {
       9             : public:
      10             :     // Initialize the covariance and state matrix
      11             :     // This is called when the landing target is located for the first time or it was lost, then relocated
      12             :     void init(float pos, float posVar, float vel, float velVar);
      13             : 
      14             :     // This functions runs the Prediction Step of the EKF
      15             :     // This is called at 400 hz
      16             :     void predict(float dt, float dVel, float dVelNoise);
      17             : 
      18             :     // fuse the new sensor measurement into the EKF calculations
      19             :     // This is called whenever we have a new measurement available
      20             :     void fusePos(float pos, float posVar);
      21             : 
      22             :     // Get the EKF state position
      23      200678 :     float getPos() const { return _state[0]; }
      24             : 
      25             :     // Get the EKF state velocity
      26      200678 :     float getVel() const { return _state[1]; }
      27             : 
      28             :     // get the normalized innovation squared
      29             :     float getPosNIS(float pos, float posVar);
      30             : 
      31             : private:
      32             :     // stored covariance and state matrix
      33             : 
      34             :     /*
      35             :     _state[0] = position
      36             :     _state[1] = velocity
      37             :     */
      38             :     float _state[2];
      39             : 
      40             :     /*
      41             :     Covariance Matrix is ALWAYS symmetric, therefore the following matrix is assumed:
      42             :     P = Covariance Matrix = |_cov[0]  _cov[1]|
      43             :                             |_cov[1]  _cov[2]|
      44             :     */
      45             :     float _cov[3];
      46             : };