From 290174f9d99fcbebe3fa2eb96cf10e064f5e23c3 Mon Sep 17 00:00:00 2001 From: Rishabh Date: Wed, 2 Jun 2021 16:09:11 +0530 Subject: [PATCH] AC_PrecLand: NFC: Refactor EKF code --- libraries/AC_PrecLand/PosVelEKF.cpp | 136 ++++++++++++++++------------ libraries/AC_PrecLand/PosVelEKF.h | 34 ++++++- 2 files changed, 111 insertions(+), 59 deletions(-) diff --git a/libraries/AC_PrecLand/PosVelEKF.cpp b/libraries/AC_PrecLand/PosVelEKF.cpp index 935a3d4ba1..7576b37c4e 100644 --- a/libraries/AC_PrecLand/PosVelEKF.cpp +++ b/libraries/AC_PrecLand/PosVelEKF.cpp @@ -2,42 +2,8 @@ #include #include -#define POSVELEKF_POS_CALC_NIS(__P, __R, __X, __Z, __RET_NIS) \ -__RET_NIS = ((-__X[0] + __Z)*(-__X[0] + __Z))/(__P[0] + __R); - -#define POSVELEKF_POS_CALC_STATE(__P, __R, __X, __Z, __RET_STATE) \ -__RET_STATE[0] = __P[0]*(-__X[0] + __Z)/(__P[0] + __R) + __X[0]; __RET_STATE[1] = __P[1]*(-__X[0] + \ -__Z)/(__P[0] + __R) + __X[1]; - -#define POSVELEKF_POS_CALC_COV(__P, __R, __X, __Z, __RET_COV) \ -__RET_COV[0] = ((__P[0])*(__P[0]))*__R/((__P[0] + __R)*(__P[0] + __R)) + __P[0]*((-__P[0]/(__P[0] + \ -__R) + 1)*(-__P[0]/(__P[0] + __R) + 1)); __RET_COV[1] = __P[0]*__P[1]*__R/((__P[0] + __R)*(__P[0] + \ -__R)) - __P[0]*__P[1]*(-__P[0]/(__P[0] + __R) + 1)/(__P[0] + __R) + __P[1]*(-__P[0]/(__P[0] + __R) + \ -1); __RET_COV[2] = ((__P[1])*(__P[1]))*__R/((__P[0] + __R)*(__P[0] + __R)) - \ -((__P[1])*(__P[1]))/(__P[0] + __R) - __P[1]*(-__P[0]*__P[1]/(__P[0] + __R) + __P[1])/(__P[0] + __R) + \ -__P[2]; - -#define POSVELEKF_PREDICTION_CALC_STATE(__P, __DT, __DV, __DV_NOISE, __X, __RET_STATE) \ -__RET_STATE[0] = __DT*__X[1] + __X[0]; __RET_STATE[1] = __DV + __X[1]; - -#define POSVELEKF_PREDICTION_CALC_COV(__P, __DT, __DV, __DV_NOISE, __X, __RET_COV) \ -__RET_COV[0] = __DT*__P[1] + __DT*(__DT*__P[2] + __P[1]) + __P[0]; __RET_COV[1] = __DT*__P[2] + \ -__P[1]; __RET_COV[2] = ((__DV_NOISE)*(__DV_NOISE)) + __P[2]; - -#define POSVELEKF_VEL_CALC_NIS(__P, __R, __X, __Z, __RET_NIS) \ -__RET_NIS = ((-__X[1] + __Z)*(-__X[1] + __Z))/(__P[2] + __R); - -#define POSVELEKF_VEL_CALC_STATE(__P, __R, __X, __Z, __RET_STATE) \ -__RET_STATE[0] = __P[1]*(-__X[1] + __Z)/(__P[2] + __R) + __X[0]; __RET_STATE[1] = __P[2]*(-__X[1] + \ -__Z)/(__P[2] + __R) + __X[1]; - -#define POSVELEKF_VEL_CALC_COV(__P, __R, __X, __Z, __RET_COV) \ -__RET_COV[0] = __P[0] + ((__P[1])*(__P[1]))*__R/((__P[2] + __R)*(__P[2] + __R)) - \ -((__P[1])*(__P[1]))/(__P[2] + __R) - __P[1]*(-__P[1]*__P[2]/(__P[2] + __R) + __P[1])/(__P[2] + __R); \ -__RET_COV[1] = __P[1]*__P[2]*__R/((__P[2] + __R)*(__P[2] + __R)) + (-__P[2]/(__P[2] + __R) + \ -1)*(-__P[1]*__P[2]/(__P[2] + __R) + __P[1]); __RET_COV[2] = ((__P[2])*(__P[2]))*__R/((__P[2] + \ -__R)*(__P[2] + __R)) + __P[2]*((-__P[2]/(__P[2] + __R) + 1)*(-__P[2]/(__P[2] + __R) + 1)); - +// Initialize the covariance and state matrix +// This is called when the landing target is located for the first time or it was lost, then relocated void PosVelEKF::init(float pos, float posVar, float vel, float velVar) { _state[0] = pos; @@ -47,47 +13,105 @@ void PosVelEKF::init(float pos, float posVar, float vel, float velVar) _cov[2] = velVar; } +// This functions runs the Prediction Step of the EKF +// This is called at 400 hz void PosVelEKF::predict(float dt, float dVel, float dVelNoise) { + // Newly predicted state and covariance matrix at next time step float newState[2]; float newCov[3]; - POSVELEKF_PREDICTION_CALC_STATE(_cov, dt, dVel, dVelNoise, _state, newState) - POSVELEKF_PREDICTION_CALC_COV(_cov, dt, dVel, dVelNoise, _state, newCov) + // We assume the following state model for this problem + newState[0] = dt*_state[1] + _state[0]; + newState[1] = dVel + _state[1]; + /* + The above state model is broken down into the needed EKF form: + newState = A*OldState + B*u + + Taking jacobian with respect to state, we derive the A (or F) matrix. + + A = F = |1 dt| + |0 1| + + B = |0| + |1| + + u = dVel + + Covariance Matrix is ALWAYS symmetric, therefore the following matrix is assumed: + P = Covariance Matrix = |cov[0] cov[1]| + |cov[1] cov[2]| + + newCov = F * P * F.transpose + Q + Q = |0 0 | + |0 dVelNoise^2| + + Post algebraic operations, and converting it to a upper triangular matrix (because of symmetry) + The Updated covariance matrix is of the following form: + */ + + newCov[0] = dt*_cov[1] + dt*(dt*_cov[2] + _cov[1]) + _cov[0]; + newCov[1] = dt*_cov[2] + _cov[1]; + newCov[2] = ((dVelNoise)*(dVelNoise)) + _cov[2]; + + // store the predicted matrices memcpy(_state,newState,sizeof(_state)); memcpy(_cov,newCov,sizeof(_cov)); } +// fuse the new sensor measurement into the EKF calculations +// This is called whenever we have a new measurement available void PosVelEKF::fusePos(float pos, float posVar) { float newState[2]; float newCov[3]; - POSVELEKF_POS_CALC_STATE(_cov, posVar, _state, pos, newState) - POSVELEKF_POS_CALC_COV(_cov, posVar, _state, pos, newCov) - - memcpy(_state,newState,sizeof(_state)); - memcpy(_cov,newCov,sizeof(_cov)); -} - -void PosVelEKF::fuseVel(float vel, float velVar) -{ - float newState[2]; - float newCov[3]; - - POSVELEKF_VEL_CALC_STATE(_cov, velVar, _state, vel, newState) - POSVELEKF_VEL_CALC_COV(_cov, velVar, _state, vel, newCov) + // innovation_residual = new_sensor_readings - OldState + const float innovation_residual = pos - _state[0]; + + /* + Measurement matrix H = [1 0] since we are directly measuring pos only + Innovation Covariance = S = H * P * H.Transpose + R + Since this is a 1-D measurement, R = posVar, which is expected variance in postion sensor reading + Post multiplication this becomes: + */ + const float innovation_covariance = _cov[0] + posVar; + + /* + Next step involves calculating the kalman gain "K" + K = P * H.transpose * S.inverse + After solving, this comes out to be: + K = | cov[0]/innovation_covariance | + | cov[1]/innovation_covariance | + + Updated state estimate = OldState + K * innovation residual + This is calculated and simplified below + */ + + newState[0] = _cov[0]*(innovation_residual)/(innovation_covariance) + _state[0]; + newState[1] = _cov[1]*(innovation_residual)/(innovation_covariance) + _state[1]; + + /* + Updated covariance matrix = (I-K*H)*P + This is calculated and simplified below. Again, this is converted to upper triangular matrix (because of symmetry) + */ + + newCov[0] = _cov[0] * posVar / innovation_covariance; + newCov[1] = _cov[1] * posVar / innovation_covariance; + newCov[2] = -_cov[1] * _cov[1] / innovation_covariance + _cov[2]; memcpy(_state,newState,sizeof(_state)); memcpy(_cov,newCov,sizeof(_cov)); } +// Returns normalized innovation squared float PosVelEKF::getPosNIS(float pos, float posVar) { - float ret; + // NIS = innovation_residual.Transpose * Innovation_Covariance.Inverse * innovation_residual + const float innovation_residual = pos - _state[0]; + const float innovation_covariance = _cov[0] + posVar; - POSVELEKF_POS_CALC_NIS(_cov, posVar, _state, pos, ret) - - return ret; + const float NIS = (innovation_residual*innovation_residual)/(innovation_covariance); + return NIS; } diff --git a/libraries/AC_PrecLand/PosVelEKF.h b/libraries/AC_PrecLand/PosVelEKF.h index a769abd9e7..3648c5a7a6 100644 --- a/libraries/AC_PrecLand/PosVelEKF.h +++ b/libraries/AC_PrecLand/PosVelEKF.h @@ -1,18 +1,46 @@ #pragma once +/* +* This class implements a simple 1-D Extended Kalman Filter to estimate the Relative body frame postion of the lading target and its relative velocity +* position and velocity of the target is predicted using delta velocity +* The predictions are corrected periodically using the landing target sensor(or camera) +*/ class PosVelEKF { public: + // Initialize the covariance and state matrix + // This is called when the landing target is located for the first time or it was lost, then relocated void init(float pos, float posVar, float vel, float velVar); - void predict(float dt, float dVel, float dVelNoise); - void fusePos(float pos, float posVar); - void fuseVel(float vel, float velVar); + // This functions runs the Prediction Step of the EKF + // This is called at 400 hz + void predict(float dt, float dVel, float dVelNoise); + + // fuse the new sensor measurement into the EKF calculations + // This is called whenever we have a new measurement available + void fusePos(float pos, float posVar); + + // Get the EKF state position float getPos() const { return _state[0]; } + + // Get the EKF state velocity float getVel() const { return _state[1]; } + // get the normalized innovation squared float getPosNIS(float pos, float posVar); private: + // stored covariance and state matrix + + /* + _state[0] = position + _state[1] = velocity + */ float _state[2]; + + /* + Covariance Matrix is ALWAYS symmetric, therefore the following matrix is assumed: + P = Covariance Matrix = |_cov[0] _cov[1]| + |_cov[1] _cov[2]| + */ float _cov[3]; };