[Math]理解卡尔曼滤波器 (Understanding Kalman Filter)

1. 卡尔曼滤波器介绍

卡尔曼滤波器的介绍，见 Wiki

这篇文章主要是翻译了 Understanding the Basis of the Kalman Filter Via a Simple and Intuitive Derivation

感谢原作者。

如果叙述有误，欢迎指正！

2. 基本模型

2.1 系统模型

卡尔曼滤波模型假设k时刻的真实状态是从(k − 1)时刻的状态演化而来，符合下式：

$技术分享$ (1)

Fk 是作用在 Xk−1 上的状态变换模型（/矩阵/矢量）。
Bk 是作用在控制器向量uk上的输入－控制模型。
Wk 是过程噪声，并假定其符合均值为零，协方差矩阵为Qk的多元正态分布。

$技术分享$ (2)

2.2 测量模型

时刻k，对真实状态 xk的一个测量zk满足下式:

$技术分享$ (3)

其中Hk是观测模型,它把真实状态空间映射成观测空间，vk 是观测噪声，其均值为零，协方差矩阵为Rk,且服从正态分布。

$技术分享$ (4)

初始状态以及每一时刻的噪声{x0, w1, ..., wk, v1 ... vk} 都认为是互相独立的.

卡尔曼滤波要做的是：

已知：

系统的初始状态 x0
每个时间的测量 Z
系统模型和测量模型

求解：

状态x随着时间变化而产生的值

3. 预测与更新

3.1 预测方程

预测是这样一个问题：

已知:

上一个状态的更新值
上一个状态的更新值和真实值之间的误差

求解：

这一个状态的预测值
这一个状态的预测值和真实值之间的误差

过程包括两个方面：

一、由上一个更新值 Xk-1|k-1 预测这一个预测值 Xk|k-1

二、由上一个更新值和真实值之间的误差 Pk-1|k-1 预测下一个预测值和真实值之间的误差 Pk|k-1

具体来说，就是以下两个方程。

$技术分享$ (预测状态) (5)

$技术分享$ (预测估计协方差矩阵) (6)

这里：

Xk-1|k-1 这种记法代表的是上一次的更新值，后面一个 k-1可以看做 Zk-1，也就是上一次经过对比Zk-1(实际就是更新)之后所估计出的状态Xk-1。

Xk|k-1 这种记法代表这一次的预测值，同理于刚才的介绍，经过上一次Zk-1之后所估计出的状态Xk。

预测公式-预测状态
也就是公式(5)，可以直接由系统模型导出。

预测公式-协方差矩阵：

P代表着估计误差的协方差，代表着一种 confidence ，比如先验估计误差(预测值与真实值之间误差)的协方差

技术分享

注：

方差有两种形式

$技术分享$

协方差的定义：

$技术分享$

如果说方差是用来衡量一个样本中，样本值的偏离程度的话。协方差就是用来衡量两个样本之间的相关性有多少，也就是一个样本的值的偏离程度，会对另外一个样本的值偏离产生多大的影响，协方差是可以用来计算相关系数的，相关系数P=Cov(a.b)/Sa*Sb, Cov(a.b)是协方差， Sa Sb 分别是样本标准差。【可以参考另一篇博客《理解协方差》】

cov(x,y)
= E( (x-u)(y-v)‘ )
= E(xy‘ - xv‘ - uy‘ + uv‘)
= E(xy‘) - E(xv‘) - E(uy‘) + E(uv‘)
= E(xy‘) - uv‘ - uv‘ + uv‘
= E(xy‘) - uv‘

比较(5)和(1), 相减：

技术分享

由于状态估计误差和系统噪声是不相关的

技术分享

注：

如果随机变量 x和y是不相关的，那么

Cov(x,y) = 0
=> E( (x-ex)(y-ey)‘ ) = 0
=> E(xy‘) - ex*ey‘ = 0

如果 ex 和ey为0 => E(x,y‘) = 0 就像上面的情况，误差和噪声都服从正态分布，所以期望都是0 .

独立的充要条件：P(xy) = P(x)P(y)

3.2 更新方程

更新过程实际上就是一下问题：

已知：

由上一个更新值得到的当前的预测值。
当前的观测值
观测模型

求解：

融合了预测值和观测的更新值
由预测值的估计误差得到更新值的估计误差

更新方程如下：

技术分享

其中K称为kalman增益，就像一个补偿，决定着预测值应该变化多少幅度，才能变成更新值。
技术分享

先看一个简单的例子，从这个例子中来推导出这三个方程。

3.3 简单的例子

3.3.1 举例

技术分享

有一个直线轨道，轨道上有一个火车，从火车站出发，在t时刻，火车想要知道自己距离火车站的位置，可以有两个信息来源：

根据 t-1时刻的状态信息，以及一些控制信息来推断，状态包括 t-1时刻的位置、速度等，控制信息包括司机刹车、加速等等。
根据 t时刻的测量数据来推断，这里假设车上有一个声波发射器，可以探测到发射到火车站需要多少时间，进而得到离车站的距离。

要想得到一个比较好的结果，显然不能只依靠某一种方法来推断，而 Kalman Filter的方法是：

首先，利用t-1时刻进行推断，这一步叫预测。
然后，利用t时刻的measurement 也可以推断，使用这个推断对预测进行校正，这一步叫更新。

技术分享

3.3.2 火车位置 - 预测

t=0的时候，火车状态如 Figure.2 ，这时候，火车的位置是比较准确的。

t=1的时候，火车的预测状态如 Figure.3 可以看到，位置的方差变大了。

