卡尔曼滤波器（Kalman Filter）基础编程
所属知识点：Robotics: Estimation or Robotics: Localization
代码地址：kf_target_tracking, temperature.m
归纳和总结机器人技术的库：ViolinLee/roboticsCV。该库挑选和收集各类与机器人技术相关的高质量资源，欢迎大家关注和Star！会一直更新下去。
1. 概念基础
估计（Estimation）可以多种形式出现。例如对于无人机，估计问题可以是“无人机在什么位置？”，“无人机的姿态如何？”，“无人机的速度多大？”等等。回答上述问题事实上就在解决机器人导航和定位问题！因此估计（Estimation）在机器人导航或定位（Navigation or Localization）中十分关键。
估计的本质是处理不确定性（Manage Uncertainty）。贝叶斯方法（Bayes Rule）能够随着时间集成数据，并生成能够最好地表示状态信息的分布。对于假设为正态分布（高斯分布）的状态变量，我们的估计目标就是生成非常窄的正态分布曲线，它代表非常高的确定性。
首先来了解一下贝叶斯滤波、高斯滤波和卡尔曼滤波之间的关系。事实上大多数计算置信度的通用算法都是由贝叶斯滤波算法给出的。贝叶斯算法是递归的，也就是说，时刻
的置信度
由时刻
的置信度
来计算。其输入是时刻的置信度，和最近的控制作用
及最近一次的一次测量
；其输出是时刻t的置信度。伪代码如下：
从历史上来看，各种高斯滤波成为连续空间的贝叶斯滤波中最早、最容易处理的工具。尽管有一些缺点，高斯滤波在目前为止仍是最流行的！所有高斯技术的基本思想：置信度用多元正态分布表示。而卡尔曼滤波的实质是：对具有线性动态测量函数的有限阶问题利用矩参数来实现贝叶斯滤波，它是较基础的高斯滤波，同时可能是最好的贝叶斯滤波。总的来说，Kalman Filter < Gaussian Filter < Bayes Filter。
利用卡尔曼滤波器进行机器人学的贝叶斯估计是十分常见的。卡尔曼滤波器是一种广泛使用的线性系统的最优跟踪算法。常见应用包括在自动驾驶中汽车对行人和车辆的追踪，或者装配线传送带上运动部件的追踪。
2 卡尔曼滤波
2.1 线性高斯系统
用高斯函数表示后验具有很重要的意义。高斯分布是单峰的，由单一的极大值。这样的后验是机器人学中很多跟踪问题的特点，后验以小的不确定性聚集在真实状态周围。但对于很多全局估计问题，由于许多很不同的假设存在，每一个假设都形成其自己的后验模式，因此高斯后验匹配性并不好。高斯滤波中参数均值和方差称为矩参数（moments parameterization）。这是因为均值和方差是概率分布的一阶矩和二阶矩；
KF是由Swerling（1958）和Kalman（1960）作为线性高斯系统（Linear Gassian system）中的预测和滤波技术而发明的，使用矩来定义的。KF实现了对连续状态的置信度计算。
KF用矩参数来表示置信度：在时刻，置信度用均值
（一阶矩）和方差
（二阶矩）表达。
使后验为高斯分布的前提，同时也是KF的特性：
（1）状态转移必须是带有随机高斯噪声的参数的线性函数。
（2）测量也与带有高斯噪声的自变量呈线性关系。
（3）初始置信度必须是正态分布的。
这三个假设足以确保后验在任何时刻t总符合高斯分布。
2.2 卡尔曼滤波算法
KF的输入是系统时刻的状态的置信度（正态分布，均值和方差分别用
和
表示）。为了更新这些参数，KF需要同样是输入的控制向量
和测量向量
。输出是系统t时刻的状态的置信度，均值为，方差为。卡尔曼滤波算法的伪代码如下：
介绍KF，通常从卡尔曼滤波器系统模型（System Model）及观测模型（Measurement Model）开始探讨，接着再讨论滤波器中的最大后验估计。
（1）基本动态系统模型![^{\left [ 1 \right ]}](http://img.e-com-net.com/image/info8/4308826bdbf940e388f9264f0f96780e.gif){kind=small}
卡尔曼滤波建立在线性代数和隐马尔可夫模型（hidden Markov model）上。其基本动态系统可以用一个马尔可夫链表示，该马尔可夫链建立在一个被高斯噪声（即正态分布的噪声）干扰的线性算子上的。系统的状态可以用一个元素为实数的向量表示。随着离散时间的每一个增加，这个线性算子就会作用在当前状态上，产生一个新的状态，并也会带入一些噪声，同时系统的一些已知的控制器的控制信息也会被加入。同时，另一个受噪声干扰的线性算子产生出这些隐含状态的可见输出。
为了从一系列有噪声的观察数据中用卡尔曼滤波器估计出被观察过程的内部状态，必须把这个过程在卡尔曼滤波的框架下建立模型。也就是说对于每一步k，定义矩阵
，
，
，
，有时也需要定义
。
卡尔曼滤波模型假设k时刻的真实状态是从（k − 1）时刻的状态演化而来，符合下式：
其中
- 是作用在
上的状态变换模型（/矩阵/矢量）。 - 是作用在控制器向量
上的输入－控制模型。 
是过程噪声，并假定其符合均值为零，协方差矩阵为的多元正态分布：
时刻k，对真实状态
的一个测量
满足下式：
其中
- 是观测模型，它把真实状态空间映射成观测空间。
是观测噪声，其均值为零，协方差矩阵为，且服从正态分布：
初始状态以及每一时刻的噪声{
, 
, ..., , 
... }都认为是互相独立的。实际上，很多真实世界的动态系统都并不确切地符合这个模型；但是由于卡尔曼滤波器被设计在有噪声的情况下工作，一个近似的符合已经可以使这个滤波器非常有用了。而且，卡尔曼滤波器存在其它稍微复杂的变种，例如扩展卡尔曼滤波器，无迹卡尔曼滤波器等。
（2）卡尔曼滤波器
卡尔曼滤波是一种递归的估计，即只要获知上一时刻状态的估计值以及当前状态的观测值就可以计算出当前状态的估计值，因此不需要记录观测或者估计的历史信息。卡尔曼滤波器与大多数滤波器不同之处，在于它是一种纯粹的时域滤波器，它不需要像低通滤波器等频域滤波器那样，需要在频域设计再转换到时域实现。
卡尔曼滤波器的状态由以下两个变量表示：
，在时刻k的状态的估计；
，后验估计误差协方差矩阵，度量估计值的精确程度。
卡尔曼滤波器的操作包括两个阶段：预测与更新。在预测阶段，滤波器使用上一状态的估计，做出对当前状态的预测。在更新阶段，滤波器利用对当前状态的观测值优化在预测阶段获得的预测值，以获得一个更精确的新估计值。
①预测
②更新
说明：使用上述公式计算仅在最优卡尔曼增益的时候有效。使用其他增益的话，公式要复杂一些，请参见推导。
2.3 图解例证
下图所示为一简单的一维定位案例（Probabilistic Robotics，第3.2.3小节），假设机器人沿着水平轴移动。①机器人定位的先验概率由图a所示的正态分布给定。②机器人依靠传感器（如GPS）返回测量值，如图b所示具有尖峰且较窄的高斯分布（粗线）。峰值（均值）是由传感器指示的值；宽度（方差）表征测量的不确定性，越宽方差越大，则不确定性越大。③将先验与测量结合，经过程序的第4~6行，得到图c所示高斯分布。该置信度的均值在两个初始均值之间，其不确定性比两个贡献的高斯分布要小（后者可能与直觉相悖，但却是KF中信息集成的一般特点）。④假定机器人向右移动。由于状态转移存在随机性，因此其不确定性增大。经过程序的第2~3行，得到图d粗线所示曲线变宽的高斯分布。⑤接着机器人返回图e所示的测量（具有与②中描述类似特性）。⑥经过程序第4~6行，获得图f所示的后验。
整个过程调用Kalman滤波程序两次，由于第一次是初始状态不存在控制量，因此整个过程包括一次初始状态①，一次运动④，两次测量②⑤和两次更新③⑥。
a) 初始置信度 b) 粗线表示具有相关不确定性的测量
c) 利用KF滤波器将测量融合到初始置信度后获得的置信度 d) 物体右移后的置信度（移动会引入不确定性）
e) 具有相关不确定性的新测量 f）获得的置信度结果
2.4 数学推导
详见《卡尔曼滤波原理及应用》3.1.2章节，或《概率机器人/Probilistic Robotics》3.2.4章节。这部分后面给出，刚开始学习时可以略过。
3 卡尔曼滤波器编程
3.1 温度测量
（1）问题描述
估计一个房间的温度。假设，根据经验可以判断房间温度在25℃左右，我们以分钟为单位定时测量房间温度（采样）。房间温度可能受空气流通、阳光等因素影响，会小幅度地波动，即引入过程噪声
，其方差为
，大小假定为
（假如不考虑过程噪声的影响，则
，即真实温度是恒定的）。
（2）系统建模
状态
是一维变量温度；根据问题描述可知，状态转移矩阵
；噪声驱动矩阵
；观测矩阵
；控制量
。
则该系统的状态方程可以写为：
假设温度计的测量误差为
℃，从出厂说明书上得知温度计的方差为0.25，即温度计存在测量噪声
，且方差为0.25。因此，测量方程可以写为：
（3）卡尔曼滤波
时刻的温度值需要靠
时刻的温度估计值及时刻的温度测量值进行估计：
假设第时刻的温度估计值为23.9℃，房间真实温度值为24.0℃，则该测量值的偏差为0.1℃，即状态的误差协方差为
。
假设第时刻，房间的真实温度是24.1℃，温度计在该时刻的测量值为24.5℃，偏差为0.4℃。因此，我们用于估算第时刻温度值的数据有两个，一个是时刻的温度估计值23.9℃（先验），另一个是时刻的温度测量值24.1℃（测量）。现在要做的，就是将先验和测量融合，得到最逼近时刻真实温度值的估计！
① 利用时刻的温度估计值预测第时刻的温度值，其系统状态的误差协方差为：
② 计算Kalman增益：
③ 这时候利用时刻的观测值，得到温度的估计值为：
℃
可以直观的看出，与23.9℃和24.5℃相比较，Kalman估计值23.944℃更接近真实值24.1℃。
④ 最后更新时刻的误差协方差：
至此，我们计算出和
，可以继续对下一时刻的观测数据
进行处理。这样，Kalman滤波器就不断地递归，从而估算出最优的温度值。在实际运用时，需要确定Kalman滤波器的两个初始值，分别是
和
。
运行代码，可以获得如下两幅图所示结果。可以看出，Kalman滤波与温度计原始测量值相比，明显降低了偏差，它使状态尽可能地逼近真实值。
（4）代码 temperature.m
代码可以直接运行。需要说明的是，实时运用的Kalman滤波器应该是被递归调用的（所以通常需要定义静态变量）。但在温度测量这个例子，我们假设了有限时间段内、测量时间间隔恒定的测量条件，且“温度真实值”
也是模拟生成的，所以为了简单直接，就用迭代的方式来实施Kalman滤波器。
function main
% System Model and Measurement Model
N=120;
CON=25;
Xexpect=CON*ones(1,N);
X=zeros(1,N);
Xkf=zeros(1,N);
Z=zeros(1,N);
P=zeros(1,N);
X(1)=25.1;
P(1)=0.01;
Z(1)=24.9;
Xkf(1)=Z(1);
Q=0.01;
R=0.25;
W=sqrt(Q)*randn(1,N);
V=sqrt(R)*randn(1,N);
F=1;
G=1;
H=1;
I=eye(1);
% Kalman Filter
for k=2:N
X(k)=F*X(k-1)+G*W(k-1); % Note that in real world, X(t) is unknown
Z(k)=H*X(k)+V(k);
X_pre=F*Xkf(k-1);
P_pre=F*P(k-1)*F'+Q;
Kg=P_pre*inv(H*P_pre*H'+R);
e=Z(k)-H*X_pre;
Xkf(k)=X_pre+Kg*e;
P(k)=(I-Kg*H)*P_pre;
end
% Analysis
Err_Messure=zeros(1,N);
Err_Kalman=zeros(1,N);
for k=1:N
Err_Messure(k)=abs(Z(k)-X(k));
Err_Kalman(k)=abs(Xkf(k)-X(k));
end
t=1:N;
figure('Name','Kalman Filter Simulation','NumberTitle','off');
plot(t,Xexpect,'-b',t,X,'-r',t,Z,'-k',t,Xkf,'-g');
legend('expected','real','measure','kalman extimate');
xlabel('sample time');
ylabel('temperature');
title('Kalman Filter Simulation');
figure('Name','Error Analysis','NumberTitle','off');
plot(t,Err_Messure,'-b',t,Err_Kalman,'-k');
legend('messure error','kalman error');
xlabel('sample time');
ylabel('error');
title('Error Analysis');
3.2 位置追踪（Position Tracking）
接下来介绍一个稍微复杂的例子，相较温度测量的例子，Kalman滤波器追踪的状态只是一个一元变量（温度值），这次使用Kalman滤波器来追踪的状态是个四元变量。
（1）问题描述
追踪2D环境下球的位置。应用场景为足球机器人：机器人需要追踪球的轨迹（Track the Trajectory），甚至预测未来的运动（Predict its future destinations），以规划接下来的动作。关于问题更详细的说明详见PDF文档。
难点：除了测量位置，机器人需要辨识出球在环境中的动力学特性。机器人为做出行动决策，关心的是隐藏状态（Hidden Variable）的高层概念。但是我们并不总能获得与状态的形式相一致的信息。例如追踪小球速度时，我们只能观测它的位置，而没有对速度的直接测量。
任务：使用卡尔曼滤波器对线性动态系统建模，并跟踪它随时间变化的不确定性：使用后验分布（Posterior Distibutions）及最大后验估计（Maximum-A-Posterior Estimation）来估计它的分布；
（2）系统建模
为了跟踪一个运动的目标，必须建立其动力系统模型。动力系统描述了目标实时变化的状态，以及如何测量这些状态。为了估计二维位置并预测未来位置，状态向量
（状态一般用
表示，此处用s只是为了不与坐标
中的混淆）应包括速度
和位置
，和都是二元变量（二维坐标的缘故），因此是四元向量。状态向量建议表示为：![s=\left [ px,vx, py, vy \right ]^{T}](http://img.e-com-net.com/image/info8/1b21bc8443b14d49a69c201a2cc1f61b.gif){kind=small}
或 ![s=\left [ px, py,vx, vy \right ]^{T}](http://img.e-com-net.com/image/info8/20533369028c476e9ece1c4df364572d.gif){kind=small}
，这里表示为后一种形式。
则动力系统的状态转移矩阵为：
![A=\left [ \begin{matrix} 1 & 0& dt &0 \\ 0& 1 & 0 & dt\\ 0& 0& 1& 0\\ 0& 0& 0& 1 \end{matrix} \right ]](http://img.e-com-net.com/image/info8/9e46d7ec6e4c4ab983c74542096d6ea9.gif)
接着考虑过程噪声的协方差的形式，这里使用最简单的对角矩阵形式。与状态变量相对应，可以表示为：
![Q=\left [ \begin{matrix} \sigma_{px}^{2} & 0& 0 &0 \\ 0& \sigma_{py}^{2} & 0 & 0 \\ 0& 0& \sigma_{vx}^{2}& 0\\ 0& 0& 0& \sigma_{vy}^{2} \end{matrix} \right ]](http://img.e-com-net.com/image/info8/4b78572fca3243e496826e4ac5d65b2c.gif)
系统方程具有以下形式（注意：用
表示）：
测量数据是以二维坐标的形式读取的，格式上为：![z=\left [ zx, zy \right ]^{T}=\left [ px, py \right ]^{T}](http://img.e-com-net.com/image/info8/bf612ad071a44aefad1dc6bd57c21e27.gif){kind=small}
。因此观测矩阵
的维度应该是
：
![C=\left [ \begin{matrix} 1 & 0& 0 &0 \\ 0& 1 & 0 & 0 \end{matrix} \right ]](http://img.e-com-net.com/image/info8/9ae34fcd5d5a47ef93e387370e749cd4.gif){kind=small}
接着考虑测量噪声的协方差
的形式，这里使用最简单的对角矩阵形式。与测量变量
相对应，可以表示为：
![R=\left [ \begin{matrix} \sigma_{zx}^{2} & 0& 0 &0 \\ 0& \sigma_{zy}^{2} & 0 & 0 \end{matrix} \right ]](http://img.e-com-net.com/image/info8/372777eeb30243059d866b3ae816a82c.gif){kind=small}
观测方程具有以下形式（注意：用
表示）：
（3）卡尔曼滤波
有了系统模型和观测模型，就可以利用Kalman滤波器了，流程和温度测量的例子是一致的。但是这里有一些需要注意的地方，特别是在设定和调节参数值时。另外与温度测量的例子不同的是，由于是取自真实数据，这里每两次相邻测量的时间间隔并不是恒定的，但也十分接近。
运行代码，可以获得下图所示结果。
（4）代码库 kf_target_tracking；下面给出的是实施Kalman滤波算法的m文件kalmanFilter.m，不能单独运行，需要下载kf_target_tracking的其他文件，在同一个路径下配合运行。kalmanFilter() 函数的接口比较具有借鉴意义。输入包括：时刻t和上一时刻previous_t，传感器测量值x和y，上一时刻的状态估计值state（二元向量），估计协方差矩阵（param.P）。
function [ predictx, predicty, state, param ] = kalmanFilter( t, x, y, state, param, previous_t )
%UNTITLED Summary of this function goes here
% Four dimensional state: position_x, position_y, velocity_x, velocity_y
%% Place parameters like covarainces, etc. here:
% P = eye(4)
% R = eye(2)
% Check if the first time running this function
if previous_t<0
state = [x, y, 0, 0];
param.P = 0.1 * eye(4);
predictx = x;
predicty = y;
return;
end
%% Naive estimate
% % As an example, here is a Naive estimate without a Kalman filter
% % You should replace this code
% vx = (x - state(1)) / (t - previous_t);
% vy = (y - state(2)) / (t - previous_t);
% % Predict 330ms into the future
% predictx = x + vx * 0.330;
% predicty = y + vy * 0.330;
% % State is a four dimensional element
% state = [x, y, vx, vy];
%% TODO: Add Kalman filter updates
dt = t - previous_t; % Time interval
% PREDICT
A = [1 0 dt 0; % Transform matrix A
0 1 0 dt;
0 0 1 0;
0 0 0 1];
C = [1 0 0 0; % Observation matrix C
0 1 0 0];
z_t = [x, y]'; % Measurement data
% Q = 0.005 * eye(4);
Q = [dt^2 0 0 0; % System (Motion) noise covariance
0 dt^2 0 0;
0 0 1 0;
0 0 0 1];
R = 0.005 * eye(2); % Measurement noise covariance
P = A * param.P * transpose(A) + Q; % Prior estimation covariance
% UPDATE
K = P * transpose(C) * inv(R + C * P * transpose(C)); % Kalman gain
state = (A * (state') + K * (z_t - C * A * (state')))'; % Posterior state estimation
param.P = P - K * C * P; % Posterior estimation covariance
% Predict 330ms into the future
predictx = state(1) + state(3) * 0.330;
predicty = state(2) + state(4) * 0.330;
endResources & References
资源及参考
[1] WikiPedia. Kalman filter, 卡尔曼滤波
[2] Matlab. Understanding Kalman Filters
[3] Coursera. Robotics: Estimation and Learning
[4] 黄小平, 王岩. 卡尔曼滤波原理及应用[M]. 电子工业出版社, 2015.
[5] Matlab Documentation. Control System Toolbox -> ... - > State Estimation -> Kalman Filter
[6] Matlab. Design and use Kalman filters in MATLAB and Simulink
[7] Matlab File Exchange. Learning the Kalman Filter
[8] Matlab File Exchange. Kalman Filter Package
[9] Sebastian Thrun. Probabilistic Robotics. PDF
[10] Probalistic Robotics Solution
Further Reading
拓展阅读
[1] Kalman-and-Bayesian-Filters-in-Python
[2] Tutorial: The Kalman Filter
[3] Moment (mathematics)