1. 概述

1.1 开篇

对于卡尔曼滤波的直观理解便是对所掌握数据的融合处理，假设手头有两个随机变量，知道其均值和方差，而且均值相同，那么我希望找到一个权重，使得把这两个随机变量组合起来，均值不变，方差减小，从线条上看当然变得平稳了一些，于是被搞工程的人成为滤波。

问题是权重如何找呢？简化一下问题，把这两个随机变量都视作服从正态分布的随机变量，这样通过计算可知，权重与它们的方差有关。

下一个问题是，往往手头只有一个滤波器，怎么办？可以给系统建模，根据历史信息计算出一个额外的数值来，为了简化问题，认为系统是线性的，那么输入数学模型给出的数值任然服从某个正态分布，均值和方差都可以从模型中导出。然后就可以开开心心地滤波了。

齐次要理解粒子滤波。接上，然而问题接踵而至，系统的模型往往都不是线性的啊！输入个正态分布的量，有的连解析式都写不出来，根本不知道输出的是个啥啊！于是想了个办法，我们不用均值和方差来表示随机变量，我们用数量居多的采样点来表示！！！把每个采样带进模型去计算，统统算完就得到了下一次的随机变量的采样点，当需要计算权重的时候再把方差给算出来。在计算力十分十分强大的仿真平台上，这方法还能说得过去。因为一般要用到的采样点很多，所以也被称作粒子云滤波。

现在可以理解无迹卡尔曼滤波了，在一般情况下，应用粒子滤波困难重重，因为那些粒子云经过多次迭代，很容易发生聚集，这样就无法精准的描述随机变量了，所以一般要择机向粒子云里稀疏的地方添加粒子，在太过稠密的地方减去粒子，不过谁能保证这么操作不会带来均值与方差的偏差？

此外为了达到精度要求，粒子的数量需要很大，这很难算的过来啊。此时相信你已经体会到了，如何更准更快的计算某个随机变量经过某个函数变成了怎样的随机变量才是卡尔曼滤波器的关键所在。无迹变换是尝试解决这个问题的一个方案，通过某种规则去采样，在粒子数量尽量小的情况下，去保证采样点云的均值和方差不变。不过我没做过无迹卡尔曼的仿真，对ut的理解不深入。

ref：无迹卡尔曼到底是什么东西？

1.2 问题定义

在时序运动系统中为了得到当前时刻自身的状态信息，一方面需要根据以往数据对当前时刻进行估计，同时需要使用传感器等测量工具对自身状态进行测量，进而调整估计模型使得估计更加准确。
xˉt=x^t+Kt(x˙t−H⋅x^t)\bar{x}_t=\hat{x}_t+\mathcal{K}_t(\dot{x}_t-\mathcal{H}\cdot\hat{x}_t)xˉt=x^t+Kt(x˙t−H⋅x^t)
Kt∈[0,1.0]\mathcal{K}_t\in[0,1.0]Kt∈[0,1.0]为卡尔曼增益，H\mathcal{H}H为测量转换矩阵。符号统一定义：

1）真实无偏差状态状态信息：xtx_txt
2）根据以往数据得到的当前时刻预测结果：x^t\hat{x}_tx^t
3）当前利用传感器等测量手段得到的测量结果：x˙t\dot{x}_tx˙t
4）根据现有预测结果和传感器测量结果得到的最后估计结果：xˉt\bar{x}_txˉt

2. KF滤波器

2.1 KF下模型建立与推导

KF滤波器是线型滤波器，其理想状态转移方程（也可称为运动方程）可以描述为：
xt=F⋅xt−1+B⋅ut+ωtx_t=\mathcal{F}\cdot x_{t-1}+\mathcal{B}\cdot u_t+\omega_txt=F⋅xt−1+B⋅ut+ωt
其中，F\mathcal{F}F代表状态转移矩阵，B\mathcal{B}B代表控制矩阵，utu_tut代表控制信号（如加速度、转向角度等变量），ωt\omega_tωt代表系统噪声扰动，服从高斯分布。而上述的过程是理想情况下对线型系统的建模，而实际中对于实际运动预测描述为：
x^t=F⋅xˉt−1+B⋅ut\hat{x}_t=\mathcal{F}\cdot \bar{x}_{t-1}+\mathcal{B}\cdot u_tx^t=F⋅xˉt−1+B⋅ut
注意，上面的式子中运动状态为上一时刻的估计结果xˉt−1\bar{x}_{t-1}xˉt−1，而不是上一时刻的预测结果x^t−1\hat{x}_{t-1}x^t−1。在上述的过程中可以看到运动估计结果与真实无偏结果是存在偏差的，这里使用协方差去度量它们之间的差异：
P^t=E(eˉeˉT)=E((xt−x^t)(xt−x^t)T)=E((F⋅xt−1+B⋅ut+ωt−F⋅xˉt−1−B⋅ut)(F⋅xt−1+B⋅ut+ωt−F⋅xˉt−1−B⋅ut)T)=E((F(xt−1−xˉt−1)+ωt)(F(xt−1−xˉt−1)+ωt)T)=E(F(xt−1−xˉt−1)(xt−1−xˉt−1)TFT+ωtωtT+F(xt−1−xˉt−1)ωtT+ωt(xt−1−xˉt−1)TFT)=FE((xt−1−xˉt−1)(xt−1−xˉt−1)T)FT+E(ωtωtT)=FE((xt−1−xˉt−1)(xt−1−xˉt−1)T)FT+Qt=FPˉt−1FT+Qt\begin{align} \hat{P}_t&=E(\bar{e}\bar{e}^T)=E((x_t-\hat{x}_t)(x_t-\hat{x}_t)^T) \\ & = E((\mathcal{F}\cdot x_{t-1}+\mathcal{B}\cdot u_t+\omega_t-\mathcal{F}\cdot \bar{x}_{t-1}-\mathcal{B}\cdot u_t)(\mathcal{F}\cdot x_{t-1}+\mathcal{B}\cdot u_t+\omega_t-\mathcal{F}\cdot \bar{x}_{t-1}-\mathcal{B}\cdot u_t)^T) \\ & = E((\mathcal{F}(x_{t-1}-\bar{x}_{t-1})+\omega_t)(\mathcal{F}(x_{t-1}-\bar{x}_{t-1})+\omega_t)^T) \\ & = E(\mathcal{F}(x_{t-1}-\bar{x}_{t-1})(x_{t-1}-\bar{x}_{t-1})^T\mathcal{F}^T+\omega_t\omega_t^T+\mathcal{F}(x_{t-1}-\bar{x}_{t-1})\omega_t^T+\omega_t(x_{t-1}-\bar{x}_{t-1})^T\mathcal{F}^T) \\ & = \mathcal{F}E((x_{t-1}-\bar{x}_{t-1})(x_{t-1}-\bar{x}_{t-1})^T)\mathcal{F}^T+E(\omega_t\omega_t^T) \\ & = \mathcal{F}E((x_{t-1}-\bar{x}_{t-1})(x_{t-1}-\bar{x}_{t-1})^T)\mathcal{F}^T+Q_t \\ & = \mathcal{F}\bar{P}_{t-1}\mathcal{F}^T+Q_t \end{align}P^t=E(eˉeˉT)=E((xt−x^t)(xt−x^t)T)=E((F⋅xt−1+B⋅ut+ωt−F⋅xˉt−1−B⋅ut)(F⋅xt−1+B⋅ut+ωt−F⋅xˉt−1−B⋅ut)T)=E((F(xt−1−xˉt−1)+ωt)(F(xt−1−xˉt−1)+ωt)T)=E(F(xt−1−xˉt−1)(xt−1−xˉt−1)TFT+ωtωtT+F(xt−1−xˉt−1)ωtT+ωt(xt−1−xˉt−1)TFT)=FE((xt−1−xˉt−1)(xt−1−xˉt−1)T)FT+E(ωtωtT)=FE((xt−1−xˉt−1)(xt−1−xˉt−1)T)FT+Qt=FPˉt−1FT+Qt
上面式子中由于状态变量和误差变量是无关，所以E(xtωt)E(x_t\omega_t)E(xtωt)相关的期望为0，E(ωtωtT)=QtE(\omega_t\omega_t^T)=Q_tE(ωtωtT)=Qt为噪声协方差。

