卡尔曼滤波器

作为一个本科学控制的，连大名鼎鼎的卡尔曼滤波器都不知道，我也确实是挺惭愧的，一直都想学习，但执行力不够，一直拖拖拖。这几天看其他论文，也看到了很多应用扩展卡尔曼滤波器的例子。所以决定立马开始学习卡尔曼滤波器。在网上找到了很多材料，良莠不齐，下面挑两篇我觉得是讲得最好的文章[1][2]，结合我的理解，希望能够把卡尔曼滤波器讲清楚。

以下内容和截图大多来自文献[1][2][3]。

---

了解卡尔曼滤波器的功能-MATLAB程序仿真

首先，结合一个网上的程序例子，我们看一下卡尔曼滤波器能够干嘛。现在可以只用看结果，知道卡尔曼滤波器能够做什么就好了。待会弄懂了后面的内容，再来看代码就很简单了。代码的注释也都解释得很清楚了。

% KALMANF - updates a system state vector estimate based upon an
%           observation, using a discrete Kalman filter.
%
% Version 1.0, June 30, 2004
%
% This tutorial function was written by Michael C. Kleder
%
% INTRODUCTION
%
% Many people have heard of Kalman filtering, but regard the topic
% as mysterious. While it's true that deriving the Kalman filter and
% proving mathematically that it is "optimal" under a variety of
% circumstances can be rather intense, applying the filter to
% a basic linear system is actually very easy. This Matlab file is
% intended to demonstrate that.
%
% An excellent paper on Kalman filtering at the introductory level,
% without detailing the mathematical underpinnings, is:
% "An Introduction to the Kalman Filter"
% Greg Welch and Gary Bishop, University of North Carolina
% http://www.cs./~welch/kalman/kalmanIntro.html
%
% PURPOSE:
%
% The purpose of each iteration of a Kalman filter is to update
% the estimate of the state vector of a system (and the covariance
% of that vector) based upon the information in a new observation.
% The version of the Kalman filter in this function assumes that
% observations occur at fixed discrete time intervals. Also, this
% function assumes a linear system, meaning that the time evolution
% of the state vector can be calculated by means of a state transition
% matrix.
%
% USAGE:
%
% s = kalmanf(s)
%
% "s" is a "system" struct containing various fields used as input
% and output. The state estimate "x" and its covariance "P" are
% updated by the function. The other fields describe the mechanics
% of the system and are left unchanged. A calling routine may change
% these other fields as needed if state dynamics are time-dependent;
% otherwise, they should be left alone after initial values are set.
% The exceptions are the observation vectro "z" and the input control
% (or forcing function) "u." If there is an input function, then
% "u" should be set to some nonzero value by the calling routine.
%
% SYSTEM DYNAMICS:
%
% The system evolves according to the following difference equations,
% where quantities are further defined below:
%
% x = Ax + Bu + w  meaning the state vector x evolves during one time
%                  step by premultiplying by the "state transition
%                  matrix" A. There is optionally (if nonzero) an input
%                  vector u which affects the state linearly, and this
%                  linear effect on the state is represented by
%                  premultiplying by the "input matrix" B. There is also
%                  gaussian process noise w.
% z = Hx + v       meaning the observation vector z is a linear function
%                  of the state vector, and this linear relationship is
%                  represented by premultiplication by "observation
%                  matrix" H. There is also gaussian measurement
%                  noise v.
% where w ~ N(0,Q) meaning w is gaussian noise with covariance Q
%       v ~ N(0,R) meaning v is gaussian noise with covariance R
%
% VECTOR VARIABLES:
%
% s.x = state vector estimate. In the input struct, this is the
%       "a priori" state estimate (prior to the addition of the
%       information from the new observation). In the output struct,
%       this is the "a posteriori" state estimate (after the new
%       measurement information is included).
% s.z = observation vector
% s.u = input control vector, optional (defaults to zero).
%
% MATRIX VARIABLES:
%
% s.A = state transition matrix (defaults to identity).
% s.P = covariance of the state vector estimate. In the input struct,
%       this is "a priori," and in the output it is "a posteriori."
%       (required unless autoinitializing as described below).
% s.B = input matrix, optional (defaults to zero).
% s.Q = process noise covariance (defaults to zero).
% s.R = measurement noise covariance (required).
% s.H = observation matrix (defaults to identity).
%
% NORMAL OPERATION:
%
% (1) define all state definition fields: A,B,H,Q,R
% (2) define intial state estimate: x,P
% (3) obtain observation and control vectors: z,u
% (4) call the filter to obtain updated state estimate: x,P
% (5) return to step (3) and repeat
%
% INITIALIZATION:
%
% If an initial state estimate is unavailable, it can be obtained
% from the first observation as follows, provided that there are the
% same number of observable variables as state variables. This "auto-
% intitialization" is done automatically if s.x is absent or NaN.
%
% x = inv(H)*z
% P = inv(H)*R*inv(H')
%
% This is mathematically equivalent to setting the initial state estimate
% covariance to infinity.

function s = kalmanf(s)

% set defaults for absent fields:
if ~isfield(s,'x'); s.x=nan*z; end
if ~isfield(s,'P'); s.P=nan; end
if ~isfield(s,'z'); error('Observation vector missing'); end
if ~isfield(s,'u'); s.u=0; end
if ~isfield(s,'A'); s.A=eye(length(x)); end
if ~isfield(s,'B'); s.B=0; end
if ~isfield(s,'Q'); s.Q=zeros(length(x)); end
if ~isfield(s,'R'); error('Observation covariance missing'); end
if ~isfield(s,'H'); s.H=eye(length(x)); end

if isnan(s.x)
   % initialize state estimate from first observation
   if diff(size(s.H))
      error('Observation matrix must be square and invertible for state autointialization.');
   end
   s.x = inv(s.H)*s.z;
   s.P = inv(s.H)*s.R*inv(s.H'); 
else

   % This is the code which implements the discrete Kalman filter:

   % Prediction for state vector and covariance:
   s.x = s.A*s.x + s.B*s.u;
   s.P = s.A * s.P * s.A' + s.Q;

   % Compute Kalman gain factor:
   K = s.P*s.H'*inv(s.H*s.P*s.H'+s.R);

   % Correction based on observation:
   s.x = s.x + K*(s.z-s.H*s.x);
   s.P = s.P - K*s.H*s.P;

   % Note that the desired result, which is an improved estimate
   % of the sytem state vector x and its covariance P, was obtained
   % in only five lines of code, once the system was defined. (That's
   % how simple the discrete Kalman filter is to use.) Later,
   % we'll discuss how to deal with nonlinear systems.

end

return
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153

% Define the system as a constant of 12 volts:
clear all
s.x = 12;
s.A = 1;
% Define a process noise (stdev) of 2 volts as the car operates:
s.Q = 2^2; % variance, hence stdev^2
% Define the voltimeter to measure the voltage itself:
s.H = 1;
% Define a measurement error (stdev) of 2 volts:
s.R = 2^2; % variance, hence stdev^2
% Do not define any system input (control) functions:
s.B = 0;
s.u = 0;
% Do not specify an initial state:
s.x = nan;
s.P = nan;
% Generate random voltages and watch the filter operate.
tru=[]; % truth voltage
for t=1:40
   tru(end+1) = randn*2+12;
   s(end).z = tru(end) + randn*2; % create a measurement
   s(end+1)=kalmanf(s(end)); % perform a Kalman filter iteration
end
%figure
hold on
grid on
% plot measurement data:
hz=plot([s(1:end-1).z],'r.');
% plot a-posteriori state estimates:
hk=plot([s(2:end).x],'b-');
ht=plot(tru,'g-');
legend([hz hk ht],'observations','Kalman output','true voltage',0)
title('Automobile Voltimeter Example')
hold off
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34

这是网上的一个例子，也是文章[2]的作者给出的。下载地址

程序运行结果：

绿色的线表示一个真实的电压值，是我们需要测量的一个值，然后红色的点表示是观测到的值，也就是测量出来的值。哦，顺便说一句，这里讲的卡尔曼滤波器主要是指离散的，因为计算机控制系统只处理离散系统，所以测量值是离散的。我们都知道用测量出来的电压值是有一定误差的，不是真实的电压值，这里假定测量值是真实值加上一个高斯噪声。可以看到红色的点距离真实的电压值偏差很大。那么如何能够从红色的观测点中得到真实的电压值（绿线）呢？卡尔曼滤波器就是干这个的，蓝色的线就是表示的是输入红色的观测值，卡尔曼滤波器输出的估计值。可以看到，蓝色的线还是比较接近绿色的线的。也就是说，卡尔曼滤波器输出的结果还是比较接近真实的情况的。

现实情况中，信号都是都噪声的，所以真实值是永远不可能得到的，所以我们只能通过各种滤波器来得到真实的估计值。卡尔曼滤波器就是干这个的，而且效果惊人！

---

卡尔曼滤波器模型

了解了卡尔曼滤波器的功能后，下满我们就正式开始学习卡尔曼滤波器。学过控制的人应该知道状态变量、状态空间等概念，那么理解下面的式子就很容易了。没学过的也不要紧，卡尔曼滤波器的状态方程比较简单，也一样能看懂。

卡尔曼滤波器模型假设t时刻的状态至于t-1时刻的状态以及t时刻的系统输入有关（有点像马尔科夫链的性质）：

• xt是 t 时刻的状态变量，可能是向量，也就是多个状态变量组成的向量。比如物体的位移，速度，加速度。
• ut是 t 时刻系统的控制输入。比如物体受到的驱动力，摩擦力等。
• Ft是 t 时刻的状态转移矩阵，表示 t-1 时刻的状态对 t 时刻状态的影响。一般是非时变的，即不随时间变化，固定量。
• Bt是 t 时刻的控制输入矩阵，表示 t 时刻的输入对系统状态变量的影响。一般也是非时变得。
• wt是 t 时刻的过程的高斯白噪声，每一个状态变量都对应一个高斯白噪声，均值为0，协方差矩阵为Qt

系统测量部分的模型可以表示为：

• zt表示 t 时刻测量出来的变量矩阵，这里测量出来的变量可能就是状态变量里的变量，也可能不是，但是有一定关系。
• Ht就是状态变换矩阵，表示状态变量到测量的量之间的转换关系。如果测量的量就是状态变量，那么Ht就是单位矩阵了。
• vt表示每一个测量量的测量噪声的矩阵，也是高斯白噪声，均值为0，协方差矩阵为Rt。

一般情况下，系统的真实状态xt是无法被直接观测到的，或者说测量。卡尔曼滤波器就提供了一种算法，把系统状态的预测值和 t 时刻的测量值结合起来得到真实值的估计值x^t，这个估计值能够比较好的近似真实值。这就是卡尔曼滤波器的魅力。

说明一下，我习惯称x^t|t?1为预测值，x^t|t为估计值。x^t|t其实就是校正后的预测值，也是滤波器的输出值。

---

卡尔曼滤波器算法

卡尔曼滤波器是基于高斯概率密度分布的，在整个计算过程中迭代计算x^t。具体来说分为两部分：预测阶段和校正阶段。

阶段一：标准的卡尔曼滤波器的预测阶段方程

• Pt|t?1 是状态变量的协方差矩阵，对角线元素是各个状态变量的方差。下表 t|t-1 表示预测值x^t|t?1和真实值xt之间的协方差矩阵。
• Pt?1|t?1 是指校正后的状态变量（也就是滤波器输出的状态变量值）的协方差矩阵。

具体地说，这里的（3）式就是前面说的（1）的具体表现形式了，通过对t-1时刻的滤波器输出值x^t?1|t?1进行迭代入状态方程（1）中，得到t时刻的状态的一个估计值x^t|t?1。因为估计是一个相对来说确定的过程，要给出具体的值计算，要去掉不确定的高斯部分，所以（3）对比（1）来说没有高斯项。现在，我们有了一个t时刻的估计值了，这个值不够精确，所以还不能够直接使用这个值。

式（4）中的Pt|t?1 定义式如下，表示预测值x^t|t?1和真实值xt之间的协方差矩阵：

具体的推导是这样的，把式（1）减去式（3），得到：

又因为有状态的估计误差和过程的噪声是不相关的，所以中间两项为0，于是得到了上面的式（4）：

阶段二：校正方程

其中的Kt应该是在第二个阶段开始最先计算得到：

暂且先不管公式（5）、（6）、（7）是怎么来的，我们来看看第二个阶段是如何校正的。得到了Kt的值，就可以按照式（5）来计算 t 时刻的滤波器输出值x^t|t，这个式子里面zt是 t 时刻的状态测量值。刚才也说了，测量值也是不准确的，以为有测量噪声在里面。所以现在就有一个估计值x^t|t?1和一个测量值zt，那么到底该相信哪一个呢？卡尔曼滤波器说对这两个值进行综合考虑，具体解释待会再说。可以先记着，综合考虑了估计值和测量值，才得到滤波器的输出值x^t|t作为t时刻的最佳估计值得。

下面这部分可以等清楚了卡尔曼滤波的具体操作后再来看：

式（5）是构造出来的一个式子，t时刻的状态估计值x^t|t等于根据t-1时刻估计出来的值x^t|t?1加上右边一项 Kt（zt?Htx^t|t?1），括号里面表示测量值和估计的测量值之差，也叫测量残差（measurement residual），系数Kt是卡尔曼增益，将测量值得量纲转换到状态量的量纲，和Ht的作用相反，这样说是为了好理解，这两者乘起来并不等于1。这里可以理解为通过测量值和测量值的预测值之间的差来弥补对状态的预测，来接近状态的真实值！
注意，到这里Kt还是一个未决定的量。
卡尔曼增益Kt的选择是想让Pt|t尽可能的小，Pt|t是校正后的预测值(估计值)与真实值的协方差，Pt|t越小，说明越接近真实值。文献[3]中有比较清楚的证明，大致过程就是写出Pt|t关于Kt的显示的式子，然后求极小值。可以通过对KtK求导，令其为0，然后求得Kt，得到的结果就是（7），然后将Kt再代入Pt|t中，就得到了式（6）。这样就得到了（5）、（6）、（7）。

通过式（5）计算出来 t 时刻的最佳估计值之后，t 时刻的滤波器输出就是x^t|t了。然后就开始计算Pt|t，为下一个循环进入到阶段一的迭代计算做准备。就这样逐次迭代，得到了每个时刻的状态变量的最佳估计值。

总结一下，卡尔曼滤波器的计算过程如下：

下面说明如何得到公式（5）、（6）、（7）的！

---

实例： 一维直线运动

问题描述

通过一个大家很熟悉的例子来讲解卡尔曼滤波器，大家也能更好懂了。

假设有一个火车沿着一个铁轨运动（FIG1），我们要观测火车的位置，那么状态变量设为火车的位置xt和速度x˙t，这样的话就有：

火车受到的合外力为ft，火车质量为m，那么输入控制矩阵ut=ftm，在时间间隔△t（也就是在时刻t-1和t之间的时间）内，状态变量变为如下：

写成矩阵的形式：

与式（1）进行比较，可以得到：

解答问题

如上图FIG2所示，初始时刻t=0时，假设火车的位置的估计值已经知道了，并且估计的置信度的概率密度分布如红色的高斯分布所示。那么t=1s时火车的位置是怎么样的呢？

如上图FIG3所示，假设我们对t=1s时火车的位置进行了预测，置信度仍然是高斯分布，不过高斯分布的方差变大了，高度变小了，表明不确定下降了，因为从t=0到t=1s中间经历了什么过程我们不清楚，只是一个粗略的估计值。

如上图FIG4所示，这时我们又有一个对于t=1时刻的位置的测量值，蓝色线下表示的高斯分布表示测量的置信度。那么现在问题就来了？t=1s时刻的位置是相信红色的预测还是蓝色的测量呢？

如上图FIG5所示，红色和蓝色的高斯分布的相乘得到了一个绿色线表示的高斯分布！这个绿色高斯分布就是一个综合了预测值和测量值的最佳估计。

等等，红色的高斯和绿色的高斯相乘以后就一定会使高斯吗？这里就用了就高斯函数的一个重要的性质：高斯函数乘以高斯函数，结果是一个新的高斯函数！这时高斯函数一个很重要的性质，在图像处理中，二维高斯函数的这个性质用得很多，比如大名鼎鼎的SIFT中的高斯金字塔的构造过程中就是用到了这个性质，通过对经过高斯平滑的图片再进行高斯卷积，等效于用一个大的高斯核对原图像进行卷积。这样就可以减少计算量！

预测值的红色的高斯分布可以表示为：

测量值的蓝色的高斯分布可以表示为：

预测和测量融合之后，得到的高斯函数为：

这里强调一下，注意前面我一直都是说的高斯函数，高斯函数乘以高斯函数，结果是一个新的高斯函数！，我并没有说是高斯分布，因为高斯分布乘以高斯分布，结果只是一个新的高斯函数，而不是高斯分布！因为不满足积分为1的条件！但是，只相差一个常数因子！所以我们只要从上式中提取出高斯分布的均值和方差就可以了！

提去均值和方差，得到了融合之后的高斯分布！读者可以自己验证一下，把式（10）中指数部分合起来就得到了。那么现在我们就得到了FIG5中的最佳估计的置信度的高斯分布了。

问题还没有完，上面的情况是考虑测量的量和估计的量是同一个量纲，具体说就是测量的也是位置（估计值是火车位置）。但是现实中很多问题不是同一个量纲，需要把估计值转换到和测量值同一个量纲，比如上例子测量的可能是火车接收到从x=0处发射的电磁波信号所需时间（即电磁波在空气中运动时间），那么就需要把估计的距离除以光速c，这样估计和测量就是同一个量纲了！

然后改写上面的式（8）、（9）：

融合后的高斯分布的均值和方差也要改写：


具体地，对应到上面的例子中，就会有H=1c，然后令K=Hσ21Hσ21+σ22，代入到式（16）中，就得到了式（17）。同样，可以得到融合之后估计的方差式（18）。

与卡尔曼滤波器方程中的矩阵和向量进行对比

为了说明上面的例子和卡尔曼滤波器是如何联系的，下面就对比一下上面例子中的一些变量，通过对比可以发现，上面例子所述就是一个卡尔曼滤波器。

从上往下依次是：

• x^t|t融合之后的状态变量（也就是滤波器输出的最佳估计值）
• x^t|t?1状态的预测值，数据融合之前的状态变量
• Pt|t数据融合之后的置信度的协方差矩阵
• Pt|t?1数据融合之前的置信度的协方差矩阵
• zt测量时的噪声带来的不确定性矩阵
• Ht变换矩阵，把估计值转换为测量值的量纲
• Kt卡尔曼增益

这里可以看出来（17）（18）和标准的卡尔曼方程的关系。从这里可以看到公式（6）是怎么来的了。当然这不是严格的证明过程。

---

Kt的选择以及校正方程(5)、(6)、(7)的推导

看完前面部分关于卡尔曼滤波器的讲解，相信你应该能够很轻松的读懂了最前面的matlab代码了。是不是觉得卡尔曼滤波真的很神奇！那卡尔曼滤波器到底是怎么工作的呢？前面一直没讲的那部分Kt是怎么得到的呢？然后下面我将得到Kt的过程推导一遍，主要是参考[3]中的内容。

为了方便将式(5)、(6)、(7)再贴一遍。式（5）是构造出来的，卡尔曼增益Kt的选择是想让Pt|t尽可能的小，这样预测的结果就会尽可能接近真实值。
回忆上面的Pt|t?1：

同样对于 Pt|t=E[(xt?x^t|t)(xt?x^t|t)T]，把式（1）减去（5）得到：
Pt|t=E[(xt?x^t|t?1?Kt（zt?Htx^t|t?1）)(xt?x^t|t?1?Kt（zt?Htx^t|t?1）)T]
Pt|t=E{[(I?KtHt)(xt?x^t|t?1)?Ktvt][(I?KtHt)(xt?x^t|t?1)?Ktvt]T} ——————（20）
其中的vt是因为把式（2）代入上面式子中的zt引入的。
把式（20）拆开，同样因为测量噪声vt与其他项无关，得到：
Pt|t=(I?KtHt)E[(xt?x^t|t?1)(xt?x^t|t?1)T](I?KtHt)T+KtE（vtvTt）KTt
Pt|t=(I?KtHt)Pt|t?1(xt?x^t|t?1)T](I?KtHt)T+KtRtKTt ——————（21）

