扩展卡尔曼滤波python_扩展卡尔曼滤波（MRPT）

#include #include#include#include#include

using namespacemrpt;using namespacemrpt::bayes;using namespacemrpt::math;using namespacemrpt::utils;using namespacemrpt::random;using namespacestd;#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 (EKF,IEKF,UKF) 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 coordinates,etc).(观测量维数)

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: