电子技术论坛_中国专业的电子工程师学习交流社区-中电网技术论坛

标题: Kalman滤波器实现（2） [打印本页]

作者: look_w 时间: 2017-9-23 15:14 标题: Kalman滤波器实现（2）

一场递推的游戏为充分利用测量值（Measurement）和预测值（Prediction），Kalman滤波并不是简单的取其中一个作为输出，也不是求平均。
设预测过程噪声w(n)~N(0,Q)，测量噪声v(n)~N(0,R)。Kalman计算输出分为预测过程和修正过程如下：

预测
预测值：
$\mathbf{\hat{x}(n|n-1)=A\hat{x}(n-1|n-1)+Bu(n)}................................(5)$
最小均方误差矩阵：
$\mathbf{P(n|n-1)=AP(n-1|n-1)A^T+Q}................................(6)$
修正
误差增益：
[img]http://www.forkosh.com/mathtex.cgi?%20%5Clarge%20%5Cmathbf%7BK%28n%29=P%28n%7Cn-1%29H%5ET%28n%29[R%28n%29+H%28n%29P%28n%7Cn-1%29H%5ET%28n%29]%5E%7B-1%7D%7D.............%287%29[/img]
修正值：
[img]http://www.forkosh.com/mathtex.cgi?%20%5Clarge%20%5Cmathbf%7B%5Chat%7Bx%7D%28n%7Cn%29=A%5Chat%7Bx%7D%28n%7Cn-1%29+K%28n%29[%7Bz%28n%29-H%28n%29%5Chat%7Bx%7D%28n%7Cn-1%29%7D]%7D......................%288%29[/img] 这里的A去掉，参见下面的图
最小均方误差矩阵：
[img]http://www.forkosh.com/mathtex.cgi?%20%5Clarge%20%5Cmathbf%7BP%28n%7Cn%29=[I-K%28n%29H%28n%29]P%28n%7Cn-1%29%7D................................%289%29[/img]

从(5)~(9)中：

x(n)：Nx1的状态矢量
z(n)：Mx1的观测矢量，Kalman滤波器的输入
x(n|n-1)：用n时刻以前的数据进行对n时刻的估计结果
x(n|n)：用n时刻及n时刻以前的数据对n时刻的估计结果，这也是Kalman滤波器的输出
P(n|n-1)：NxN，最小预测均方误差矩阵，其定义式为
[img]http://www.forkosh.com/mathtex.cgi?%20%5Clarge%20%5Cmathbf%7BP%28n%7Cn-1%29=E%5C%7B[x%28n%29-%5Chat%7Bx%7D%28n%7Cn-1%29][x%28n%29-%5Chat%7Bx%7D%28n%7Cn-1%29]%5ET%5C%7D%7D[/img]
通过计算最终得到(6)式。
P(n|n)：NxN，修正后最小均方误差矩阵。
K(n)：NxM，误差增益，从增益的表达式看，相当于“预测最小均方误差”除以“n时刻的测量误差+预测最小均方误差”，直观含义就是用n-1预测n时刻状态的预测最小均方误差在n时刻的总误差中的比重，比重越大，说明真值接近预测值的概率越小（接近测量值的概率越大），这也可以从(8)式中看到。

Kalman滤波算法的步骤是(5)(6)->(7)->(8)(9)。当然，建议找本教材复习下上面公式的推导过程，或参见Wiki上的介绍http://en.wikipedia.org/wiki/Kalman_filter。
公式就是那么的抽象，一旦认真研究懂了却也是茅塞顿开，受益也比只知皮毛的多。尽管如此，我还算更喜欢先感性后理性。仍以上面的运动的例子来直观分析：

Example:
还可以更简单一些：设小车做匀速（而非匀加速）直线运动，方便计算，假设速度绝对的恒定（不波动，所以相关的方差都为0），则u(t)==0恒成立。设预测（过程）位移噪声w(n)~N(0,2^2)，测量位移噪声v(n)~N(0,1^2)，n-1状态的位移 $\Small \hat{x}(n-1|n-1)=50m}$ ，速度为v=10m/s，n时刻与n-1时刻的物理时差为ΔT=1s。同时，也用尺子测了一下，结果位移为z(n)=62m。
则A = [1 ΔT; 0 1]=[1 1; 0 1]，根据(5)，预测值为
[img]http://www.forkosh.com/mathtex.cgi?%20%5CSmall%20%5Chat%7Bx%7D%28n%7Cn-1%29=1*%5Chat%7Bx%7D%28n-1%7Cn-1%29+v*%5CDelta%7BT%7D=[60m;%2010m/s]%7D[/img]。
现在已经有了估计值和测量值，哪个更接近真值，这就通过最小均方误差矩阵来决定！
要求已知上次的修正后的最小均方误差P(n-1|n-1)=[1 0; 0 0]（匀速，所以P(2,2)=0，右斜对角线上为协方差，值为0，P(1,1)为n-1时刻位移量的均方误差，因为要计算P(1,1)还得先递推往前计算P(n-2|n-2)，所以这里暂时假设为1），则根据(6)式，最小预测预测均方误差为P(n|n-1)=[1 0; 0 0][1 1; 0 1][1 0; 0 0]=[1 0; 0 0]。
由物理量的关系知，H(n)=[1 1]，增益K(n)=[1;0]{1+[1 1][1 0; 0 0][1; 1]}^(-1)=[1/2;0]。
所以，最后的n时刻估计值既不是用n-1得到的估计值，也不是测量值，而是：[img]http://www.forkosh.com/mathtex.cgi?%20%5CSmall%20%5Chat%7Bx%7D%28n%7Cn%29=[60;1]+[1/2;0]%7B62-[1%201][60;1]%7D=[60.5m;%201m/s][/img]，因此，最终的Kalman滤波器的输出位移是60.5m。

