擴展卡爾曼濾波的狀態方程和觀測方程可以是非線性的。在一般情況下，無法確定過程噪聲、測量噪聲與方程的函數關系，因此可以簡化為加性噪聲：
EKF relies on a linearisation of the evolution and observation functions which are good approximations of the original functions if these functions are close to linear. The state-space formulation of EKF reads :
Non-linear evolution and observation functions are handled within EKF by linearising these functions around some estimates of the state; for example for the evolution function is linearized around the previous estimate of the state $\hat{x}_k$:
$$f\left(x_k\right)\approx f\left(\hat{x}_k\right)+\frac{\partial f}{\text{dx}}\left(\hat{x}_k\right)\left(x_k-\hat{x}_k\right)$$
The first step in applying EKF is to linearize the evolution function around the previous estimate of the state $\hat{x}_{k-1}$
擴展卡爾曼濾波流程如下圖所示：
一個簡單的例子：假設一架飛機以恆定水平速度飛行（高度不變），地面上有一個雷達可以發射電磁波測量飛機到雷達的距離$r$。則有如下關系：
$$\theta=arctan(\frac{y}{x})$$
$$r^2=x^2+y^2$$
我們想知道某一時刻飛機的水平位置和垂直高度，以水平位置、水平速度、垂直高度作為狀態變量：
$$\mathbf{\textbf{x}}=\left(\begin{array}{c}\text{distance} \\\text{velocity} \\\text{altitude}\end{array}\right)=\left(\begin{array}{c}x \\\dot{x} \\y\end{array}\right)$$
則觀測值與狀態變量之間的關系為：$h\left(\hat{x}\right)=\sqrt{x^2+y^2}$，可以看出這是一個非線性的表達式。對於這個問題來說，觀測方程的雅克比矩陣為：$J_H=\left[\frac{\partial h}{\partial x}\quad\frac{\partial h}{\partial \dot{x}}\quad\frac{\partial h}{\partial y}\text{ }\right]$，即
$$J_H=\left[\frac{x}{\sqrt{x^2+y^2}} \quad 0 \quad \frac{y}{\sqrt{x^2+y^2}}\right]$$
狀態轉移方程的雅克比矩陣為：
得到上述矩陣后我們就可以設定初值和噪聲，然后根據流程圖中的步驟進行迭代計算。
- MRPT中的卡爾曼濾波器
卡爾曼濾波算法都集中在 mrpt::bayes::CKalmanFilterCapable這個虛類中。 這個類中包括系統狀態向量和系統協方差矩陣，以及根據選擇的算法執行一個完整迭代的通用方法。在解決一個特定問題時需要從這個虛類派生一個新的類，並實現狀態轉移函數、觀測函數以及它們的雅克比矩陣(采用EKF時)。內部的mrpt::bayes::CKalmanFilterCapable::runOneKalmanIteration()函數會依次調用用戶改寫的虛函數，每調用一次該函數執行一步預測+校正操作（runOneKalmanIteration():The main entry point, executes one complete step: prediction + update）
使用MRPT解決上述問題的C++代碼如下：
#include <mrpt/bayes/CKalmanFilterCapable.h> #include <mrpt/random.h> #include <mrpt/system/os.h> #include <mrpt/system/threads.h> #include <iostream> 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 (EKF,IEKF,UKF) implementations. template<size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE> 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: 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_u,KFArray_VEH &inout_x,bool &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 prediction step and the update step * This method will be called just once for each complete KF iteration. * \note It is assumed that the observations are independent, i.e. there are NO cross-covariances between them. */ void OnGetObservationsAndDataAssociation( vector_KFArray_OBS &out_z, mrpt::vector_int &out_data_association, const vector_KFArray_OBS &in_all_predictions, const KFMatrix &in_S, const vector_size_t &in_lm_indices_in_S, const KFMatrix_OxO &in_R ); // Implements the observation prediction void OnObservationModel(const vector_size_t &idx_landmarks_to_predict,vector_KFArray_OBS &out_predictions) const; // Implements the observation Jacobians void OnObservationJacobians(const size_t &idx_landmark_to_predict,KFMatrix_OxV &Hx,KFMatrix_OxF &Hy) const; }; CRange::CRange() { KF_options.method = kfEKFNaive; // 狀態變量初始值 State: (x,vx,y) m_xkk.resize(3); //對於動態矩陣可以通過resize()函數來動態修改矩陣的大小 m_xkk[0]= VEHICLE_INITIAL_X; m_xkk[1]= VEHICLE_INITIAL_V; m_xkk[2]= VEHICLE_INITIAL_Y; // Initial cov: Large uncertainty m_pkk.setSize(3,3); m_pkk.unit(); m_pkk = 50 * m_pkk; } CRange::~CRange() { } void CRange::Process( double DeltaTime, double observationRange) { m_deltaTime = (float)DeltaTime; m_obsRange = (float)observationRange; runOneKalmanIteration(); // executes one complete step: prediction + update } // Must return the action vector u. // param out_u: The action vector which will be passed to OnTransitionModel void CRange::OnGetAction( KFArray_ACT &out_u ) const { } /** Implements the transition model(Project the state ahead) param in_u : The vector returned by OnGetAction. param inout_x: prediction value param out_skip: Set this to true if for some reason you want to skip the prediction step. Default:false */ void CRange::OnTransitionModel(const KFArray_ACT &in_u, KFArray_VEH &inout_x, bool &out_skipPrediction) const { // The constant-velocities model is implemented simply as: inout_x[0] += m_deltaTime * inout_x[1]; inout_x[1] = inout_x[1]; inout_x[2] = inout_x[2]; } /** Implements the transition Jacobian param out_F Must return the Jacobian. The returned matrix must be N*N with N being the size of the whole state vector. */ void CRange::OnTransitionJacobian(KFMatrix_VxV &F) const { F.unit(); F(0,1) = m_deltaTime; } /** Implements the transition noise covariance param out_Q Must return the covariance matrix. The returned matrix must be of the same size than the jacobian from OnTransitionJacobian */ void CRange::OnTransitionNoise(KFMatrix_VxV &Q) const { Q.unit(); Q *= square(TRANSITION_MODEL_STD); } /** Return the observation NOISE covariance matrix, that is, the model of the Gaussian additive noise of the sensor. param out_R : The noise covariance matrix. It might be non diagonal, but it'll usually be. */ void CRange::OnGetObservationNoise(KFMatrix_OxO &R) const { R.unit(); R *= square(RANGE_SENSOR_NOISE_STD); } // This is called between the KF prediction step and the update step void CRange::OnGetObservationsAndDataAssociation( vector_KFArray_OBS &out_z, mrpt::vector_int &out_data_association, const vector_KFArray_OBS &in_all_predictions, const KFMatrix &in_S, const vector_size_t &in_lm_indices_in_S, const KFMatrix_OxO &in_R ) { //out_z: N vectors, N being the number of "observations" out_z.resize(1); out_z[0][0] = m_obsRange; } /** Implements the observation prediction param idx_landmark_to_predict: The indices of the landmarks in the map whose predictions are expected as output. For non SLAM-like problems, this input value is undefined and the application should just generate one observation for the given problem. param out_predictions: The predicted observations. */ void CRange::OnObservationModel(const vector_size_t &idx_landmarks_to_predict,vector_KFArray_OBS &out_predictions) const { // idx_landmarks_to_predict is ignored in NON-SLAM problems out_predictions.resize(1); out_predictions[0][0] = sqrt( square(m_xkk[0]) + square(m_xkk[2]) ); } // Implements the observation Jacobians void CRange::OnObservationJacobians(const size_t &idx_landmark_to_predict,KFMatrix_OxV &Hx,KFMatrix_OxF &Hy) const { Hx.zeros(); Hx(0,0) = m_xkk[0] / sqrt(square(m_xkk[0])+square(m_xkk[2])); Hx(0,2) = m_xkk[2] / sqrt(square(m_xkk[0])+square(m_xkk[2])); } int main () { // Create class instance CRange EKF; EKF.KF_options.method = kfEKFNaive; //select the KF algorithm // Initiate simulation float x=0, y=1000, v=100; //狀態變量真實值 float t=0; while (!mrpt::system::os::kbhit()) { // Simulate noisy observation: x += v * DELTA_TIME; float realRange = sqrt(square(x)+square(y)); // double mrpt::random::CRandomGenerator::drawGaussian1D_normalized(double * likelihood = NULL) // Generate a normalized (mean=0, std=1) normally distributed sample float obsRange = max(0.0, realRange + RANGE_SENSOR_NOISE_STD * randomGenerator.drawGaussian1D_normalized() ); printf("Real/Simulated range: %.03f / %.03f \n", realRange, obsRange ); // Process with EKF EKF.Process(DELTA_TIME, obsRange); // Show EKF state: CRange::KFVector EKF_xkk; CRange::KFMatrix EKF_pkk; EKF.getState( EKF_xkk, EKF_pkk ); printf("Real state: x:%.03f v=%.03f y=%.03f \n",x,v,y); cout << "EKF estimation:" <<endl<< EKF_xkk << endl; cout <<"-------------------------------------------"<<endl; // Delay(An OS-independent method for sending the current thread to "sleep" for a given period of time) mrpt::system::sleep((int)(DELTA_TIME*1000)); t += DELTA_TIME; } return 0; }
運行一段時間后結果如下圖所示，可以看出狀態變量基本收斂到真實值（由於傳感器和模型噪聲不可消除，因此只能是對真實狀態的最優估計）。
參考：
Eigen: C++開源矩陣計算工具——Eigen的簡單用法
KFilter - Free C++ Extended Kalman Filter Library
How to Use this Extended Kalman Filter Library?
http://www.mrpt.org/Kalman_Filters
http://reference.mrpt.org/devel/classmrpt_1_1bayes_1_1_c_kalman_filter_capable.html
https://github.com/MRPT/mrpt/blob/master/samples/bayesianTracking/test.cpp