mirror of https://github.com/ArduPilot/ardupilot
AC_PrecLand: NFC: Refactor EKF code
This commit is contained in:
Rishabh
Randy Mackay
parent
4fde26aa01
commit
290174f9d9
|
|
@ -2,42 +2,8 @@
|
|||
#include <math.h>
|
||||
#include <string.h>
|
||||
|
||||
#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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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];
|
||||
};
|
||||
|
|
|
|||
Loading…
Reference in New Issue