另外测量方程（也就是测量结果）描述为：
x˙t=H⋅xt+vt\dot{x}_t=\mathcal{H}\cdot x_t+v_tx˙t=H⋅xt+vt
其中，H\mathcal{H}H代表测量转换矩阵（运动状态空间转换到测量空间），vtv_tvt代表测量噪声，服从高斯分布。对应的在当前估计结果基础上得到的测量：
x˙t′=H⋅x^t\dot{x}_t^{'}=\mathcal{H}\cdot \hat{x}_tx˙t′=H⋅x^t
这里也通过协方差的形式度量真实测量值和以估计为基准得到测量值的偏差，并套用上面关于状态变量协方差的推导形式，可以得到：
P˙t=E((x˙t−x˙t′)(x˙t−x˙t′)T)=HP^tHT+Rt\begin{align} \dot{P}_t & = E((\dot{x}_t-\dot{x}_t^{'})(\dot{x}_t-\dot{x}_t^{'})^T) \\ & = \mathcal{H}\hat{P}_t\mathcal{H}^T+R_t \end{align}P˙t=E((x˙t−x˙t′)(x˙t−x˙t′)T)=HP^tHT+Rt
在卡尔曼滤波中最后估计值的描述为，其中Kt\mathcal{K}_tKt为未知且需要估计的量：
xˉt=x^t+Kt(x˙t−x^t′)=x^t+Kt(Hxt+vt−Hx^t)=x^t+KtHxt+Ktvt−KtHx^t\begin{align} \bar{x}_t&=\hat{x}_t+\mathcal{K}_t(\dot{x}_t-\hat{x}_t^{'}) \\ & = \hat{x}_t+\mathcal{K}_t(\mathcal{H}x_t+v_t-\mathcal{H}\hat{x}_t) \\ & = \hat{x}_t+\mathcal{K}_t\mathcal{H}x_t+\mathcal{K}_tv_t-\mathcal{K}_t\mathcal{H}\hat{x}_t\\ \end{align}xˉt=x^t+Kt(x˙t−x^t′)=x^t+Kt(Hxt+vt−Hx^t)=x^t+KtHxt+Ktvt−KtHx^t
那么对应的估计值与真实值的偏差为：
xˉt=x^t+KtHxt+Ktvt−KtHx^txˉt=x^t+KtH(xt−xˉt)+Ktvtxˉt−xt=(x^t−xt)+KtH(xt−x^t)+Ktvteˉt=e^t−KtHe^t+Ktvteˉt=(1−KtH)e^t+Ktvt\begin{align} \bar{x}_t&= \hat{x}_t+\mathcal{K}_t\mathcal{H}x_t+\mathcal{K}_tv_t-\mathcal{K}_t\mathcal{H}\hat{x}_t\\ \bar{x}_t&= \hat{x}_t+\mathcal{K}_t\mathcal{H}(x_t-\bar{x}_t)+\mathcal{K}_tv_t\\ \bar{x}_t-x_t&= (\hat{x}_t-x_t)+\mathcal{K}_t\mathcal{H}(x_t-\hat{x}_t)+\mathcal{K}_tv_t\\ \bar{e}_t&= \hat{e}_t-\mathcal{K}_t\mathcal{H}\hat{e}_t+\mathcal{K}_tv_t\\ \bar{e}_t&= (1-\mathcal{K}_t\mathcal{H})\hat{e}_t+\mathcal{K}_tv_t\\ \end{align}xˉtxˉtxˉt−xteˉteˉt=x^t+KtHxt+Ktvt−KtHx^t=x^t+KtH(xt−xˉt)+Ktvt=(x^t−xt)+KtH(xt−x^t)+Ktvt=e^t−KtHe^t+Ktvt=(1−KtH)e^t+Ktvt
对应的误差用协方差进行描述得到：
Pˉt=E(eˉteˉtT)=E(((1−KtH)e^t+Ktvt)((1−KtH)e^t+Ktvt)T)=E((1−KtH)e^te^tT(1−KtH)T+KtvtvtTKtT)=(1−KtH)E(e^te^tT)(1−KtH)T+KtRtKtT=(1−KtH)P^t(1−KtH)T+KtRtKtT\begin{align} \bar{P}_t&=E(\bar{e}_t\bar{e}_t^T) \\ & = E(((1-\mathcal{K}_t\mathcal{H})\hat{e}_t+\mathcal{K}_tv_t)((1-\mathcal{K}_t\mathcal{H})\hat{e}_t+\mathcal{K}_tv_t)^T) \\ & = E((1-\mathcal{K}_t\mathcal{H})\hat{e}_t\hat{e}_t^T(1-\mathcal{K}_t\mathcal{H})^T+\mathcal{K}_tv_tv_t^T\mathcal{K}_t^T)\\ & = (1-\mathcal{K}_t\mathcal{H})E(\hat{e}_t\hat{e}_t^T)(1-\mathcal{K}_t\mathcal{H})^T+\mathcal{K}_tR_t\mathcal{K}_t^T\\ & = (1-\mathcal{K}_t\mathcal{H})\hat{P}_t(1-\mathcal{K}_t\mathcal{H})^T+\mathcal{K}_tR_t\mathcal{K}_t^T\\ \end{align}Pˉt=E(eˉteˉtT)=E(((1−KtH)e^t+Ktvt)((1−KtH)e^t+Ktvt)T)=E((1−KtH)e^te^tT(1−KtH)T+KtvtvtTKtT)=(1−KtH)E(e^te^tT)(1−KtH)T+KtRtKtT=(1−KtH)P^t(1−KtH)T+KtRtKtT
为了使得卡尔曼滤波器的估计值与真实值的误差最小化，需要将上述协方差最小化则对上述协方差求去关于增益参数K\mathcal{K}K的导数：
∂Pˉt∂Kt=−2P^tHT+2Kt(HP^tHT+Rt)\frac{\partial\bar{P}_t}{\partial\mathcal{K}_t}=-2\hat{P}_t\mathcal{H}^T+2\mathcal{K}_t(\mathcal{H}\hat{P}_t\mathcal{H}^T+R_t)∂Kt∂Pˉt=−2P^tHT+2Kt(HP^tHT+Rt)
则上面的协方差在上述导数为0的时候达到最小化，也就解得最佳卡尔曼增益为：
Kt=P^tHT(HP^tHT+Rt)−1\mathcal{K}_t=\hat{P}_t\mathcal{H}^T(\mathcal{H}\hat{P}_t\mathcal{H}^T+R_t)^{-1}Kt=P^tHT(HP^tHT+Rt)−1
进而得到最小协方差结果为：
Pˉt=P^t−KtHP^t\bar{P}_t=\hat{P}_t-\mathcal{K}_t\mathcal{H}\hat{P}_tPˉt=P^t−KtHP^t

2.2 KF迭代过程

Step1：预测阶段
得到运动预测状态：
x^t=F⋅xˉt−1+B⋅ut\hat{x}_t=\mathcal{F}\cdot \bar{x}_{t-1}+\mathcal{B}\cdot u_tx^t=F⋅xˉt−1+B⋅ut
得到当前运动状态协方差：
P^t=FPˉt−1FT+Qt\hat{P}_t= \mathcal{F}\bar{P}_{t-1}\mathcal{F}^T+Q_tP^t=FPˉt−1FT+Qt

Step2：更新阶段
计算卡尔曼增益：
Kt=P^tHT(HP^tHT+Rt)−1\mathcal{K}_t=\hat{P}_t\mathcal{H}^T(\mathcal{H}\hat{P}_t\mathcal{H}^T+R_t)^{-1}Kt=P^tHT(HP^tHT+Rt)−1
得到卡尔曼最终估计结果：
xˉt=x^t+Kt(x˙t−Hx^t)\bar{x}_t=\hat{x}_t+\mathcal{K}_t(\dot{x}_t-\mathcal{H}\hat{x}_t)xˉt=x^t+Kt(x˙t−Hx^t)
更新卡尔曼最终估计结果协方差：
Pˉt=P^t−KtHP^t\bar{P}_t=\hat{P}_t-\mathcal{K}_t\mathcal{H}\hat{P}_tPˉt=P^t−KtHP^t

Step3：结果返回
xˉt,Pˉt\bar{x}_t,\bar{P}_txˉt,Pˉt

2.3 KF实现鼠标追踪的例子

#include <opencv2/opencv.hpp>
#include <iostream>cv::KalmanFilter kalmanfilter(2,2);
cv::Mat last_measurement(2,1,CV_32FC1);
cv::Mat current_measurement(2,1,CV_32FC1);cv::Mat last_prediction(2,1,CV_32FC1);
cv::Mat current_prediction(2,1,CV_32FC1);cv::Mat frame(800,800,CV_8UC3);void onMouseMove(int event,int x,int y,int flag,void* data)
{last_prediction = current_prediction;last_measurement = current_measurement;current_measurement.at<float>(0) = x;current_measurement.at<float>(1) = y;std::cout << current_measurement << std::endl;kalmanfilter.correct(current_measurement);current_prediction = kalmanfilter.predict();//kalmanfilter.predict();cv::line(frame,cv::Point(last_measurement.at<float>(0),last_measurement.at<float>(1)),cv::Point(current_measurement.at<float>(0),current_measurement.at<float>(1)),cv::Scalar(0,255,0),2);cv::line(frame,cv::Point(last_prediction.at<float>(0),last_prediction.at<float>(1)),cv::Point(current_prediction.at<float>(0),current_prediction.at<float>(1)),cv::Scalar(0,0,255),2);std::cout << "on mouse move:" << current_prediction <<std::endl;}int main(int argc,char** argv)
{//cv::Mat frame(800,800,CV_8UC3);cv::namedWindow("test");cv::setMouseCallback("test",onMouseMove,NULL);cv::Mat F(2,2,CV_32F,cv::Scalar(0));//m.at<float>(0,0) = 1;//m.at<float>(1,1) = 1;cv::setIdentity(F,cv::Scalar(1));cv::Mat H(2,2,CV_32F);cv::setIdentity(H,cv::Scalar(1));cv::Mat p(2,2,CV_32F);cv::setIdentity(p,cv::Scalar(0.1));kalmanfilter.measurementMatrix = H;kalmanfilter.transitionMatrix = F;kalmanfilter.processNoiseCov = p;while (true){cv::imshow("test",frame);if (cv::waitKey(30) == 113)break;}cv::destroyAllWindows();return 0;
}

PS：例子素材来源（侵删）：卡尔曼滤波器算法

FK存在的问题：

1）对运动方程和测量方程中使用的噪声是高斯噪声，可能与实际情况不符合。
2）针对线性系统有效，对于非线性系统会偏差变大甚至失效。

3. EKF滤波器

3.1 EKF下的问题描述与协方差推导

在KF滤波器中所用的运动方程和测量方程都是线性函数，但是在一些非线性情况下，如转弯、控制信号周期变换等，上述的KF滤波器中的线性函数就变成了非线性存在，直接套用KF就会出现较大偏差。也就是上面提到的运动方程和观测方程变为了如下非线性函数组合的形式，对于运动方程：
xt=f(xt−1,ut)+ωtx_t=f(x_{t-1},u_t)+\omega_txt=f(xt−1,ut)+ωt
EKF算法的思想便是对于非线性函数在某一点上进行泰勒展开，这样使用n阶多项式的形式逼近原来的函数，对于上述函数非线性函数f(xt−1,ut)f(x_{t-1},u_t)f(xt−1,ut)在上一次卡尔曼滤波结果估计结果xˉt−1\bar{x}_{t-1}xˉt−1处展开得到：
f(x,u)∣x=xˉt−1,u=ut=f(xˉt−1,ut)+∂f∂x∣x=xˉt−1,u=ut(x−xˉt−1)+∂f∂u∣x=xˉt−1,u=ut(u−ut)+O(x−xˉt−1)+O(u−ut)≈f(xˉt−1,ut)+∂f∂x∣x=xˉt−1,u=ut(x−xˉt−1)≈f(xˉt−1,ut)+Ft(x−xˉt−1)\begin{align} f(x,u)|_{x=\bar{x}_{t-1},u=u_t}&=f(\bar{x}_{t-1},u_t)+\frac{\partial f}{\partial x}|_{x=\bar{x}_{t-1},u=u_t}(x-\bar{x}_{t-1})+\frac{\partial f}{\partial u}|_{x=\bar{x}_{t-1},u=u_t}(u-u_t)+\mathcal{O}(x-\bar{x}_{t-1}) + \mathcal{O}(u-u_t)\\ & \approx f(\bar{x}_{t-1},u_t)+\frac{\partial f}{\partial x}|_{x=\bar{x}_{t-1},u=u_t}(x-\bar{x}_{t-1})\\ & \approx f(\bar{x}_{t-1},u_t)+F_t(x-\bar{x}_{t-1}) \end{align}f(x,u)∣x=xˉt−1,u=ut=f(xˉt−1,ut)+∂x∂f∣x=xˉt−1,u=ut(x−xˉt−1)+∂u∂f∣x=xˉt−1,u=ut(u−ut)+O(x−xˉt−1)+O(u−ut)≈f(xˉt−1,ut)+∂x∂f∣x=xˉt−1,u=ut(x−xˉt−1)≈f(xˉt−1,ut)+Ft(x−xˉt−1)
其中，Ft=∂f∂x∣x=xˉt−1,u=utF_t=\frac{\partial f}{\partial x}|_{x=\bar{x}_{t-1},u=u_t}Ft=∂x∂f∣x=xˉt−1,u=ut。则近似得到在上一次卡尔曼估计结果xˉt−1\bar{x}_{t-1}xˉt−1处的泰勒一阶逼近的运动方程：
xt≈f(xˉt−1,ut)+Ft(x−xˉt−1)+ωtx_t\approx f(\bar{x}_{t-1},u_t)+F_t(x-\bar{x}_{t-1})+\omega_txt≈f(xˉt−1,ut)+Ft(x−xˉt−1)+ωt
由之前的真值与预测值的协方差定义得到（其中x^t=f(xˉt−1,ut)\hat{x}_t=f(\bar{x}_{t-1},u_t)x^t=f(xˉt−1,ut)）：
P^t=E((xt−x^t)(xt−x^t)T)=E((Ft(x−xˉt−1)+ωt)(Ft(x−xˉt−1)+ωt)T)=FtPˉt−1FtT+Qt\begin{align} \hat{P}_t&=E((x_t-\hat{x}_t)(x_t-\hat{x}_t)^T)\\ &=E((F_t(x-\bar{x}_{t-1})+\omega_t)(F_t(x-\bar{x}_{t-1})+\omega_t)^T)\\ &=F_t\bar{P}_{t-1}F_t^T+Q_t \end{align}P^t=E((xt−x^t)(xt−x^t)T)=E((Ft(x−xˉt−1)+ωt)(Ft(x−xˉt−1)+ωt)T)=FtPˉt−1FtT+Qt

对应的非线性观测方程变为：
x˙t=g(xt)+vt\dot{x}_t=g(x_t)+v_tx˙t=g(xt)+vt
同样需要对g(xt)g(x_t)g(xt)进行泰勒展开，展开的位置是x^t\hat{x}_tx^t处（当前时刻预测结果）：
g(x)∣x=x^t=g(x^t)+∂g∂x∣x=x^t(x−x^t)+O(x−x^t)≈g(x^t)+∂g∂x∣x=x^t(x−x^t)≈g(x^t)+Gt(x−x^t)\begin{align} g(x)|_{x=\hat{x}_t}&=g(\hat{x}_t)+\frac{\partial g}{\partial x}|_{x=\hat{x}_t}(x-\hat{x}_t)+ \mathcal{O}(x-\hat{x}_t)\\ & \approx g(\hat{x}_t)+\frac{\partial g}{\partial x}|_{x=\hat{x}_t}(x-\hat{x}_t)\\ & \approx g(\hat{x}_t)+G_t(x-\hat{x}_t) \end{align}g(x)∣x=x^t=g(x^t)+∂x∂g∣x=x^t(x−x^t)+O(x−x^t)≈g(x^t)+∂x∂g∣x=x^t(x−x^t)≈g(x^t)+Gt(x−x^t)
其中，Gt=∂g∂x∣x=x^tG_t=\frac{\partial g}{\partial x}|_{x=\hat{x}_t}Gt=∂x∂g∣x=x^t。则观测方程在当前时刻预测结果x^t\hat{x}_tx^t处的泰勒一阶近似化结果为：
x˙t≈g(x^t)+Gt(x−x^t)+vt\dot{x}_t \approx g(\hat{x}_t)+G_t(x-\hat{x}_t)+v_tx˙t≈g(x^t)+Gt(x−x^t)+vt
同样按照上一节中对于测量误差计算协方差：
P˙t=GtP^tGtT+Rt\dot{P}_t=G_t\hat{P}_tG_t^T+R_tP˙t=GtP^tGtT+Rt

3.2 EKF迭代过程

接下来就是需要计算在泰勒展开近似的情况下卡尔曼滤波算法的更新过程了，具体的推导过程和线性卡尔曼滤波算法的原理近似，主要的变化是运动转换矩阵和观测矩阵变化了：F→Ft,H→Gt\mathcal{F}\rightarrow F_t,\mathcal{H}\rightarrow G_tF→Ft,H→Gt。这里就直接给出其更新迭代的计算公式：

Step1：预测阶段
得到运动预测状态：
x^t=f(xˉt−1,ut)\hat{x}_t=f(\bar{x}_{t-1},u_t)x^t=f(xˉt−1,ut)
得到当前运动状态协方差：
P^t=FtPˉt−1FtT+Qt\hat{P}_t= F_t\bar{P}_{t-1}F_t^T+Q_tP^t=FtPˉt−1FtT+Qt

Step2：更新阶段
计算卡尔曼增益：
Kt=P^tGtT(GtP^tGtT+Rt)−1\mathcal{K}_t=\hat{P}_tG_t^T(G_t\hat{P}_tG_t^T+R_t)^{-1}Kt=P^tGtT(GtP^tGtT+Rt)−1
得到卡尔曼最终估计结果：
xˉt=x^t+Kt(x˙t−g(x^t))\bar{x}_t=\hat{x}_t+\mathcal{K}_t(\dot{x}_t-g(\hat{x}_t))xˉt=x^t+Kt(x˙t−g(x^t))
更新卡尔曼最终估计结果协方差：
Pˉt=P^t−KtGtP^t\bar{P}_t=\hat{P}_t-\mathcal{K}_tG_t\hat{P}_tPˉt=P^t−KtGtP^t

Step3：结果返回
xˉt,Pˉt\bar{x}_t,\bar{P}_txˉt,Pˉt

EKF存在的问题：

1）EKF算法假设噪声形式为高斯噪声，但实际情况下噪声形式往往为有色噪声形式。
2）EKF算法的线性化工作点往往不是输入状态真实的均值（上文中也使用上一轮卡尔曼滤波的结果处展开），而是一个估计的均值，这样的偏差会导致计算出的雅可比矩阵也不是最好的。
3）EKF使用泰勒展开进行线性逼近，但是线性化过程丢弃了许多高阶项，这是为了计算效率而选择了一阶近似，这会导致EKF算法的性能下降，甚至导致滤波器发散。

3.3 EKF改进算法IEKF

对于上一小结中提到的EKF问题，迭代扩展卡尔曼滤波算法（IEKF）能有效的改善，其基本的思想与残差逼近类似。IEKF会将上一步修正后的输出作为下一次迭代修正误差的输入值，通过不断迭代来修正误差减小误差，具体的IEKF算法的步骤可表达如下：

Step1：预测阶段
得到运动预测状态：
x^t=f(xˉt−1,ut)\hat{x}_t=f(\bar{x}_{t-1},u_t)x^t=f(xˉt−1,ut)
得到当前运动状态协方差：
P^t=FtPˉt−1FtT+Qt\hat{P}_t= F_t\bar{P}_{t-1}F_t^T+Q_tP^t=FtPˉt−1FtT+Qt

Step2：迭代更新阶段
令：x^t0=x^t,P^t0=P^t,Gt0=Gt\hat{x}_t^0=\hat{x}_t,\hat{P}_t^0=\hat{P}_t,G_t^0=G_tx^t0=x^t,P^t0=P^t,Gt0=Gt，之后迭代更新计算：
for i=1 to Ndo:Kti=P^ti−1Gti−1T(Gti−1P^ti−1Gti−1T+Rt)−1xˉti=x^ti−1+Kt(x˙t−g(x^ti−1))Pˉti=P^ti−1−KtGti−1P^ti−1\begin{align} \text{for $i$}&=\text{1 to $N$ do}:\\ \mathcal{K}_t^i&=\hat{P}_t^{i-1}{G_t^{i-1}}^T(G_t^{i-1}\hat{P}_t^{i-1}{G_t^{i-1}}^T+R_t)^{-1}\\ \bar{x}_t^i&=\hat{x}_t^{i-1}+\mathcal{K}_t(\dot{x}_t-g(\hat{x}_t^{i-1}))\\ \bar{P}_t^i&=\hat{P}_t^{i-1}-\mathcal{K}_tG_t^{i-1}\hat{P}_t^{i-1} \end{align}for iKtixˉtiPˉti=1 to N do:=P^ti−1Gti−1T(Gti−1P^ti−1Gti−1T+Rt)−1=x^ti−1+Kt(x˙t−g(x^ti−1))=P^ti−1−KtGti−1P^ti−1

Step3：结果返回
xˉti,Pˉti\bar{x}_t^i,\bar{P}_t^ixˉti,Pˉti

ref：迭代扩展卡尔曼滤波算法-IEKF

4. UKF

4.1 UKF下的问题描述

在UKF中不在某点处展开，而是通过采样的形式（称为sigma点）寻找一个与真实分布近似的高斯分布（这些采样点是通过权重组合的：样本点权重wmiw_m^iwmi和wciw_c^iwci），其想法是近似非线性函数的概率分布要比近似非线性函数本身要容易，因而这里的先验假设还是没有摆脱高斯分布的影响。对于非线性系统的运动方程：
xt=f(xt−1,ut)+ωtx_t=f(x_{t-1},u_t)+\omega_txt=f(xt−1,ut)+ωt
对应的观测方程：
x˙t=g(xt)+vt\dot{x}_t=g(x_t)+v_tx˙t=g(xt)+vt
给定上一时刻卡尔曼滤波算法输出结果：xˉt−1,Pˉt−1\bar{x}_{t-1},\bar{P}_{t-1}xˉt−1,Pˉt−1，也就是上一轮迭代结果的均值方差。则按照UKF的算法原理，这里在上一轮迭代结果为高斯参数的指引下采样数目为2n+12n+12n+1（其中nnn为与Pˉt−1\bar{P}_{t-1}Pˉt−1维度相关的数）的采样，这里将这些采样点记为x^ti\hat{x}_t^ix^ti：
x^ti={xˉt−1,if i=1xˉt−1+((n+λ)Pˉt−1)i,if n∈[2,…,n+1]xˉt−1−((n+λ)Pˉt−1)i−n,if n∈[n+2,…,2n+1]\hat{x}_t^i= \begin{cases} \bar{x}_{t-1}, & \text{if $i$ =1} \\ \bar{x}_{t-1}+(\sqrt{(n+\lambda)\bar{P}_{t-1}})_i, & \text{if $n\in[2,\dots,n+1]$}\\ \bar{x}_{t-1}-(\sqrt{(n+\lambda)\bar{P}_{t-1}})_{i-n}, & \text{if $n\in[n+2,\dots,2n+1]$} \end{cases}x^ti=⎩⎨⎧xˉt−1,xˉt−1+((n+λ)Pˉt−1)i,xˉt−1−((n+λ)Pˉt−1)i−n,if i =1if n∈[2,…,n+1]if n∈[n+2,…,2n+1]
其中，((n+λ)Pˉt−1)i−n(\sqrt{(n+\lambda)\bar{P}_{t-1}})_{i-n}((n+λ)Pˉt−1)i−n中的i−ni-ni−n代表矩阵((n+λ)Pˉt−1)i−n(\sqrt{(n+\lambda)\bar{P}_{t-1}})_{i-n}((n+λ)Pˉt−1)i−n的第i−ni-ni−n列，矩阵开放可用采用Cholesky分解计算出Pˉt−1=LLT\bar{P}_{t-1}=LL^TPˉt−1=LLT（LLL为三角矩阵）。λ\lambdaλ表示采样点距离均值多远，越远权重越小。该参数满足：
λ=α2(n+k)−n\lambda=\alpha^2(n+k)-nλ=α2(n+k)−n
完成上述采样过程基础上，同时在采样的过程中需要满足如下约束（wiw^iwi为采样权重）：
∑i=12n+1wi=1xˉt−1=∑i=12n+1wix^tiPˉt−1=∑i=12n+1wi(x^ti−xˉt−1)(x^ti−xˉt−1)T\begin{matrix} \sum_{i=1}^{2n+1}w^i=1 \\ \bar{x}_{t-1}=\sum_{i=1}^{2n+1}w^i\hat{x}_t^i \\ \bar{P}_{t-1}=\sum_{i=1}^{2n+1}w^i(\hat{x}_t^i-\bar{x}_{t-1})(\hat{x}_t^i-\bar{x}_{t-1})^T \\ \end{matrix} ∑i=12n+1wi=1xˉt−1=∑i=12n+1wix^tiPˉt−1=∑i=12n+1wi(x^ti−xˉt−1)(x^ti−xˉt−1)T
具体对于上述权重wmiw_m^iwmi其权重定义如下
wmi={λn+λ,if n=112(n+λ),if n∈[2,…,2n+1]w_m^i = \begin{cases} \frac{\lambda}{n+\lambda}, & \text{if $n$ =1} \\ \frac{1}{2(n+\lambda)}, & \text{if $n\in[2,\dots,2n+1]$} \end{cases}wmi={n+λλ,2(n+λ)1,if n =1if n∈[2,…,2n+1]
具体对于上述权重wciw_c^iwci其权重定义如下
wci={λn+λ+1−α2+β,if n=112(n+λ),if n∈[2,…,2n+1]w_c^i = \begin{cases} \frac{\lambda}{n+\lambda}+1-\alpha^2+\beta, & \text{if $n$ =1} \\ \frac{1}{2(n+\lambda)}, & \text{if $n\in[2,\dots,2n+1]$} \end{cases}wci={n+λλ+1−α2+β,2(n+λ)1,if n =1if n∈[2,…,2n+1]
上述的过程都是在高斯分布的假设下实现的，则对于上述超参数取值建议为：β=2,α∈[0,1.],k=3−n\beta=2,\alpha\in[0,1.],k=3-nβ=2,α∈[0,1.],k=3−n。则按照上述过程中的采样准则之后新分布下的运动状态表述为：
x^t=∑i=12n+1wmix^ti\hat{x}_t=\sum_{i=1}^{2n+1}w_m^i\hat{x}_t^ix^t=i=1∑2n+1wmix^ti
对应的预测协方差矩阵描述为（RtR_tRt项是为了模拟过程噪声）：
P^t=∑i=12n+1wci(x^ti−x^t)(x^ti−x^t)T+Rt\hat{P}_t=\sum_{i=1}^{2n+1}w_c^i(\hat{x}_t^i-\hat{x}_t)(\hat{x}_t^i-\hat{x}_t)^T+R_tP^t=i=1∑2n+1wci(x^ti−x^t)(x^ti−x^t)T+Rt
同样对于观测方程，也可以使其在(x^t,P^t)(\hat{x}_t,\hat{P}_t)(x^t,P^t)上进行采样对应高斯采样点，但是一般为了效率直接沿用了上面的采样结果，这样带来的问题便是精度不足了。则对应的观测方程的预测结果描述为：
x˙t′=∑i=12n+1wmig(x^ti)\dot{x}_t^{'}=\sum_{i=1}^{2n+1}w_m^ig(\hat{x}_t^i)x˙t′=i=1∑2n+1wmig(x^ti)
对应的预测协方差矩阵描述为（QtQ_tQt项是为了模拟过程噪声）：
P˙t=∑i=12n+1wci(g(x^ti)−x˙t′)(g(x^ti)−x˙t′)T+Qt\dot{P}_t=\sum_{i=1}^{2n+1}w_c^i(g(\hat{x}_t^i)-\dot{x}_t^{'})(g(\hat{x}_t^i)-\dot{x}_t^{'})^T+Q_tP˙t=i=1∑2n+1wci(g(x^ti)−x˙t′)(g(x^ti)−x˙t′)T+Qt
回顾之前关于卡尔曼增益的迭代计算式子：Kt=P^tHtT(HtP^tHtT+Rt)−1\mathcal{K}_t=\hat{P}_t\mathcal{H}_t^T(\mathcal{H}_t\hat{P}_t\mathcal{H}_t^T+R_t)^{-1}Kt=P^tHtT(HtP^tHtT+Rt)−1，则上述观测协方差的计算过程不就正好与该式子中取逆部分对应：
P˙t≈HtP^tHtT+Rt\dot{P}_t \approx \mathcal{H}_t\hat{P}_t\mathcal{H}_t^T+R_tP˙t≈HtP^tHtT+Rt
为了令式子简洁设：
P^tx^,x˙=P^tHtT\hat{P}_t^{\hat{x},\dot{x}}=\hat{P}_t\mathcal{H}_t^TP^tx^,x˙=P^tHtT
则增益矩阵便为：
Kt=P^tx^,x˙(P˙t)−1\mathcal{K}_t=\hat{P}_t^{\hat{x},\dot{x}}(\dot{P}_t)^{-1}Kt=P^tx^,x˙(P˙t)−1
对于协方差更新：
Pˉt=(1−KtHt)P^t=P^t−KtHtP^t=P^t−Kt(P^tx^,x˙)T=P^t−Kt(P^tx^,x˙P˙t−1P˙t)T=P^t−Kt(KtP˙t)T=P^t−KtP˙tTKtT=P^t−KtP˙tKtT\begin{align} \bar{P}_t&=(1-\mathcal{K}_t\mathcal{H}_t)\hat{P}_t\\ &=\hat{P}_t-\mathcal{K}_t\mathcal{H}_t\hat{P}_t\\ &=\hat{P}_t-\mathcal{K}_t(\hat{P}_t^{\hat{x},\dot{x}})^T\\ &=\hat{P}_t-\mathcal{K}_t(\hat{P}_t^{\hat{x},\dot{x}}\dot{P}_t^{-1}\dot{P}_t)^T\\ &=\hat{P}_t-\mathcal{K}_t(\mathcal{K}_t\dot{P}_t)^T\\ &=\hat{P}_t-\mathcal{K}_t\dot{P}_t^T\mathcal{K}_t^T\\ &=\hat{P}_t-\mathcal{K}_t\dot{P}_t\mathcal{K}_t^T\\ \end{align}Pˉt=(1−KtHt)P^t=P^t−KtHtP^t=P^t−Kt(P^tx^,x˙)T=P^t−Kt(P^tx^,x˙P˙t−1P˙t)T=P^t−Kt(KtP˙t)T=P^t−KtP˙tTKtT=P^t−KtP˙tKtT

4.2 UKF迭代过程

Step1：预测阶段
得到运动预测状态：
x^t=∑i=12n+1wmix^ti\hat{x}_t=\sum_{i=1}^{2n+1}w_m^i\hat{x}_t^ix^t=i=1∑2n+1wmix^ti
得到当前运动状态协方差：
P^t=∑i=12n+1wci(x^ti−x^t)(x^ti−x^t)T+Rt\hat{P}_t=\sum_{i=1}^{2n+1}w_c^i(\hat{x}_t^i-\hat{x}_t)(\hat{x}_t^i-\hat{x}_t)^T+R_tP^t=i=1∑2n+1wci(x^ti−x^t)(x^ti−x^t)T+Rt
得到预测状态：
x˙t′=∑i=12n+1wmig(x^ti)\dot{x}_t^{'}=\sum_{i=1}^{2n+1}w_m^ig(\hat{x}_t^i)x˙t′=i=1∑2n+1wmig(x^ti)
预测协方差矩阵描述为：
P˙t=∑i=12n+1wci(g(x^ti)−x˙t′)(g(x^ti)−x˙t′)T+Qt\dot{P}_t=\sum_{i=1}^{2n+1}w_c^i(g(\hat{x}_t^i)-\dot{x}_t^{'})(g(\hat{x}_t^i)-\dot{x}_t^{'})^T+Q_tP˙t=i=1∑2n+1wci(g(x^ti)−x˙t′)(g(x^ti)−x˙t′)T+Qt

Step2：更新阶段
计算卡尔曼增益：
Kt=P^tx^,x˙(P˙t)−1\mathcal{K}_t=\hat{P}_t^{\hat{x},\dot{x}}(\dot{P}_t)^{-1}Kt=P^tx^,x˙(P˙t)−1
得到卡尔曼最终估计结果：
xˉt=x^t+Kt(x˙t−x˙t′)\bar{x}_t=\hat{x}_t+\mathcal{K}_t(\dot{x}_t-\dot{x}_t^{'})xˉt=x^t+Kt(x˙t−x˙t′)
更新卡尔曼最终估计结果协方差：
Pˉt=P^t−KtP˙tKtT\bar{P}_t=\hat{P}_t-\mathcal{K}_t\dot{P}_t\mathcal{K}_t^TPˉt=P^t−KtP˙tKtT

Step3：结果返回
xˉt,Pˉt\bar{x}_t,\bar{P}_txˉt,Pˉt

5. 参考资料

CKF及鲁棒滤波在飞行器姿态估计中的应用研究
卡尔曼滤波（3）-- EKF, UKF
第六课无迹卡尔曼滤波（UKF）
自动驾驶感知融合-无迹卡尔曼滤波(Lidar&Radar)
卡尔曼滤波算法原理(KF,EKF,AKF,UKF)

KF、EKF、IEKF、UKF卡尔曼滤波器相关推荐

卡尔曼滤波算法原理(KF,EKF,AKF,UKF)
卡尔曼滤波算法原理(KF,EKF,AKF,UKF) 主要是KF.EKF.UKF算法公式推导,直接看公式会比较枯燥,建议推导一下. 新增文章卡尔曼运动模型公式推导CTRV+CTRA,主要是EKF的CTR ...
五.卡尔曼滤波器(EKF)开发实践之五: 编写自己的EKF替换robot_pose_ekf中EKF滤波器
本系列文章主要介绍如何在工程实践中使用卡尔曼滤波器,分七个小节介绍: 一.卡尔曼滤波器开发实践之一: 五大公式二.卡尔曼滤波器开发实践之二: 一个简单的位置估计卡尔曼滤波器三.卡尔曼滤波器(EK ...
卡尔曼滤波滤波方程_了解卡尔曼滤波器及其方程
卡尔曼滤波滤波方程 Before getting into what a Kalman filter is or what it does, let's first do an exercise. O ...
【信号处理】基于扩展卡尔曼滤波器和无迹卡尔曼滤波器的窄带信号时变频率估计（Matlab代码实现）
目录 1 概述 2 数学模型 3 运行结果 4 结论 5 参考文献 6 Matlab代码实现 1 概述本文讲解和比较了基于卡尔曼滤波器的频率跟踪方法的能力,例如扩展卡尔曼滤波器 (EKF) 和无味卡 ...
【滤波】设计卡尔曼滤波器
%matplotlib inline #format the book import book_format book_format.set_style() 简介在上一章节中,我们讨论了教科书式的问 ...
了解卡尔曼滤波器4--非线性状态估算器(EKF,UKF,PF)
一般来说,我们希望我们的生活是线性的,就像这条线,这可能表示成功.收入或者幸福.但实际上,生活并不是线性的,它充满了起伏,有时甚至更复杂. 如果您是工程师,您经常会需要处理非线性系统,为了帮助您,我们 ...
自动驾驶算法-滤波器系列（二）—— 卡尔曼滤波简介及其变种（EKF、UKF、PF）介绍
KF&EKF&UKF&PF 1. 基础知识概要协方差矩阵多维高斯分布状态空间表达式 2. 什么是卡尔曼滤波器 3. 五个重要的公式公式介绍公式推导过程 4. 卡尔曼滤 ...
运动目标跟踪（一）--搜索算法预测模型之KF,EKF,UKF
这里先总体介绍下,原文转自: http://www.cnblogs.com/gaoxiang12/p/5560360.html 任何传感器,激光也好,视觉也好,整个SLAM系统也好,要解决的问题只有一 ...
无迹卡尔曼滤波器（UKF）
无迹卡尔曼滤波器(UKF) UKF依然没有脱离KF的框架.只不过对下一时刻状态的预测方法变成了sigma点集的扩充与非线性映射.这样做有两个优点:1.避免了复杂非线性函数雅可比矩阵的复杂运算:2.保证 ...

KF、EKF、IEKF、UKF卡尔曼滤波器