ekf对飞机位置进行滤波

C++代码实现对飞机位置的滤波，以水平位置、水平速度、垂直高度作为状态空间。具体的参考我的博客：https://blog.csdn.net/O_MMMM_O/article/details/106078679

#include 
#include 
#include 
#include 

#include 

using namespace mrpt;
using namespace mrpt::bayes;
using namespace mrpt::math;
using namespace mrpt::utils;
using namespace mrpt::random;
using namespace std;


#define DELTA_TIME                    0.05f    // Time Step between Filter Steps

// 系统状态变量初始值（猜测值）
#define VEHICLE_INITIAL_X            10.0f
#define VEHICLE_INITIAL_Y            2000.0f
#define VEHICLE_INITIAL_V           200.0f

#define TRANSITION_MODEL_STD         1.0f    // 模型噪声
#define RANGE_SENSOR_NOISE_STD         5.0f    // 传感器噪声


/* --------------------------------------------------------------------------------------------
        Virtual base for Kalman Filter （EKFIEKFUKF） implementations.
template
class mrpt::bayes::CKalmanFilterCapable< VEH_SIZE OBS_SIZE FEAT_SIZE ACT_SIZE KFTYPE >

The meaning of the template parameters is:
VEH_SIZE: The dimension of the “vehicle state“（系统状态变量数目）
OBS_SIZE: The dimension of each observation （eg 2 for pixel coordinates 3 for 3D coordinatesetc）.（观测量维数）
FEAT_SIZE: The dimension of the features in the system state （the “map“） or 0 if not applicable （the default if not implemented）.
ACT_SIZE: The dimension of each “action“ u_k （or 0 if not applicable）.（控制量的维数）
KFTYPE: The numeric type of the matrices （default: double）

This base class stores the state vector and covariance matrix of the system. It has virtual methods 
that must be completed by derived classes to address a given filtering problem.
 ---------------------------------------------------------------------------------------------- */
// Implementation of the system models as a EKF
class CRange: public CKalmanFilterCapable<3 1 0 0>
{
public:
    CRange（ ）;
    virtual ~CRange（）;

    void  Process（ double DeltaTime double observationRange）;

    void getState（ KFVector &xkk KFMatrix &pkk）
    { 
        xkk = m_xkk;  //The system state vector.
        pkk = m_pkk;  //The system full covariance matrix
    }


protected:
    float m_obsRange;    // 观测值
    float m_deltaTime;    // Time Step between Filter Steps

    // return the action vector u
    void OnGetAction（ KFArray_ACT &out_u ） const;  

    // Implements the transition model 
    void OnTransitionModel（const KFArray_ACT &in_uKFArray_VEH &inout_xbool &out_skipPrediction） const;

    // Implements the transition Jacobian
    void OnTransitionJacobian（KFMatrix_VxV  &out_F ） const;

    // Implements the transition noise covariance
    void OnTransitionNoise（KFMatrix_VxV &out_Q ） const;

    // Return the observation NOISE covariance matrix that is the model of the Gaussian additive noise of the sensor.
    void OnGetObservationNoise（KFMatrix_OxO &out_R） const;

    /** This is called between the KF pred

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

    .......      9364  2020-05-12 16:52  ekf_飞机位置一维空间\main.cpp

     目录          0  2020-05-12 16:52  ekf_飞机位置一维空间

----------- ---------  ---------- -----  ----

                 9364                    2