因为Kt，Rt，Ht都是固定值，下面为了简洁就去掉下标，右边的Pt|t?1简写为P，拆开（21）得到：
Pt|t=P?KHP?PHTKT+K（HPHT+R）KT
把K当变量看待，可以把Pt|t看作是K的二次函数，有极小值。这个极小情况所对应的K就是我们要求的，因为这时的协方差误差最小，估计值最接近真实值。[3]中为了避免矩阵求导操作，使用了另外一种方式证明。我将利用矩阵求导来证明，不熟悉矩阵求导的同学可以自己查阅相关资料，我将直接给出矩阵求导结果：
?Pt|t?K=?PTHT?PHT+K(HPTHT+RT)+K(HPHT+R)
这里，P、R都是协方差矩阵，是对称的，所以P=PT、R=RT，进一步得到：
?Pt|t?K=?2PHT+2K(HPHT+R)=0
求得K=PHT(HPHT+R)?1 ——————（22）
这就是我们前面直接用的式（7）！终于知道是怎么来的。^_^

下面继续推导出式（6）：
把（22）稍微变形PHT=K(HPHT+R)，再转置HPT=HP=(HPHT+R)TKT=(HPHT+R)KT ——————（23）

注意，这里的(HPHT+R)是对称矩阵，代入到Pt|t的表达式中，得到：
Pt|t=P?KHP?PHTKT+K（HPHT+R）KT
Pt|t=P?K(HPHT+R)KT?PHTKT+K（HPHT+R）KT
Pt|t=P?PHTKT
两边转置，得到:
Pt|t=P?KHP ——————（24）
这样也就得到了式（6）。终于都推导完了。。

