Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_PrecLand: NFC: Refactor EKF code

This commit is contained in:
Rishabh 2021-06-02 16:09:11 +05:30 committed by Randy Mackay
parent 4fde26aa01
commit 290174f9d9
2 changed files with 111 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

136
libraries/AC_PrecLand/PosVelEKF.cpp
View File

@ -2,42 +2,8 @@
#include <math.h> #include <math.h>
#include <string.h> #include <string.h>
#define POSVELEKF_POS_CALC_NIS(__P, __R, __X, __Z, __RET_NIS) \ // Initialize the covariance and state matrix
__RET_NIS = ((-__X[0] + __Z)*(-__X[0] + __Z))/(__P[0] + __R); // This is called when the landing target is located for the first time or it was lost, then relocated
#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));
void PosVelEKF::init(float pos, float posVar, float vel, float velVar) void PosVelEKF::init(float pos, float posVar, float vel, float velVar)
{ {
_state[0] = pos; _state[0] = pos;
@ -47,47 +13,105 @@ void PosVelEKF::init(float pos, float posVar, float vel, float velVar)
_cov[2] = 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) void PosVelEKF::predict(float dt, float dVel, float dVelNoise)
{ {
// Newly predicted state and covariance matrix at next time step
float newState[2]; float newState[2];
float newCov[3]; float newCov[3];
POSVELEKF_PREDICTION_CALC_STATE(_cov, dt, dVel, dVelNoise, _state, newState) // We assume the following state model for this problem
POSVELEKF_PREDICTION_CALC_COV(_cov, dt, dVel, dVelNoise, _state, newCov) 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(_state,newState,sizeof(_state));
memcpy(_cov,newCov,sizeof(_cov)); 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) void PosVelEKF::fusePos(float pos, float posVar)
{ {
float newState[2]; float newState[2];
float newCov[3]; float newCov[3];
POSVELEKF_POS_CALC_STATE(_cov, posVar, _state, pos, newState) // innovation_residual = new_sensor_readings - OldState
POSVELEKF_POS_CALC_COV(_cov, posVar, _state, pos, newCov) const float innovation_residual = pos - _state[0];
memcpy(_state,newState,sizeof(_state)); /*
memcpy(_cov,newCov,sizeof(_cov)); 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
void PosVelEKF::fuseVel(float vel, float velVar) Post multiplication this becomes:
{ */
float newState[2]; const float innovation_covariance = _cov[0] + posVar;
float newCov[3];
/*
POSVELEKF_VEL_CALC_STATE(_cov, velVar, _state, vel, newState) Next step involves calculating the kalman gain "K"
POSVELEKF_VEL_CALC_COV(_cov, velVar, _state, vel, newCov) 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(_state,newState,sizeof(_state));
memcpy(_cov,newCov,sizeof(_cov)); memcpy(_cov,newCov,sizeof(_cov));
} }
// Returns normalized innovation squared
float PosVelEKF::getPosNIS(float pos, float posVar) 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) const float NIS = (innovation_residual*innovation_residual)/(innovation_covariance);
return NIS;
return ret;
} }

34
libraries/AC_PrecLand/PosVelEKF.h
View File

@ -1,18 +1,46 @@
#pragma once #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 { class PosVelEKF {
public: 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 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]; } float getPos() const { return _state[0]; }
// Get the EKF state velocity
float getVel() const { return _state[1]; } float getVel() const { return _state[1]; }
// get the normalized innovation squared
float getPosNIS(float pos, float posVar); float getPosNIS(float pos, float posVar);
private: private:
// stored covariance and state matrix
/*
_state[0] = position
_state[1] = velocity
*/
float _state[2]; 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]; float _cov[3];
}; };