mirror of https://github.com/ArduPilot/ardupilot
47 lines
1.5 KiB
C++
47 lines
1.5 KiB
C++
#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);
|
|
|
|
// 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];
|
|
};
|