技术分享

火车的预测主要遵循式(1)，而预测的方差在不断变大,也就是说预测的准确度在下降, 这是由累积误差 w 导致的。

3.3.3 火车位置 - 测量

t=1 的时候，我们还有 measurement ，同样可以推断火车的位置，见下图中的蓝色的pdf

技术分享

3.3.4 火车位置 - 更新

将两个pdf相乘，得到下图中的绿色pdf，绿色pdf中较高的位置，意味着预测和测量对这个位置都比较支持。

技术分享

这里有一个高斯分布的一个重要性质就是，两个高斯分布的乘积还是高斯分布。

This is critical as it permits an endless number of Gaussian pdfs to be multiplied over time, but the resulting
function does not increase in complexity or number of terms; after each time epoch the new pdf is fully represented by a Gaussian function. This is the key to the elegant recursive properties of the Kalman filter。

3.3.5 推导更新方程

红色的pdf是预测的火车位置, 方程如下：

技术分享

蓝色的pdf是测量的火车位置, 方程如下：

技术分享

绿色的pdf二者融合的或者位置, 方程如下：

技术分享

写成如下形式：

技术分享

这里：

技术分享

这两个式子，就是kalman滤波的更新方程

但是，这只是一个很特殊的例子，因为这里假设预测和测量都是采用同样的坐标系

更现实的情况是二者需要统一到一个 domain 中

比如上面所举的例子中：

预测的时候，预测值是用米作为单位的。
但是当测量的时候，测量得到的值是用声波经过的秒数作为单位的。

必须先要把两个量统一到同一个domain才能进行融合。

比如上式子中， y2 (measurement)实际是声波传递时间的一个正态分布，也就是说单位是秒。

一般做法是把
预测值 => 测量值

y1就变成：

技术分享

y2不变：

技术分享

这样两个坐标系都在 mesaurement domain 了。

两个pdf所在坐标系的横轴都是表示时间，而且以秒为单位了。

`
统一domain之后，更新方程就有了如下形式

3.3.5.1 期望更新方程:

技术分享

将

H = 1/c

K = 技术分享代入，得到

技术分享

这里的H就相当于观测方程中的H， K就是卡尔曼增益。

3.3.5.2 方差更新方程:

类似地，融合之后的方差更新变成了

技术分享

4. 总结

各个变量对应的情况如下：

技术分享

最终的更新方程

技术分享

5. 实现

Talk is cheap, show me the code.

%本例子从百度文库中得到， 稍加注释
clear
N=200;%取200个数

%% 生成噪声数据 计算噪声方差
w=randn(1,N); %产生一个1×N的行向量，第一个数为0，w为过程噪声（其和后边的v在卡尔曼理论里均为高斯白噪声）
w(1)=0;
Q=var(w); % R、Q分别为过程噪声和测量噪声的协方差(此方程的状态只有一维，方差与协方差相同) 

v=randn(1,N);%测量噪声
R=var(v);

%% 计算真实状态
x_true(1)=0;%状态x_true初始值
A=1;%a为状态转移阵，此程序简单起见取1
for k=2:N
    x_true(k)=A*x_true(k-1)+w(k-1);  %系统状态方程，k时刻的状态等于k-1时刻状态乘以状态转移阵加噪声（此处忽略了系统的控制量）
end


%% 由真实状态得到测量数据， 测量数据才是能被用来计算的数据， 其他都是不可见的
H=0.2;
z=H*x_true+v;%量测方差，c为量测矩阵，同a简化取为一个数


%% 开始 预测-更新过程

% x_predict: 预测过程得到的x
% x_update:更新过程得到的x
% P_predict:预测过程得到的P
% P_update:更新过程得到的P

%初始化误差 和 初始位置
x_update(1)=x_true(1);%s(1)表示为初始最优化估计
P_update(1)=0;%初始最优化估计协方差

for t=2:N
    %-----1. 预测-----
    %-----1.1 预测状态-----
    x_predict(t) = A*x_update(t-1); %没有控制变量
    %-----1.2 预测误差协方差-----
    P_predict(t)=A*P_update(t-1)*A‘+Q;%p1为一步估计的协方差，此式从t-1时刻最优化估计s的协方差得到t-1时刻到t时刻一步估计的协方差

    %-----2. 更新-----
    %-----2.1 计算卡尔曼增益-----
    K(t)=H*P_predict(t) / (H*P_predict(t)*H‘+R);%b为卡尔曼增益，其意义表示为状态误差的协方差与量测误差的协方差之比(个人见解)
    %-----2.2 更新状态-----
    x_update(t)=x_predict(t)  +  K(t) * (z(t)-H*x_predict(t));%Y(t)-a*c*s(t-1)称之为新息，是观测值与一步估计得到的观测值之差，此式由上一时刻状态的最优化估计s(t-1)得到当前时刻的最优化估计s(t)
    %-----2.3 更新误差协方差-----
    P_update(t)=P_predict(t) - H*K(t)*P_predict(t);%此式由一步估计的协方差得到此时刻最优化估计的协方差
end

%% plot
%作图，红色为卡尔曼滤波，绿色为量测，蓝色为状态
%kalman滤波的作用就是 由绿色的波形得到红色的波形， 使之尽量接近蓝色的真实状态。
t=1:N;
plot(t,x_update,‘r‘,t,z,‘g‘,t,x_true,‘b‘);

6. Reference

Wiki-卡尔曼滤波器

Understanding the Basis of the Kalman Filter Via a Simple and Intuitive Derivation

Wiki-协方差矩阵