卡尔曼滤波公式及推导

1、简介

\qquad 卡尔曼滤波（Kalman filtering）是一种线性系统状态最优估计算法。

2、离散卡尔曼滤波器

状态方程：
$$x_k=A{x}_{k-1}+B{u}_{k-1}+w_{k-1}$$
观测方程：
$$z_k=H{x}_k+v_k$$
过程激励噪声和观测噪声为相互独立，正态分布的白噪声：
$$p(w)\sim N(0,Q)$$
$$p(v)\sim N(0,R)$$
卡尔曼滤波器用反馈控制的方法估计过程状态：滤波器估计过程某一时刻的状态，然后以（含噪声的）测量变量的方式获得反馈。因此卡尔曼滤波器可分为两个部分：时间更新方程和测量更新方程。

预测方程：
$${\hat{x}}_k^{-}=A{\hat{x}}_{k-1}+B{u}_{k-1}$$
$${\hat{P}}_k^{-}=A{\hat{P}}_{k-1}{A}^T+Q$$
更新方程：
$$K_k={\hat{P}}_k^{-}{H}^T(H{\hat{P}}_k^{-}{H}^T+R{)}^{-1}$$
$${\hat{x}}_k={\hat{x}}_k^{-}+K_k(z_k-H{\hat{x}}_k^{-})$$
$${\hat{P}}_k=(I-K_{k}H){\hat{P}}_k^{-}$$

3、推导

这里设定的最优准则是使误差方差最小，即$P_k$的迹最小。依据这一准则，下面来推导卡尔曼增益$K$的表达式。
首先
$$P_k=E((e_k)(e_k{)}^T)$$
其中，$e_k=x_k-{\hat{x}}_k=x_k-{\hat{x}}_k^{-}-K_k(z_k-H{\hat{x}}_k^{-})$,将$z_k=H{x}_k+v_k$代入上式，
$$P_k=E((I-K_{k}H){\hat{e}}_k^{-}-K_k{v}_k)(I-K_{k}H){\hat{e}}_k^{-}-K_k{v}_k{)}^T)$$
其中，${\hat{e}}_k^{-}=x_k-{\hat{x}}_k^{-}$
由于$v_k$和${\hat{e}}_k^{-}$相互独立，上式可以展开为：
$$P_k={\hat{P}}_k^{-}-2K_{k}H{\hat{P}}_k^{-}+K_{k}H{\hat{P}}_k^{-}{H}^T{K}_k^T+K_{k}R{K}_k^T$$
其中，${\hat{P}}_k^{-}=E({\hat{e}}_k^{-}({\hat{e}}_k^{-}{)}^T)$
所以
$$tr(P_k)=tr({\hat{P}}_k^{-})-2tr(K_{k}H{\hat{P}}_k^{-})+tr(K_{k}H{\hat{P}}_k^{-}{H}^T{K}_k^T)+tr(K_{k}R{K}_k^T)$$
上式对$K_k$求导，可得:
$$(K_k{)}_{opt}={\hat{P}}_k^{-}{H}^T(H{\hat{P}}_k^{-}{H}^T+R{)}^{-1}$$
其中，使用到的两个迹的求导公式：$d(tr(AB))/d(A)=B^T$,$d(tr(AB{A}^T))/d(A)=2AB$。

参考：https://www.bilibili.com/video/BV1hC4y1b7K7
一个简单的ekf例子：

#include<vector>
#include<Eigen/Core>
#include<stdlib.h>
#include<cmath>
#include<iostream>

using namespace std;
using namespace Eigen;

template<class T>
auto f = [](T s, T e, T dt)
{	
    vector<T> r;
    T b = s;
    for(;b<=e;b+=dt)
    {		
	r.push_back(b);
    }
    return r;
};


auto hx = [](Vector3d x)
{
    return sqrt(x(0)*x(0)+x(2)*x(2));
};

auto Hjacob(Vector3d xp){
    Vector3d H = Vector3d::Zero();
    H(0) = xp(0)/sqrt(xp(0)*xp(0)+xp(2)*xp(2));
    H(1) = 0;
    H(2) = xp(2)/sqrt(xp(0)*xp(0)+xp(2)*xp(2));
    return H;
};

class EKF
{
public:
    EKF(){}
    ~EKF(){}
public:
int posp = 0;
Matrix3d A,Q,P;
Vector3d x;
double R;
bool initialized = false;

double GetRadar(double dt)
{
    double vel = 100 + 5*rand()/double(RAND_MAX);
    double alt = 1000 + 10*rand()/double(RAND_MAX);
    double pos = posp + vel*dt;
    posp = pos;
    double v = 0 + pos*0.05*rand()/double(RAND_MAX);
    return sqrt(pos*pos + alt*alt)+v;
}

Vector3d RadarEKF(double z, double dt)
{

    if(!initialized)
    {
	Matrix3d m3d;
	m3d<<0,1,0,0,0,0,0,0,0;
	A = Matrix3d::Identity() + dt*m3d;
	Q << 0,0,0,0,0.001,0,0,0,0.001;
	R = 10;
	x<< 0,90,1100;
	P = 10*Matrix3d::Identity();
	initialized = true;
    }
    Vector3d H = Hjacob(x);
    Vector3d xp = A*x;
    Matrix3d Pp = A*P*A.transpose()+Q;
    Vector3d K = Pp*H/(H.transpose()*Pp*H+R);
    x = xp + K*(z-hx(xp));
    P = Pp - K*H.transpose()*Pp;

    return x;
}

};


int main()
{
    double dt = 0.05;
    double s = 0.0, e = 20.0;
    vector<double> t = f<double>(s,e,dt);
    int Nsamples = t.size();
    vector<Vector3d> Xsaved(Nsamples,Vector3d({0,0,0}));
    vector<double> Zsaved(Nsamples,0);
    EKF ekf;
    for(auto k=0;k<Nsamples;k++)
    {
	double r = ekf.GetRadar(dt);
	Vector3d pva = ekf.RadarEKF(r,dt);
	std::cout<<pva<<std::endl;
	Xsaved[k] = pva;
	Zsaved[k] = r;
    }
    
    return 0;	
}