C++下扩展卡尔曼类（EKF）的实现

最初的卡尔曼滤波用于解决离散系统的滤波问题，然而工程中常遇到的滤波问题是连续系统产生的滤波问题。
卡尔曼滤波器具有如下形式的离散的状态方程：
$$X_{k+1}={\Psi }_{k+1,k}{X}_k+W_k\text{\hspace{1em}(1)}$$
但这只是一个高度简化的方程，更多的时候我们能获得的是关于连续系统的如下形式的方程：
$$\dot{X}=f(X)+W\text{\hspace{1em}(2)}$$
其中$f$通常是一个非线性方程。
针对上述的非线性方程，扩展卡尔曼滤波的做法如下：
首先，状态一定存在一个轨迹$X(t)$，尽管根据式(2)我们很难具体地解出这个轨迹。假设有一个$t_0$时刻的状态估计值$\hat{X}$，在状态的估计值$\hat{X}$附近把$X(t)$展开得到
$$X(t)=\hat{X}+\frac{\partial X}{\partial t}{\mid }_{X=\hat{X}}(t-t_0)+\cdots \text{\hspace{1em}(3)}$$
而
$$\frac{\partial X}{\partial t}{\mid }_{X=\hat{X}}=f(\hat{X})\text{\hspace{1em}(4)}$$
因此滤波器的时间更新为
$$X_{k+1}=X_k+Tf(X_k)\text{\hspace{1em}(5)}$$
$$P_{k+1}={\Psi }_{k+1,k}{P}_k{\Psi }_{k+1,k}^T+Q_k$$
而状态转移矩阵${\Psi }_{k+1,k}$为
$${\Psi }_{k+1,k}=I+T\frac{\partial \dot{X}}{\partial X}{\mid }_{X=\hat{X}}\text{\hspace{1em}(6)}$$
测量方程随可能同样为非线性
$$Z=h(X)+V$$
但它可以直接线性化
$$Z_k=h(\hat{X})+\frac{\partial Z}{\partial X}{\mid }_{X=\hat{X}}(X_k-\hat{X})+\cdots$$
于是$H$阵为$H=\frac{\partial Z}{\partial X}{\mid }_{X=\hat{X}}$

接下来只剩下测量更新，按照卡尔曼滤波的那一套来执行就行了。
另外，上述在式（3）只保留了一阶项，若嫌精度低可保留二阶项，相应的，状态转移矩阵也要保留二阶项。
一般的，卡尔曼滤波的Q阵要比R阵更难准确获得，因此建议先确定R阵，之后经验性的给定Q阵，观察滤波效果以决定对Q阵的调整。

---

在C++下将上述过程编写为类。考虑通用性，对所有关于状态$X$的方程都抽象为如下形式的函数

mat equation(time_stamp& time,mat& X,mat& para);

mat是矩阵类，第一个参数time是时刻，第二个参数X是状态，第三个参数para是方程中需要用到的参数。
以四元数微分方程为例：
$$\dot{Q}=\frac{1}{2}[\omega \times ]Q$$
则$X=Q$，$para=\omega$，函数返回$\dot{Q}$

头文件kalman.h，其中矩阵类mat和时间戳类time_stamp请自行实现：

#ifndef KALMAN_H
#define KALMAN_H

//时间更新发生在测量完成之后，即测量完成时刻为当前滤波时刻
//因此时间更新指从上一个滤波时刻更新到当前滤波时刻

typedef mat (*pEquation_of_X)(time_stamp& time,mat& X,mat& para);

class EKF
{
public:
    EKF();
    ~EKF();

    //指向状态微分方程
    pEquation_of_X pState_differential_equation;
    
    //指向状态微分方程的雅可比矩阵的计算
    pEquation_of_X pJacob_of_state_DE;
    
    //指向测量方程的雅可比矩阵即H阵的计算
    pEquation_of_X pJacob_of_measure_equation;
    
    //指向测量方程
    pEquation_of_X pMeasure_equation;

    mat X;//状态
    mat P;//状态协方差
    mat Z;//测量量
    mat Q;//Q阵
    mat R;//R阵
    mat H;//H阵
    mat innovation;//新息
    mat gain;//增益阵

    mat para_of_time_update;//时间更新的参数矩阵
    mat para_of_measure_update;//测量更新的参数矩阵
    time_stamp time;//指示滤波器的时间戳

    void time_update(void);//时间更新
    void measure_update(void);//测量更新

};

#endif

cpp文件kalman.cpp：

#include <iostream>
#include "kalman.h"

EKF::EKF()
{
    pState_differential_equation=NULL;
    pJacob_of_state_DE=NULL;
    pJacob_of_measure_equation=NULL;
    pMeasure_equation=NULL;
}
EKF::~EKF()
{}

void EKF::time_update(void)
{
    if(pState_differential_equation!=NULL&&pJacob_of_state_DE!=NULL)
    {
        double detT=time.current_sec-time.last_sec;
        X=X+detT*pState_differential_equation(time,X,para_of_time_update);
        mat STM=detT*pJacob_of_state_DE(time,X,para_of_time_update);
        for(int i=0;i<STM.rows();i++)
        {
            STM[i][i]=STM[i][i]+1;
        }
        P=STM*P*STM.trans()+Q;
    }
    else
    {
        std::cout<<"state differential equation is NULL or Jacob matrix of SDE is NULL!"<<std::endl;
    }
}

void EKF::measure_update(void)
{
    if(pMeasure_equation!=NULL&&pJacob_of_measure_equation!=NULL)
    {
        innovation=Z-pMeasure_equation(time,X,para_of_measure_update);
        H=pJacob_of_measure_equation(time,X,para_of_measure_update);
        gain=P*H.trans()*inv(H*P*H.trans()+R);
        X=X+gain*innovation;
        P=(diag(X.rows(),1)-gain*H)*P;
    }
    else
    {
        std::cout<<"measure equation is NULL or jacob of ME is NULL!"<<std::endl;
    }
}