---

卡尔曼滤波器工作原理解释

这里，再把上面的图贴出来看，我将结合自己学习卡尔曼滤波器的心得来尝试解释一下为什么卡尔曼滤波器能够这么神奇的工作。当然我不保证解释是对的，大家不认同的话可以提出来讨论。

这里图中的符号和前面使用的符号有一定出入，不过如果你看懂了前面部分的内容的话肯定不会难到你了。

卡尔曼滤波器工作分为两个阶段，一个是预测（predict，也叫time update），一个是校正（correct，也叫measurement update）。
初始时，给出一个估计的状态值x^k?1和对应的协方差矩阵的值pk?1（真实值xk?1和估计值x^k?1之差的协方差矩阵），然后进入阶段一中的计算，得到状态预测值x^?k和对应的协方差矩阵p?k(真实值xk和预测值x^?k之差的协方差矩阵)。
接着进入到阶段二中，这里将根据测量值对预测值进行校正。这里是卡尔曼滤波器的关键。如前面所述，通过构造得到式（5），把测量值和预测值同时考虑进来得到最终的估计值，在这个过程中，为了让估计值尽可能接近真实值，就要让协方差矩阵（真实值和估计值之差的协方差矩阵）尽可能地小，越小说明两者越接近！根据计算得到这样时的K应该为式（7），也就是上图中校正步骤中的第一步，得到了K之后就校正阶段一中的预测值，得到优化过后的估计值！这个估计是结合当前已知信息是最优的估计值！这就是卡尔曼滤波器之所以能很好的工作的原因。同时再跟新协方差矩阵。然后再在下一个时刻进入到步骤一中，迭代计算。

---

卡尔曼滤波器的应用

在实际应用中，卡尔曼滤波器需要知道系统的状态方程，也就是要能够对系统进行数学上的建模，得到F、B、H等参数，这样才能使用卡尔曼滤波器。
在调试卡尔曼滤波器时，最重要的两个参数是过程噪声和测量噪声的方差。测量噪声的方差可以根据通过实验，给定真实值对测量值进行统计得到，但是过程噪声的方差不能直接得到。（可能是跟PID一样试凑吧）提前调整好卡尔曼滤波器的这些参数，以后系统工作的时候就可以正常使用了。

---

参考资料

[1] Ramsey Faragher，Understanding the Basis of the Kalman Filter Via a Simple and Intuitive Derivation
[2] Greg Welch and Gary Bishop，An Introduction to the Kalman Filter
[3] Brown, R. G. and P. Y. C. Hwang. 1992. Introduction to Random Signals and Applied Kalman Filtering, Second Edition