0. 前言
之前关于卡尔曼滤波看了很多博客和论文，开始总有一种晕圈的感觉，因为很多文章都是上来给你摆出来几个数学公式，告诉你哪几个公式是预测的，哪几个公式是更新的，然后就稀里糊涂地仿个真让你看下估计量准不准。
一开始做项目的时候并没有关心其中的因为所以，直接就拿来用了，现在又面临着毕设和考研复试都和卡尔曼滤波相关，所以最近就用了几天时间通过各种渠道反复思考，终于对其中的原理、因果和来源有一个深入的了解。
今天就以通俗易懂的方式从“最小均方误差准则”的角度写一篇自己对卡尔曼滤波的较为完整的见解，也希望能够给初学者提供一些帮助，如有错误，欢迎批评指正。
文章目录
1. 卡尔曼滤波简介
Kalman滤波是一种线性、无偏、最小均方误差估计算法，是最优估计方法的一种。
“滤波”二字就是在测量目标信号的基础上，采取最优算法，对目标的状态进行精确估计。
所谓最小均方误差准则，是要求目标的状态估计值与真实值误差的方差达到最小值。
之所以称为“估计”，是因为实际系统中必然有各种随机噪声的存在，这就造成无论是状态的线性转移推导还是利用传感器观测实际数据都会存在误差，如果没有这些随机噪声，那就是确定性问题的求解，不存在“估计”问题。
2. 为什么要用卡尔曼滤波
下面举个最常见的例子：
假设我们要研究的对象是一个房间的温度。根据你的经验判断，这个房间的温度是恒定的， 也就是现在这一分钟的温度等于过去一分钟的温度（假设我们用一分钟来做时间单位）（先验估计） 。假设你对你的经验不是 100% 的相信，可能会有上下偏差几度。我们把这些偏差看成是高斯白噪声，也就是这些偏差跟前后时间是没有关系的而且符合高斯分布。另外，我们在房间里放一个温度计，但是这个温度计也不准确的， 测量值和实际值有偏差，我们也把这些偏差看成是高斯白噪声。好了， 现在对于某一分钟我们有两个有关于该房间的温度值： 你根据经验的预测值 （系统的 预测值）和温度计的值（测量值）。
Kalman要解决的问题是如何使用这两个值结合他们各自的噪声来估算出房间的实际温度值。
假如我们要估算k时刻的实际温度值.首先你要根据k-1时刻的温度值,来预测k时刻的温度.因为你相信温度是恒定的,所以你会得到k时刻的温度预测值是跟k-1时刻一样的,假定是23度,同时该值的高斯噪声的偏差是5度[5是这样得到的：如果k-1时刻最优估计误差为3,你对自己预测噪声标准差是4度,他们平方和再开方,就是5）.至于为何是平方和，可以看做两个高斯过程相加[上次最优估计结果是个高斯过程，这次预测也是高斯过程]，所得的也是高斯过程，方差为原先两者的方差之和）]
然后，你从温度计那里得到了 k 时刻的温度值，假设是 25 度，同时该值的噪声标准差是 4 度。 由于我们用于估算 k 时刻的实际温度有两个温度值， 分别是 23 度和 25 度。 究竟实际温度是多少呢？相信自己还是相信温度计呢？究竟相信谁多一点， 我们可以上次的估计值的噪声方差及上次的最优估计方差总和之比判断。算出比例因子Kg: Kg^2 = 5^2 + 4^2) ，所以 Kg=0.78 ，我们可以估算出 k 时刻的实际温度值（最优估计）是： 23+0.78*(25-23)=24.56 度[估计值+Kg*(测量值-估计值)].可以看出,因为温度计的协方差比较小（比较相信温度计）,所以估算出的最优温度值偏向温度计的值。
现在我们已经得到 k 时刻的最优温度值了，下一步就是要进入 k+1 时刻，进行新的最优估算。到现在为止，好像还没看到什么递归的东西出现。对了，在进入 k+1 时刻之前，我们还要算出 k 时刻那个最优值（ 24.56 度）的噪声标准差。算法如下： ((1-Kg)*5^2) ^0.5 = 2.35 。这里 的 5 就是上面的k 时刻你预测的那个 23 度温度值的标准差，得出的 2.35 就是进入 k+1 时刻以 后 k 时刻估算出的最优温度值的标准（对应于上面的 3 ） 。 就是这样，卡尔曼滤波器就不断递归，从而估算出最优的温度值。他运行的 很快， 而且它只保留了上一时刻的 最优估计误差标准差。 上面的Kg ， 就是卡尔曼增益 （ Kalman Gain ） 。 他可以随不同的时刻而改变他自己的值！
3. 卡尔曼滤波的核心思想
以k-1时刻的最优估计 x k − 1 ∧ \overset{\wedge }{\mathop{{{x}_{k-1}}}}\, xk−1∧为准，预测k时刻的状态向量，得到预测量 x k − ∧ \overset{\wedge }{\mathop{{{x}_{k}^{-}}}}\, xk−∧；同时又对k时刻的状态进行观测，得到观测向量 z k {{z}_{k}} zk，再以观测量对预测量进行修正，从而得到k时刻的最优状态估计 x k ∧ \overset{\wedge }{\mathop{{{x}_{k}}}}\, xk∧。
4. 预先的数学知识准备
（1）先验估计：先验状态估计是根据系统过程原理或者经验得到进行的预测。
（2）后验估计：后验状态估计是结合之前的先验状态估计值，再加权观测值得到的估计结果。
（3）先验估计误差：先验估计量（即预测量）
x
k
−
∧
\overset{\wedge }{\mathop{{{x}_{k}}^{-}}}\,
xk−∧和真实状态量
x
k
{{x}_{k}}
xk之间的误差：
e
k
−
=
x
k
−
x
k
−
∧
e_{k}^{-}={{x}_{k}}-\overset{\wedge }{\mathop{{{x}_{k}}^{-}}}\,
ek−=xk−xk−∧。
（4）后验估计误差：后验估计量（即最优估计量）
x
k
∧
\overset{\wedge }{\mathop{{{x}_{k}}}}\,
xk∧和真实状态量
x
k
{{x}_{k}}
xk之间的误差：
e
k
=
x
k
−
x
k
∧
{{e}_{k}}={{x}_{k}}-\overset{\wedge }{\mathop{{{x}_{k}}}}\,
ek=xk−xk∧。
（5）均方误差：它是"误差"的平方的期望值（误差就是每个估计值与真实值的差），也就是多个样本的时候，均方误差等于每个样本的误差平方再乘以该样本出现的概率的和。
（6）协方差（矩阵）：
协方差
C
o
v
(
X
,
Y
)
Cov(X,Y)
Cov(X,Y)用于衡量两个变量X与Y之间的总体误差，协方差的绝对值越大，则两个变量相互影响越大；
协方差矩阵
C
=
[
∑
11
⋯
∑
1
j
⋯
∑
1
n
⋮
⋱
⋮
⋱
⋮
∑
i
1
⋯
∑
i
j
⋯
∑
i
n
⋮
⋱
⋮
⋱
⋮
∑
n
1
⋯
∑
n
j
⋯
∑
n
n
]
C\text{=}\left[ \begin{matrix} {{\sum }_{11}} & \cdots & {{\sum }_{1j}} & \cdots & {{\sum }_{1n}} \\ \vdots & \ddots & \vdots & \ddots & \vdots \\ {{\sum }_{i1}} & \cdots & {{\sum }_{ij}} & \cdots & {{\sum }_{in}} \\ \vdots & \ddots & \vdots & \ddots & \vdots \\ {{\sum }_{n1}} & \cdots & {{\sum }_{nj}} & \cdots & {{\sum }_{nn}} \\ \end{matrix} \right]
C=⎣⎢⎢⎢⎢⎢⎢⎡∑11⋮∑i1⋮∑n1⋯⋱⋯⋱⋯∑1j⋮∑ij⋮∑nj⋯⋱⋯⋱⋯∑1n⋮∑in⋮∑nn⎦⎥⎥⎥⎥⎥⎥⎤中的每个元素
∑
i
j
{{\sum }_{ij}}
∑ij是向量中第
i
i
i个变量元素和第
j
j
j个变量元素两个随机变量之间的协方差，衡量它们之间的线性相关性，那么协方差矩阵
C
C
C则表示一组随机变量之间的两两线性相关性，用于衡量多个变量的总体误差。
Notes：
<1>对于线性系统中的状态信息，一般不只包含一个变量（如小车运动时的状态信息一般为位置p和速度v），那么则以状态向量的形式表示，故该状态向量的协方差矩阵是衡量向量内各变量元素的两两线性相关性。
<2>协方差矩阵的主对角线元素分别为每个变量的方差。方差是协方差的一种特殊情况，即当两个变量是相同的情况，描述的是样本集合的各个样本点到均值的距离之平均；
<3>卡尔曼滤波中共涉及三个协方差矩阵：
状态协方差矩阵（误差协方差矩阵）
P
k
{{P}_{k}}
Pk，过程噪声协方差矩阵
Q
k
{{Q}_{k}}
Qk，测量噪声协方差矩阵
R
k
{{R}_{k}}
Rk；
<4>由协方差的定义及期望的性质，有：
①[X]和[Y]均为一维随机变量时：
协方差：
C
o
v
(
X
,
Y
)
=
E
{
[
X
−
E
(
X
)
]
[
Y
−
E
(
Y
)
]
}
=
E
{
X
Y
−
X
E
(
Y
)
−
Y
E
(
X
)
−
E
(
X
)
E
(
Y
)
}
=
E
(
X
Y
)
−
E
(
X
)
E
(
Y
)
−
E
(
Y
)
E
(
X
)
+
E
(
X
)
E
(
Y
)
=
E
(
X
Y
)
−
E
(
X
)
E
(
Y
)
\begin{aligned} & Cov(X,Y)=E\left\{ \left[ X-E(X) \right]\left[ Y-E(Y) \right] \right\} \\ & =E\left\{ XY-XE(Y)-YE(X)-E(X)E(Y) \right\} \\ & =E(XY)-E(X)E(Y)-E(Y)E(X)+E(X)E(Y) \\ & =E(XY)-E(X)E(Y) \\ \end{aligned}
Cov(X,Y)=E{[X−E(X)][Y−E(Y)]}=E{XY−XE(Y)−YE(X)−E(X)E(Y)}=E(XY)−E(X)E(Y)−E(Y)E(X)+E(X)E(Y)=E(XY)−E(X)E(Y)
方差： D ( X ) = C o v ( X , X ) = E { [ X − E ( X ) ] [ X − E ( X ) ] } = E { [ X − E ( X ) ] 2 } D(X)=Cov(X,X)=E\left\{ \left[ X-E(X) \right]\left[ X-E(X) \right] \right\}=E\left\{ {{\left[ X-E(X) \right]}^{2}} \right\} D(X)=Cov(X,X)=E{[X−E(X)][X−E(X)]}=E{[X−E(X)]2}
②[X]为n维向量时：
协方差矩阵：
C
=E
{
[
X
−
E
(
X
)
]
[
X
−
E
(
X
)
]
T
}
=
[
∑
11
⋯
∑
1
j
⋯
∑
1
n
⋮
⋱
⋮
⋱
⋮
∑
i
1
⋯
∑
i
j
⋯
∑
i
n
⋮
⋱
⋮
⋱
⋮
∑
n
1
⋯
∑
n
j
⋯
∑
n
n
]
\begin{aligned} C&\text{=E}\left\{ \left[ X-E(X) \right]{{\left[ X-E(X) \right]}^{T}} \right\}\\ &\text{=}\left[ \begin{matrix} {{\sum }_{11}} & \cdots & {{\sum }_{1j}} & \cdots & {{\sum }_{1n}} \\ \vdots & \ddots & \vdots & \ddots & \vdots \\ {{\sum }_{i1}} & \cdots & {{\sum }_{ij}} & \cdots & {{\sum }_{in}} \\ \vdots & \ddots & \vdots & \ddots & \vdots \\ {{\sum }_{n1}} & \cdots & {{\sum }_{nj}} & \cdots & {{\sum }_{nn}} \\ \end{matrix} \right]\end{aligned}
C=E{[X−E(X)][X−E(X)]T}=⎣⎢⎢⎢⎢⎢⎢⎡∑11⋮∑i1⋮∑n1⋯⋱⋯⋱⋯∑1j⋮∑ij⋮∑nj⋯⋱⋯⋱⋯∑1n⋮∑in⋮∑nn⎦⎥⎥⎥⎥⎥⎥⎤
协方差矩阵的性质：
若
X
X
X的协方差矩阵为
C
=E
{
[
X
−
E
(
X
)
]
[
X
−
E
(
X
)
]
T
}
C\text{=E}\left\{ \left[ X-E(X) \right]{{\left[ X-E(X) \right]}^{T}} \right\}
C=E{[X−E(X)][X−E(X)]T}
则
A
X
AX
AX的协方差矩阵为
C
′
=E
{
[
A
X
−
E
(
A
X
)
]
[
A
X
−
E
(
A
X
)
]
T
}
=E
{
A
[
X
−
E
(
X
)
]
[
X
−
E
(
X
)
]
T
A
T
}
=
A
E
{
[
X
−
E
(
X
)
]
[
X
−
E
(
X
)
]
T
}
A
T
=
A
C
T
A
\begin{aligned} & C'\text{=E}\left\{ \left[ AX-E(AX) \right]{{\left[ AX-E(AX) \right]}^{T}} \right\} \\ & \text{=E}\left\{ A\left[ X-E(X) \right]{{\left[ X-E(X) \right]}^{T}}{{A}^{T}} \right\}\\ &\text{=}A\text{E}\left\{ \left[ X-E(X) \right]{{\left[ X-E(X) \right]}^{T}} \right\}{{A}^{T}} \\ & \text{=}A{{C}^{T}}A \\ \end{aligned}
C′=E{[AX−E(AX)][AX−E(AX)]T}=E{A[X−E(X)][X−E(X)]TAT}=AE{[X−E(X)][X−E(X)]T}AT=ACTA
5. 线性系统的方程描述
在线性离散时间系统中，目标运动的状态（转移）方程和观测方程分别可以表示为：
x k = A k x k − 1 + B k u k + w k (1) {{x}_{k}}={{A}_{k}}{{x}_{k-1}}+{{B}_{k}}{{u}_{k}}+{{w}_{k}}{\tag{1}} xk=Akxk−1+Bkuk+wk(1)
z k = H x k + v k (2) {{z}_{k}}=H{{x}_{k}}+{{v}_{k}}{\tag{2}} zk=Hxk+vk(2)
注意：一定要清楚这两个方程只是用来描述动态线性系统的，不要和后面卡尔曼预测方程混淆，并且关于这两个方程的由来在下面第6节 我也会介绍。
第一遍看的时候你只需要清楚每个变量叫什么就可以，看完下面第6节的介绍再返回来仔细看这两个公式及各字母的含义。
上述两式中，
x
k
{{x}_{k}}
xk是k时刻的系统状态向量(M维)；
A k {{A}_{k}} Ak为状态转移矩阵（M阶方阵），描述动态系统在时刻k-1的状态到时刻k的状态之间的转移，应为已知；
B k {{B}_{k}} Bk为k时刻影响控制量 u k {{u}_{k}} uk的控制矩阵（M阶方阵）；
u
k
{{u}_{k}}
uk是k时刻对系统的控制量(M维)，
B
k
u
k
{{B}_{k}}{{u}_{k}}
Bkuk和
A
k
x
k
−
1
{{A}_{k}}{{x}_{k-1}}
Akxk−1之和用来描述由k-1时刻到k时刻状态的线性关系；
（例如一元二次方程
y
=
a
x
+
b
y=ax+b
y=ax+b，
a
a
a相当于状态转移矩阵
A
k
{{A}_{k}}
Ak，
b
b
b相当于
B
k
u
k
{{B}_{k}}{{u}_{k}}
Bkuk，
y
y
y和
x
x
x则分别相当于
x
k
{{x}_{k}}
xk和
x
k
−
1
{{x}_{k-1}}
xk−1）；
（很多关于卡尔曼滤波的文章都直接一句“很多情况下 u k {{u}_{k}} uk默认为0”带过，压根没有告诉你这是个什么东西，而实际上 u k {{u}_{k}} uk完全是根据实际需要赋予的值，比如小车加速、飞机拐弯等这些由目标自身改变的动作，而不是与先前的运动保持类似的动作这种。看完下面第6节关于方程由来的介绍你就会明白为什么我这么说。）
z
k
{{z}_{k}}
zk是k时刻的观测向量(N维)；
（有人会问：为什么观测向量和状态向量不是同维度的？——因为状态向量
x
k
−
1
{{x}_{k-1}}
xk−1中的各变量元素不一定都是直接可观测的呀，就比如小车的例子，观测出来位置容易但很难观测出来速度（也可能是通过观测时间得到位置和速度），因此观测向量和状态向量之间要有一个转换，而完成这个转换就是靠观测矩阵H）；
H为观测矩阵(N×M)，应为已知，它把M维状态变量转换到与N维观测量相对应；
（谁转换到谁这句话反过来说也可以，总之就是把状态向量和观测向量对应起来）；
w k {{w}_{k}} wk表示过程噪声向量(M维)，它描述理论状态转移与实际过程之间的误差；
v k {{v}_{k}} vk表示观测噪声向量(N维)，它描述观测不准带来的误差；
w k {{w}_{k}} wk和 v k {{v}_{k}} vk互不相关，并均视为零均值高斯白噪声，他们的协方差矩阵分别是 Q k {{Q}_{k}} Qk， R k {{R}_{k}} Rk；
Q k {{Q}_{k}} Qk：表示过程噪声 w k {{w}_{k}} wk的协方差矩阵（协方差矩阵请参考第3节的数学知识）；
R
k
{{R}_{k}}
Rk：表示观测噪声
v
k
{{v}_{k}}
vk的协方差矩阵。
为此，就有动态线性系统模型如下：
6. 线性系统方程的由来
6.1 状态方程(1)的由来
为便于理解，以小车的运动模型为例：
x
=
[
p
v
]
x=\left[ \begin{matrix} p \\ v \\ \end{matrix} \right]
x=[pv]：含有噪声的状态向量
x
x
x，内含信息有位置
p
p
p（position）和速度
v
v
v（velocity）；
P
k
=
[
∑
p
p
∑
p
v
∑
v
p
∑
v
v
]
{{P}_{k}}=\left[ \begin{matrix} \sum{_{pp}} & \sum{_{pv}} \\ \sum{_{vp}} & \sum{_{vv}} \\ \end{matrix} \right]
Pk=[∑pp∑vp∑pv∑vv]状态
x
x
x的协方差矩阵。
假设状态
x
x
x的分布如下：
中心点为状态的期望中值
μ
\mu
μ （含每个变量的期望），此处的概率密度最大。
Note: 本图例是
p
p
p和
v
v
v成正相关（真实系统可能正相关或负相关或不相关），二者的协方差大于0，则状态向量
x
x
x的协方差矩阵的各元素均大于0
（关于协方差的意义，可参考如下链接：协方差的意义）
方程推导：
（1）理想状态下，考虑CV和CA两种运动状态，如下：
① 当小车做CV匀速运动时，
p
k
=
p
k
−
1
+
v
k
−
1
∗
△
t
{{p}_{k}}={{p}_{k-1}}+{{v}_{k-1}}*\vartriangle t
pk=pk−1+vk−1∗△t
v
k
=
v
k
−
1
{{v}_{k}}={{v}_{k-1}}
vk=vk−1
写成矩阵的形式就是：
x
k
=
[
p
k
v
k
]
=
[
p
k
−
1
+
v
k
−
1
∗
△
t
v
k
−
1
]
=
[
1
△
t
0
1
]
[
p
k
−
1
v
k
−
1
]
=
A
k
x
k
−
1
\begin{aligned} {{x}_{k}}&=\left[ \begin{matrix} {{p}_{k}} \\ {{v}_{k}} \\ \end{matrix} \right]=\left[ \begin{matrix}\\ {{p}_{k-1}}+{{v}_{k-1}}*\vartriangle t \\ {{v}_{k-1}} \\ \end{matrix} \right]\\ &=\left[ \begin{matrix} 1 & \vartriangle t \\ 0 & 1 \\ \end{matrix} \right]\left[ \begin{matrix}\\ {{p}_{k-1}} \\ {{v}_{k-1}} \\ \end{matrix} \right]\\ &={{A}_{k}}{{x}_{k-1}} \end{aligned}
xk=[pkvk]=⎣⎡pk−1+vk−1∗△tvk−1⎦⎤=[10△t1]⎣⎡pk−1vk−1⎦⎤=Akxk−1
② 当小车做CA匀加速运动时，
p
k
=
p
k
−
1
+
v
k
−
1
∗
△
t
+
1
2
a
t
2
{{p}_{k}}={{p}_{k-1}}+{{v}_{k-1}}*\vartriangle t\text{+}\frac{1}{2}a{{t}^{2}}
pk=pk−1+vk−1∗△t+21at2
v
k
=
v
k
−
1
+
a
∗
△
t
{{v}_{k}}={{v}_{k-1}}+a*\vartriangle t
vk=vk−1+a∗△t
写成矩阵的形式就是：
x
k
=
[
p
k
v
k
]
=
[
p
k
−
1
+
v
k
−
1
∗
△
t
+
1
2
a
∗
△
t
2
v
k
−
1
+
a
∗
△
t
]
=
[
p
k
−
1
+
v
k
−
1
∗
△
t
v
k
−
1
]
+
[
1
2
a
∗
△
t
2
a
∗
△
t
]
=
[
1
△
t
0
1
]
[
p
k
−
1
v
k
−
1
]
+
[
1
2
△
t
2
△
t
]
a
=
A
k
x
k
−
1
+
B
k
u
k
\begin{aligned} {{x}_{k}}&=\left[ \begin{matrix} {{p}_{k}} \\ {{v}_{k}} \\ \end{matrix} \right]=\left[ \begin{matrix} {{p}_{k-1}}+{{v}_{k-1}}*\vartriangle t+\frac{1}{2}a*\vartriangle {{t}^{2}} \\ {{v}_{k-1}}+a*\vartriangle t \\ \end{matrix} \right]\\ &=\left[ \begin{matrix} {{p}_{k-1}}+{{v}_{k-1}}*\vartriangle t \\ {{v}_{k-1}} \\ \end{matrix} \right]+\left[ \begin{matrix} \frac{1}{2}a*\vartriangle {{t}^{2}} \\ a*\vartriangle t \\ \end{matrix} \right]\\ &=\left[ \begin{matrix} 1 & \vartriangle t \\ 0 & 1 \\ \end{matrix} \right]\left[ \begin{matrix} {{p}_{k-1}} \\ {{v}_{k-1}} \\ \end{matrix} \right]+\left[ \begin{matrix} \frac{1}{2}\vartriangle {{t}^{2}} \\ \vartriangle t \\ \end{matrix} \right]a\\ &={{A}_{k}}{{x}_{k-1}}+{{B}_{k}}{{u}_{k}} \end{aligned}
xk=[pkvk]=[pk−1+vk−1∗△t+21a∗△t2vk−1+a∗△t]=[pk−1+vk−1∗△tvk−1]+[21a∗△t2a∗△t]=[10△t1][pk−1vk−1]+[21△t2△t]a=Akxk−1+Bkuk
（2）考虑噪声问题，上述公式就转换如下：
① 有噪声的CV运动下
x
k
=
A
k
x
k
−
1
+
w
k
{{x}_{k}}={{A}_{k}}{{x}_{k-1}}+{{w}_{k}}
xk=Akxk−1+wk
② 有噪声的CA运动下
x
k
=
A
k
x
k
−
1
+
B
k
u
k
+
w
k
{{x}_{k}}={{A}_{k}}{{x}_{k-1}}+{{B}_{k}}{{u}_{k}}+{{w}_{k}}
xk=Akxk−1+Bkuk+wk
****因此，用
x
k
=
A
k
x
k
−
1
+
B
k
u
k
+
w
k
{{x}_{k}}={{A}_{k}}{{x}_{k-1}}+{{B}_{k}}{{u}_{k}}+{{w}_{k}}
xk=Akxk−1+Bkuk+wk
来作为线性系统状态转移的普适方程。
6.2 观测方程(2)的由来
上面说到，状态向量 x k {{x}_{k}} xk中的各变量元素不一定都是直接可观测的，就比如小车的例子，观测出来位置容易但很难观测出来速度（也可能通过观测时间来得到位置和速度）。
但是它们之间存在着一定的数学关系，因此观测向量和状态向量之间要有一个转换，而完成这个转换就是靠观测矩阵H。
在实际工程中，我们有时可以通过传感器直接观测得到需要的变量数据，但是有时测量值的噪声太大了，根本不能直接用它来进行计算（试想一个本来是朝着一个方向做匀加速运动的小车，由于噪声太大，你可能测量出来的位置确是前后移动），那么在仿真的时候一般都是使用观测方程来得到观测值，添加噪声 v k {{v}_{k}} vk是用来模拟观测时的误差。
****因此，用
z
k
=
H
x
k
+
v
k
{z_k} = H{x_{k}} + {v_k}
zk=Hxk+vk
来作为线性系统的观测方程。
Note： 此处一定要注意所说的观测量和状态量之间为何需要转换——因为状态向量
x
k
{{x}_{k}}
xk中的各变量元素不一定都是直接可观测的。后面用状态预测量和观测量结合得到最优估计量，这就必须要建立结合双方要对应的前提下。
两个量的对应就是靠观测矩阵实现的，
H
x
k
H{{x}_{k}}
Hxk就将状态预测量转换到可以直接和观测量
z
k
{{z}_{k}}
zk进行加权结合的程度。
7.卡尔曼滤波预测和更新模型
7.1 预测模型
7.1.1 先验预测状态量 x k - ∧ \overset{\wedge }{\mathop{{{x}_{k}}^{\text{-}}}}\, xk-∧
x k - ∧ = A k x k − 1 ∧ + B k u k (3) \overset{\wedge }{\mathop{{{x}_{k}}^{\text{-}}}}\,={{A}_{k}}\overset{\wedge }{\mathop{{{x}_{k-1}}}}\,+{{B}_{k}}{{u}_{k}}\tag{3} xk-∧=Akxk−1∧+Bkuk(3)
其中，
x
k
-
∧
\overset{\wedge }{\mathop{{{x}_{k}}^{\text{-}}}}\,
xk-∧：k时刻的先验预测状态向量，是仅通过状态预测模型得到的一步预测量（右上角“-”代表“先验值”）；
x k − 1 ∧ \overset{\wedge }{\mathop{{{x}_{k-1}}}}\, xk−1∧：k-1时刻的后验估计状态向量（也即最优状态估计量，它是利用先验预测量和观测量进行加权计算得到的最优估计）；
Note： 大部分文章直接给出这个方程就完事了，初学者在看到这个状态预测方程时会觉得怎么这么熟悉，没错，这个方程就是根据上面所说的系统状态方程公式(1)得到的，但是对比发现为什么这俩方程还不一样啊，状态预测方程为什么没有像公式(1)那样添加噪声了呢？（刚开始那阵真是懵的我头皮发麻，问天天不应问地地不灵，那么多文章几乎都没有解释这一点，就扔给你个公式震住你，管你十万个为什么）
现在我来告诉你！
卡尔曼滤波的状态预测方程不需要像系统状态方程中那样加 w k {{w}_{k}} wk这样的噪声。
原因： 卡尔曼滤波核心就是要把理论预测值和观测值结合，而理论预测值怎么还能再考虑噪声去得到一个不唯一的结果呢(体会一下“理论”二字的含义），所以预测模型（包括状态预测和误差的协方差矩阵的得到）实际上是得到下一时刻的预测均值和方差，状态预测方程若加了噪声反而给预测均值引入了不确定性。
一定要清楚虽然状态预测方程有预测误差，但这个预测误差是预测值和实际值之间的误差，这个状态预测方程是为了得到理论预测值而不是直接得到系统状态的最优估计值。
总结： 状态预测方程就是假设误差为零均值条件下根据系统状态方程(1)推导出来的，它是一个均值型的估计量，其误差带来的不确定度是在协方差矩阵P中体现的。
（就比如小车运动模型，理论上我做匀速直线运动，那么预测方程就是 x k - ∧ = x k − 1 ∧ \overset{\wedge }{\mathop{{{x}_{k}}^{\text{-}}}}\,=\overset{\wedge }{\mathop{{{x}_{k-1}}}}\, xk-∧=xk−1∧，但是这毕竟是理想情况，所以肯定会有和实际运动情况之间的误差，那么这个误差就需要用协方差矩阵来体现，而不是在状态预测方程中加入随机噪声，现在应该明白系统状态方程和卡尔曼滤波中的状态预测方程这两个公式之间的差别了吧。）
（PS：但确实有些方法是在一步预测时会加这么个噪声项的，粒子滤波就是这样。粒子滤波是在给定上一时刻的某一状态，根据运动方程预测当前时刻的，预测值需要依概率获得，这就需要一个抽样的过程。）
可算讲完这里了……
7.1.2 先验预测误差的协方差矩阵 P k − P_{k}^{-} Pk−
OK，接下来我们就要把先验预测量的不确定度（因为预测的不一定准啊，所以会有不确定度）表示出来。
假设k-1时刻的最优估计状态向量
x
k
−
1
∧
\overset{\wedge }{\mathop{{{x}_{k-1}}}}\,
xk−1∧的协方差矩阵为
P
k
−
1
{{P}_{k-1}}
Pk−1，k 时刻的先验预测状态向量（一步预测量）
x
k
-
∧
\overset{\wedge }{\mathop{{{x}_{k}}^{\text{-}}}}\,
xk-∧的协方差矩阵为
P
k
−
P_{k}^{-}
Pk−，则由(3)及协方差矩阵的性质（参考上面数学知识）得：
P
k
−
=
A
k
P
k
−
1
A
k
T
+
Q
(4)
P_{k}^{-}\text{=}{{A}_{k}}{{P}_{k-1}}A_{k}^{T}+Q\tag{4}
Pk−=AkPk−1AkT+Q(4)
Note： 公式中 Q Q Q的来源——在状态转移过程中，除了状态本身具有不确定性外（如小车前一时刻的最优估计状态，比如最优估计出了在3m处，而实际在3.1m处），移动也具有不确定性（如小车前后时刻状态的转移，比如理论上是匀速运动，实际并不是匀速），我们将该不确定性通过过程噪声的协方差矩阵 Q Q Q来表示，该噪声源自于外部干扰（如小车打滑等）。
7.2 更新模型
7.2.1 加权得到后验估计量 x k ∧ \overset{\wedge }{\mathop{{{x}_{k}}}}\, xk∧
有了预测值 x k − ∧ \overset{\wedge }{\mathop{{{x}_{k}}^{-}}}\, xk−∧，有了观测值 z k {{z}_{k}} zk，那么就可以加权得到后验估计值，以 K K K作为加权因子，则有
x k ∧ = K z k + ( I − K H ) x k - ∧ = x k - ∧ + K ( z k − H x k - ∧ ) (5) \begin{aligned} \overset{\wedge }{\mathop{{{x}_{k}}}}\,&\text{=}K{{z}_{k}}+(I-KH)\overset{\wedge }{\mathop{{{x}_{k}}^{\text{-}}}}\,\\ &\text{=}\overset{\wedge }{\mathop{{{x}_{k}}^{\text{-}}}}\,\text{+}K({{z}_{k}}-H\overset{\wedge }{\mathop{{{x}_{k}}^{\text{-}}}}\,) \end{aligned}\tag{5} xk∧=Kzk+(I−KH)xk-∧=xk-∧+K(zk−Hxk-∧)(5)
其中， z k − H x k - ∧ {{z}_{k}}-H\overset{\wedge }{\mathop{{{x}_{k}}^{\text{-}}}}\, zk−Hxk-∧称为残差，描述预测量和你观测量之间的差距（还记得前面说过的这两个量之间的转换吗，现在知道为什么这里有H了吧）。如果残差等于0，说明预测和观测出的结果完全吻合。公式(5)也正是卡尔曼滤波最核心的公式。
7.2.2 误差的协方差矩阵 P k {{P}_{k}} Pk
接下来，就需要考虑随着时刻变化而进行的迭代更新了。
先验预测误差，即先验预测量
x
k
−
∧
\overset{\wedge }{\mathop{{{x}_{k}}^{-}}}\,
xk−∧和真实状态量
x
k
{{x}_{k}}
xk之间的误差：
e
k
−
=
x
k
−
x
k
−
∧
e_{k}^{-}={{x}_{k}}-\overset{\wedge }{\mathop{{{x}_{k}}^{-}}}\,
ek−=xk−xk−∧，
其对应的协方差矩阵为
P k − = E [ e k − e k − T ] (6) P_{k}^{-}=E\left[ e_{k}^{-}e{{_{k}^{-}}^{T}} \right]\tag{6} Pk−=E[ek−ek−T](6)
PS：预测模型中需要的先验预测误差协方差矩阵也可以用如下方式得到公式(4):
P
k
−
=
E
[
e
k
−
e
k
−
T
]
=
E
[
(
x
k
−
x
k
−
∧
)
(
x
k
−
x
k
−
∧
)
T
]
=
E
[
(
A
k
x
k
−
1
+
B
k
u
k
+
w
k
−
A
k
x
k
−
1
∧
−
B
k
u
k
)
(
A
k
x
k
−
1
+
B
k
u
k
+
w
k
−
A
k
x
k
−
1
∧
−
B
k
u
k
)
T
]
=
E
{
[
A
k
(
x
k
−
1
−
x
k
−
1
∧
)
+
w
k
]
[
A
k
(
x
k
−
1
−
x
k
−
1
∧
)
+
w
k
]
T
}
=
E
{
[
A
k
e
k
−
1
+
w
k
]
[
A
k
e
k
−
1
+
w
k
]
T
}
\begin{aligned} & P_{k}^{-}=E\left[ e_{k}^{-}e{{_{k}^{-}}^{T}} \right]=E\left[ \left( {{x}_{k}}-\overset{\wedge }{\mathop{x_{k}^{-}}}\, \right){{\left( {{x}_{k}}-\overset{\wedge }{\mathop{x_{k}^{-}}}\, \right)}^{T}} \right] \\ & =E\left[ \left( {{A}_{k}}{{x}_{k-1}}+{{B}_{k}}{{u}_{k}}+{{w}_{k}}-{{A}_{k}}\overset{\wedge }{\mathop{{{x}_{k-1}}}}\,-{{B}_{k}}{{u}_{k}} \right){{\left( {{A}_{k}}{{x}_{k-1}}+{{B}_{k}}{{u}_{k}}+{{w}_{k}}-{{A}_{k}}\overset{\wedge }{\mathop{{{x}_{k-1}}}}\,-{{B}_{k}}{{u}_{k}} \right)}^{T}} \right] \\ & =E\left\{ \left[ {{A}_{k}}({{x}_{k-1}}-x_{k-1}^{\wedge })+{{w}_{k}} \right]{{\left[ {{A}_{k}}({{x}_{k-1}}-x_{k-1}^{\wedge })+{{w}_{k}} \right]}^{T}} \right\} \\ & =E\left\{ \left[ {{A}_{k}}{{e}_{k-1}}+{{w}_{k}} \right]{{\left[ {{A}_{k}}{{e}_{k-1}}+{{w}_{k}} \right]}^{T}} \right\} \\ \end{aligned}
Pk−=E[ek−ek−T]=E[(xk−xk−∧)(xk−xk−∧)T]=E[(Akxk−1+Bkuk+wk−Akxk−1∧−Bkuk)(Akxk−1+Bkuk+wk−Akxk−1∧−Bkuk)T]=E{[Ak(xk−1−xk−1∧)+wk][Ak(xk−1−xk−1∧)+wk]T}=E{[Akek−1+wk][Akek−1+wk]T}
由于系统状态变量和噪声之间是独立，故上式可以继续写成：
=
E
[
(
A
k
e
k
−
1
)
(
A
k
e
k
−
1
)
T
]
+
E
[
w
k
w
k
T
]
=
A
k
P
k
−
1
A
k
T
+
Q
\begin{array}{l} = E\left[ {({A_k}{e_{k - 1}}){{({A_k}{e_{k - 1}})}^T}} \right] + E\left[ {{w_k}{w_k}^T} \right]\\ = {A_k}{P_{k-1}}{A_k}^T + Q \end{array}
=E[(Akek−1)(Akek−1)T]+E[wkwkT]=AkPk−1AkT+Q
后验估计误差，即后验估计量（最优估计量）
x
k
∧
\overset{\wedge }{\mathop{{{x}_{k}}}}\,
xk∧和真实状态量
x
k
{{x}_{k}}
xk之间的误差：
e
k
=
x
k
−
x
k
∧
{{e}_{k}}={{x}_{k}}-\overset{\wedge }{\mathop{{{x}_{k}}}}\,
ek=xk−xk∧，
其对应的协方差矩阵为
P
k
=
E
[
e
k
e
k
T
]
(7)
{{P}_{k}}=E\left[ {{e}_{k}}{{e}_{k}}^{T} \right]\tag{7}
Pk=E[ekekT](7)
7.2.3 卡尔曼增益 K k {{K}_{k}} Kk
将公式(5)(6)代入公式(7)可得后验估计误差的协方差矩阵有如下关系：
P
k
=
E
[
e
k
e
k
T
]
=
E
[
(
x
k
−
x
k
∧
)
(
x
k
−
x
k
∧
)
T
]
=
E
{
[
x
k
−
x
k
-
∧
-
K
k
(
z
k
−
H
x
k
-
∧
)
)
]
[
x
k
−
x
k
-
∧
-
K
k
(
z
k
−
H
x
k
-
∧
)
)
]
T
}
=
E
{
[
x
k
−
x
k
-
∧
-
K
k
(
H
x
k
+
v
k
−
H
x
k
-
∧
)
)
]
[
x
k
−
x
k
-
∧
-
K
k
(
H
x
k
+
v
k
−
H
x
k
-
∧
)
)
]
T
}
=
E
{
[
(
I
−
K
k
H
)
(
x
k
−
x
k
-
∧
)-
K
k
v
k
]
[
(
I
−
K
k
H
)
(
x
k
−
x
k
-
∧
)-
K
k
v
k
]
T
}
=
(
I
−
K
k
H
)
E
[
(
x
k
−
x
k
-
∧
)
(
x
k
−
x
k
-
∧
)
T
]
(
I
−
K
k
H
)
T
+
K
k
E
[
v
k
v
k
T
]
K
k
T
=
(
I
−
K
k
H
)
P
k
-
(
I
−
K
k
H
)
T
+
K
k
R
K
k
T
\begin{aligned} & {{P}_{k}}=E\left[ {{e}_{k}}{{e}_{k}}^{T} \right]\text{=}E\left[ ({{x}_{k}}-\overset{\wedge }{\mathop{{{x}_{k}}}}\,){{({{x}_{k}}-\overset{\wedge }{\mathop{{{x}_{k}}}}\,)}^{T}} \right] \\ & =E\left\{ \left[ {{x}_{k}}-\overset{\wedge }{\mathop{{{x}_{k}}^{\text{-}}}}\,\text{-}{{K}_{k}}({{z}_{k}}-H\overset{\wedge }{\mathop{{{x}_{k}}^{\text{-}}}}\,)) \right]{{\left[ {{x}_{k}}-\overset{\wedge }{\mathop{{{x}_{k}}^{\text{-}}}}\,\text{-}{{K}_{k}}({{z}_{k}}-H\overset{\wedge }{\mathop{{{x}_{k}}^{\text{-}}}}\,)) \right]}^{T}} \right\} \\ & =E\left\{ \left[ {{x}_{k}}-\overset{\wedge }{\mathop{{{x}_{k}}^{\text{-}}}}\,\text{-}{{K}_{k}}(H{{x}_{k}}+{{v}_{k}}-H\overset{\wedge }{\mathop{{{x}_{k}}^{\text{-}}}}\,)) \right]{{\left[ {{x}_{k}}-\overset{\wedge }{\mathop{{{x}_{k}}^{\text{-}}}}\,\text{-}{{K}_{k}}(H{{x}_{k}}+{{v}_{k}}-H\overset{\wedge }{\mathop{{{x}_{k}}^{\text{-}}}}\,)) \right]}^{T}} \right\} \\ & =E\left\{ \left[ (I-{{K}_{k}}H)({{x}_{k}}-\overset{\wedge }{\mathop{{{x}_{k}}^{\text{-}}}}\,\text{)-}{{K}_{k}}{{v}_{k}} \right]{{\left[ (I-{{K}_{k}}H)({{x}_{k}}-\overset{\wedge }{\mathop{{{x}_{k}}^{\text{-}}}}\,\text{)-}{{K}_{k}}{{v}_{k}} \right]}^{T}} \right\} \\ & =(I-{{K}_{k}}H)E\left[ ({{x}_{k}}-\overset{\wedge }{\mathop{{{x}_{k}}^{\text{-}}}}\,\text{)}{{({{x}_{k}}-\overset{\wedge }{\mathop{{{x}_{k}}^{\text{-}}}}\,\text{)}}^{T}} \right]{{(I-{{K}_{k}}H)}^{T}}+{{K}_{k}}E\left[ {{v}_{k}}{{v}_{k}}^{T} \right]{{K}_{k}}^{T} \\ &=(I-{{K}_{k}}H){{P}_{k}}^{\text{-}}{{(I-{{K}_{k}}H)}^{T}}\text{+}{{K}_{k}}R{{K}_{k}}^{T} \\ \end{aligned}
Pk=E[ekekT]=E[(xk−xk∧)(xk−xk∧)T]=E{[xk−xk-∧-Kk(zk−Hxk-∧))][xk−xk-∧-Kk(zk−Hxk-∧))]T}=E{[xk−xk-∧-Kk(Hxk+vk−Hxk-∧))][xk−xk-∧-Kk(Hxk+vk−Hxk-∧))]T}=E{[(I−KkH)(xk−xk-∧)-Kkvk][(I−KkH)(xk−xk-∧)-Kkvk]T}=(I−KkH)E[(xk−xk-∧)(xk−xk-∧)T](I−KkH)T+KkE[vkvkT]KkT=(I−KkH)Pk-(I−KkH)T+KkRKkT(
I
{{I}}
I为单位矩阵)
注意协方差矩阵是对称矩阵，则
P
k
−
T
=
P
k
−
{{P}_{k}}{{^{-}}^{T}}\text{=}{{P}_{k}}^{-}
Pk−T=Pk−，将上式继续展开，得
P
k
=
P
k
−
−
K
k
H
P
k
−
−
P
k
−
H
T
K
k
T
+
K
k
(
H
P
k
−
H
T
+
R
)
K
k
T
(8)
{{P}_{k}}={{P}_{k}}^{-}-{{K}_{k}}H{{P}_{k}}^{-}-{{P}_{k}}^{-}{{H}^{T}}{{K}_{k}}^{T}+{{K}_{k}}(H{{P}_{k}}^{-}{{H}^{T}}+R){{K}_{k}}^{T}\tag{8}
Pk=Pk−−KkHPk−−Pk−HTKkT+Kk(HPk−HT+R)KkT(8)
接下来，就要利用“最小均方误差准则”（该准则可以自行百度学习）
而协方差矩阵的迹
T
[
P
k
]
T\left[ {{P}_{k}} \right]
T[Pk]是状态变量的均方误差，因此我们需要让协方差矩阵
P
k
{{P}_{k}}
Pk的迹（对角线元素之和）最小即可满足该准则。
P
k
{{P}_{k}}
Pk的迹记为
T
[
P
k
]
T\left[ {{P}_{k}} \right]
T[Pk]，由公式（8），得到
T
[
P
k
]
=
T
[
P
k
−
]
−
T
[
K
k
H
P
k
−
]
−
T
[
P
k
−
H
T
K
k
T
]
+
T
[
K
k
(
H
P
k
−
H
T
+
R
)
K
k
T
]
=
T
[
P
k
−
]
−
2
T
[
K
k
H
P
k
−
]
+
T
[
K
k
(
H
P
k
−
H
T
+
R
)
K
k
T
]
(9)
\begin{aligned} T\left[ {{P}_{k}} \right]& \text{=}T\left[ {{P}_{k}}^{-} \right]-T\left[ {{K}_{k}}H{{P}_{k}}^{-} \right]-T\left[ {{P}_{k}}^{-}{{H}^{T}}{{K}_{k}}^{T} \right]+T\left[ {{K}_{k}}(H{{P}_{k}}^{-}{{H}^{T}}+R){{K}_{k}}^{T} \right] \\ & \text{=}T\left[ {{P}_{k}}^{-} \right]-2T\left[ {{K}_{k}}H{{P}_{k}}^{-} \right]+T\left[ {{K}_{k}}(H{{P}_{k}}^{-}{{H}^{T}}+R){{K}_{k}}^{T} \right] \\ \end{aligned}\tag{9}
T[Pk]=T[Pk−]−T[KkHPk−]−T[Pk−HTKkT]+T[Kk(HPk−HT+R)KkT]=T[Pk−]−2T[KkHPk−]+T[Kk(HPk−HT+R)KkT](9)
为使
T
[
P
k
]
T\left[ {{P}_{k}} \right]
T[Pk]最小，我们就对未知量
K
k
{{K}_{k}}
Kk求导,令导函数等于0，从而得到符合准则的
K
k
{{K}_{k}}
Kk值。
关于求导，这里用到两个矩阵的微分公式：
∂ T [ A B ] ∂ A = B T \frac{\partial T\left[ AB \right]}{\partial A}\text{=}{{B}^{T}} ∂A∂T[AB]=BT
∂ T [ A B A T ] ∂ A = 2 A B \frac{\partial T\left[ AB{{A}^{T}} \right]}{\partial A}\text{=}2AB ∂A∂T[ABAT]=2AB
则由公式（9）对
K
k
{{K}_{k}}
Kk求导，并令导数=0，得
∂
T
[
P
k
]
∂
K
k
=-
2
∂
T
[
K
k
H
P
k
−
]
∂
K
k
+
∂
T
[
K
k
(
H
P
k
−
H
T
+
R
)
K
k
T
]
∂
K
k
=
0
\begin{aligned} & \frac{\partial T\left[ {{P}_{k}} \right]}{\partial {{K}_{k}}}\text{=-}2\frac{\partial T\left[ {{K}_{k}}H{{P}_{k}}^{-} \right]}{\partial {{K}_{k}}}+\frac{\partial T\left[ {{K}_{k}}(H{{P}_{k}}^{-}{{H}^{T}}+R){{K}_{k}}^{T} \right]}{\partial {{K}_{k}}}\text{=}0 \\ & \\ \end{aligned}
∂Kk∂T[Pk]=-2∂Kk∂T[KkHPk−]+∂Kk∂T[Kk(HPk−HT+R)KkT]=0
即
-
2
(
H
P
k
−
)
T
+
2
K
k
(
H
P
k
−
H
T
+
R
)
=
0
\text{-}2{{\left( H{{P}_{k}}^{-} \right)}^{T}}\text{+}2{{K}_{k}}(H{{P}_{k}}^{-}{{H}^{T}}+R)\text{=}0
-2(HPk−)T+2Kk(HPk−HT+R)=0
解得，
K
k
=
(
H
P
k
−
)
T
(
H
P
k
−
H
T
+
R
)
−
1
=
P
k
−
T
H
T
(
H
P
k
−
H
T
+
R
)
−
1
\begin{aligned} {{K}_{k}} & \text{=}{{\left( H{{P}_{k}}^{-} \right)}^{T}}{{(H{{P}_{k}}^{-}{{H}^{T}}+R)}^{-1}}\\ & \text{=}{{P}_{k}}{{^{-}}^{T}}{{H}^{T}}{{(H{{P}_{k}}^{-}{{H}^{T}}+R)}^{-1}} \end{aligned}
Kk=(HPk−)T(HPk−HT+R)−1=Pk−THT(HPk−HT+R)−1
而协方差矩阵是对称矩阵，则
P
k
−
T
=
P
k
−
{{P}_{k}}{{^{-}}^{T}}\text{=}{{P}_{k}}^{-}
Pk−T=Pk−，故卡尔曼增益公式如下：
K
k
=
P
k
−
H
T
(
H
P
k
−
H
T
+
R
)
−
1
(10)
{{K}_{k}}\text{=}{{P}_{k}}^{-}{{H}^{T}}{{(H{{P}_{k}}^{-}{{H}^{T}}+R)}^{-1}}\tag{10}
Kk=Pk−HT(HPk−HT+R)−1(10)
7.2.4 误差协方差矩阵 P k {{P}_{k}} Pk的更新
至此，还差估计值和真实值之间的误差协方差矩阵 P k {{P}_{k}} Pk未进行更新。
将公式（10）代回公式（8）中最后一项，得
P
k
=
P
k
−
−
K
k
H
P
k
−
−
P
k
−
H
T
K
k
T
+
K
k
(
H
P
k
−
H
T
+
R
)
K
k
T
=
P
k
−
−
K
k
H
P
k
−
−
P
k
−
H
T
K
k
T
+
P
k
−
H
T
(
H
P
k
−
H
T
+
R
)
−
1
(
H
P
k
−
H
T
+
R
)
K
k
T
=
P
k
−
−
K
k
H
P
k
−
−
P
k
−
H
T
K
k
T
+
P
k
−
H
T
K
k
T
=
P
k
−
−
K
k
H
P
k
−
=
(
I
−
K
k
H
)
P
k
−
\begin{aligned} {{P}_{k}} & \text{=}{{P}_{k}}^{-}-{{K}_{k}}H{{P}_{k}}^{-}-{{P}_{k}}^{-}{{H}^{T}}{{K}_{k}}^{T}+{{K}_{k}}(H{{P}_{k}}^{-}{{H}^{T}}+R){{K}_{k}}^{T} \\ & \text{=}{{P}_{k}}^{-}-{{K}_{k}}H{{P}_{k}}^{-}-{{P}_{k}}^{-}{{H}^{T}}{{K}_{k}}^{T}+{{P}_{k}}^{-}{{H}^{T}}{{(H{{P}_{k}}^{-}{{H}^{T}}+R)}^{-1}}(H{{P}_{k}}^{-}{{H}^{T}}+R){{K}_{k}}^{T} \\ & \text{=}{{P}_{k}}^{-}-{{K}_{k}}H{{P}_{k}}^{-}-{{P}_{k}}^{-}{{H}^{T}}{{K}_{k}}^{T}\text{+}{{P}_{k}}^{-}{{H}^{T}}{{K}_{k}}^{T} \\ & \text{=}{{P}_{k}}^{-}-{{K}_{k}}H{{P}_{k}}^{-} \\ & \text{=}(I-{{K}_{k}}H){{P}_{k}}^{-} \\ \end{aligned}
Pk=Pk−−KkHPk−−Pk−HTKkT+Kk(HPk−HT+R)KkT=Pk−−KkHPk−−Pk−HTKkT+Pk−HT(HPk−HT+R)−1(HPk−HT+R)KkT=Pk−−KkHPk−−Pk−HTKkT+Pk−HTKkT=Pk−−KkHPk−=(I−KkH)Pk−
即，后验估计误差的协方差矩阵更新方程为
P
k
=
(
I
−
K
k
H
)
P
k
−
(11)
{{P}_{k}}=(I-{{K}_{k}}H){{P}_{k}}^{-}\tag{11}
Pk=(I−KkH)Pk−(11)
8. 卡尔曼滤波架构总结
至此，历时四天终于把卡尔曼滤波的整个原理和公式的由来都已阐述完毕（把东西写出来可真不是件容易事……）
如有错误欢迎指正，后期还会继续更新EKF和UKF的原理阐述。
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
