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

最初的卡尔曼滤波用于解决离散系统的滤波问题，然而工程中常遇到的滤波问题是连续系统产生的滤波问题。
卡尔曼滤波器具有如下形式的离散的状态方程：

Xk+1=Ψk+1,kXk+Wk(1)

但这只是一个高度简化的方程，更多的时候我们能获得的是关于连续系统的如下形式的方程：

X˙=f(X)+W(2)

其中f通常是一个非线性方程。
针对上述的非线性方程，扩展卡尔曼滤波的做法如下：
首先，状态一定存在一个轨迹X(t)，尽管根据式(2)我们很难具体地解出这个轨迹。假设有一个t0时刻的状态估计值X^，在状态的估计值X^附近把X(t)展开得到

X(t)=X^+∂X∂t∣X=X^(t−t0)+⋯(3)

而

∂X∂t∣X=X^=f(X^)(4)

因此滤波器的时间更新为

Xk+1=Xk+Tf(Xk)(5)

Pk+1=Ψk+1,kPkΨTk+1,k+Qk

而状态转移矩阵Ψk+1,k为

Ψk+1,k=I+T∂X˙∂X∣X=X^(6)

测量方程随可能同样为非线性

Z=h(X)+V

但它可以直接线性化

Zk=h(X^)+∂Z∂X∣X=X^(Xk−X^)+⋯

于是H阵为H=∂Z∂X∣X=X^

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

---

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

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

mat是矩阵类，第一个参数time是时刻，第二个参数X是状态，第三个参数para是方程中需要用到的参数。
以四元数微分方程为例：

Q˙=12[ω×]Q

则X=Q，para=ω，函数返回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_measure_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;
    }
}