从上面的递推关系知道，要估计n时刻就必须知道n-1时刻，那么n=0时刻该如何估计，因此，卡尔曼滤波要初始化的估计值x(-1|-1)和误差矩阵P(-1|-1)，设x(-1,-1)~N(Us, Cs)，则初始化：
$\mathbf{x(-1|-1)=\mu_s}$
$\mathbf{P(-1|-1)=C_s}$
综上，借用一张图说明一下Kalman滤波算法的流程：

图中的符号和本文符号稍有差异，主要是P的表示上。从上图也可以看出，Kalman滤波就是给定-1时刻的初始值，然后在预测（状态空间）和修正（观测空间）之间不停的递推，求取n时刻的估计x和均方误差矩阵P。
均方误差中的门道到这里，应该对Kalman滤波有个总体的概念了，有几个观点很重要，是建立Kalman滤波器的基础：

一个是n-1对n时刻估计值，一个是n时刻的测量值，估计值和测量值都存在误差，且误差都假设满足独立的高斯分布
Kalman滤波器就是充分结合了估计值和测量值得到n时刻更接近真值的估计结果
Kalman滤波器引入状态空间的目的是避免了“像Wiener滤波器一样需要对过去所有[0,n-1]时刻协方差先验知识都已知”，而直接可以通过上一时刻即n-1时刻的状态信息和均方误差信息就可递推得到n时刻的估计。尽管递推使得实际应用中方便了，但n-1对n时刻的估计实际上使用到了所有前[0,n-1]时刻的信息，只不过信息一直通过最小均方误差进行传递到n-1时刻。基于此，Kalman滤波也需要先验知识，即-1时刻的初始值。

在上小节中只看到Kalman的结论，那么Kalman滤波器是如何将估计值和测量值结合起来，如何将信息传递下去的呢？这其中，“独立高斯分布”的假设条件功劳不可谓不大！测量值z(n)~N(uz,σz^2)，估计值x(n)~N(ux,σx^2)。

Kalman滤波器巧妙的用“独立高斯分布的乘积”将这两个测量值和估计值进行融合！

如下图：估计量的高斯分布和测量量的高斯分布经过融合后为绿色的高斯分布曲线。

$y_f=\frac{1}{\sqrt{2\pi{\sigma^2_z}}}\exp^{-\frac{(r-uz)^2}{2\sigma^2_z}}*\frac{1}{\sqrt{2\pi{\sigma^2_x}}}\exp^{-\frac{(r-ux)^2}{2\sigma^2_x}}=\frac{1}{\sqrt{2\pi{\sigma^2}}}\exp^{-\frac{(r-\mu)^2}{2\sigma^2}}$
稍微计算一下，通过上式求出u和σ^2，
$\mu=\frac{u_x\sigma^2_z+u_z\sigma^2_x}{\sigma^2_z+\sigma^2_x}=u_x+\frac{\sigma^2_x}{\sigma^2_z+\sigma^2_x}(u_z-u_x)........(10)$ $\sigma^2=\frac{\sigma^2_x\sigma^2_z}{\sigma^2_x+\sigma^2_z}=\sigma^2_x(1-\frac{\sigma^2_x}{\sigma^2_x+\sigma^2_z})........(11)$ 现在令
$K=\frac{\sigma^2_x}{\sigma^2_x+\sigma^2_z}........(12)$ 则(10)(11)变成：
$\mu==u_x+K(u_z-u_x)........(13)$ $\sigma^2==\sigma^2_x(1-K)........(14)$ 到这里，请将(13)-(14)与(8)-(9)式对比！标量的情况下，在小车的应用中有：A=1，H=1，正态分布的均值u就是我们要的输出结果，正态分布的方差σz^2就是最小均方误差。推广到矢量的情况，最小均方误差矩阵就是多维正态分布的协方差矩阵。
从(12)式也很容易看到卡尔曼增益K的含义：就是估计量的方差占总方差（包括估计方差和测量方差）的比重。
一切都变得晴朗起来了，然而这一切的一切，却都源自于“估计量和测量量的独立高斯分布”这条假设。进一步总结Kalman滤波器：

假设状态空间的n-1时刻估计值和观测空间的n时刻测量值都满足独立高斯分布，Kalman滤波器就是通过高斯分布的乘积运算将估计值和测量值结合，获得最接近真值的n时刻估计。
高斯分布乘积运算的结果仍为高斯分布，高斯分布的均值对应n时刻的估计值，高斯分布的方差对应n时刻的均方误差。

欢迎光临电子技术论坛_中国专业的电子工程师学习交流社区-中电网技术论坛 (http://bbs.eccn.com/)