前言
很多文章都介绍过卡尔曼滤波器，所以有些朋友应该知道这个滤波器是一种可以用递推的方式对系统的状态进行滤波估计的。也知道这个滤波器在很多领域，尤其是多信息融合，无人机组合导航等领域应用很广，效果非常好。论坛里很多文章也对这个滤波器的原理做出形象的解释。但是还是有很多人觉得，文章看得懂，可是就是不知道怎么用。而且这个卡尔曼滤波为什么能够对系统状态进行估计呢。
我准备写一些文章，从最优估计的基本概念入手，对卡尔曼滤波器及相关理论进行一次系统的整理。文中会涉及基础概率统计，线性方程、微分方程等基础知识的。毕竟，能研究最优估计了，矩阵论，概率统计论应该是都学过了。别告诉我你不知道矩阵乘除、求逆，随机序列方差、数学期望、协方差、相关性等这些基本概念。
一、最优估计概述
都说卡尔曼滤波是最优估计，那最优估计是什么鬼？所谓最优估计通俗点讲，就是对某一感兴趣的变量，进行了一系列的观测（就是测量），然后从获取的测量数据中以某一种最优指标对你感兴趣的变量进行估算。比如，用弹簧秤去称量某一物体重量。从弹簧秤上其实获得的是弹簧被拉伸的长度，然后通过这个长度去计算物体的重量。
这个过程如果用数学语言表达就是：
根据测量得出的与状态X有关的数据Z=h(X)+V，从Z解算出X的计算值
X
^
\hat X
X^（一般来讲，某一变量上面如果标了个小尖儿，就代表这是那个变量的估计值）。
注意，这里的V是测量误差，随机的，所以你不可能获得X的精确值，而只能获得它的估计值，也就是说X和
X
^
\hat X
X^之间是有误差的。这时，我们就可以这么叫，
X
^
\hat X
X^是X的估计，Z是X的量测，Z=h(X)+V叫做量测方程。我们要做的就是从Z计算出
X
^
\hat X
X^。在大多数情况下，那个量测方程是线性方程，也就是说h(X)可以写成HX。还要强调一点的是，这里的X和Z可不一定是一个数，而是一组数，所以X和Z其实都是向量形式的，X是状态向量，Z是观测向量。对线性量测，H就是一个矩阵，叫做观测矩阵。
从Z计算出X的估计值
X
^
\hat X
X^如果遵循某一最优指标，那么这种估计就可以叫做最优估计。不同的最优指标，估计的算法也是不同的，估计的精度也不同。卡尔曼滤波也有它的最优指标，就是估计方差最小。这个等以后写到卡尔曼滤波的时候再说。先来看一个最简单的最优估计方法：最小二乘估计。
二、最小二乘估计
从前面的介绍已经知道，最优估计是要从量测Z通过一种算法获得X的估计
X
^
\hat X
X^，使得某一种指标达到最优。最小二乘法的指标是什么呢？先看量测方程：
Z
=
H
X
+
V
Z=HX+V
Z=HX+V
Z是观测向量，m维的，X是状态向量，n维，H是观测矩阵m×n的规模，V是量测噪声，是随机量。
而最小二乘估计的指标就是：Z与估计值
X
^
\hat X
X^确定的量测估计
Z
^
=
H
X
^
\hat Z = H\hat X
Z^=HX^之间的方差和最小。用数学表达式就是已知H和Z，求
X
^
\hat X
X^，使得：
(
Z
−
H
X
^
)
T
(
Z
−
H
X
^
)
(Z-H\hat X)^ T(Z-H\hat X)
(Z−HX^)T(Z−HX^)
达到最小。上标T表示矩阵的转置。
高数讲过，求极值只要对上式对
X
^
\hat X
X^求导后等于0，解出 就行了。可是，这里的Z和
X
^
\hat X
X^都是向量，H甚至是矩阵，这个式子对向量求导并不简单。在这里就不详细推导了。直接给出结论：
X
^
=
(
H
T
H
)
−
1
H
T
Z
\hat X=(H^ TH)^{-1}H^TZ
X^=(HTH)−1HTZ
这就是最小二乘估计的表达式了。
有公式是一回事，怎么用呢，先来看一个最简单的应用例子，中学的时候见过这个结论。
举个例子
说用一台仪器对一个未知的标量X进行r次直接测量。测量值分别是 Z 1 、 Z 2 、 … 、 Z r Z_1、Z_2、…、Z_r Z1、Z2、…、Zr。请用最小二乘法求X的估计值 X ^ \hat X X^。
先确定观测向量Z。这个很明显，
Z
1
Z_1
Z1、
Z
2
Z_2
Z2、…、
Z
r
Z_r
Zr都是观测值，用一个向量表示
Z
=
(
Z
1
Z
2
⋮
Z
r
)
Z=\begin{pmatrix}Z_1\\Z_2\\\vdots\\Z_r\end{pmatrix}
Z=⎝⎜⎜⎜⎛Z1Z2⋮Zr⎠⎟⎟⎟⎞
向量都是竖着写的，所以向量也可以看成是很多行却只有一列的矩阵。
然后确定观测向量。直接测量？那观测矩阵就是1，有多少次测量，就有多少个1。所以
H
=
(
1
1
⋮
1
)
H=\begin{pmatrix}1\\1\\\vdots\\1\end{pmatrix}
H=⎝⎜⎜⎜⎛11⋮1⎠⎟⎟⎟⎞
上公式 X ^ = ( H T H ) − 1 H T Z \hat X=(H^ TH)^{-1}H^TZ X^=(HTH)−1HTZ
代入，就会得到：
X
^
=
1
r
(
Z
1
+
Z
2
+
⋯
+
Z
r
)
\hat X=\frac{1}{r}(Z_1+Z_2+⋯+Z_r)
X^=r1(Z1+Z2+⋯+Zr)
结论就是，如果对一个量进行多次直接测量，那么它的最小二乘估计就是这些量测量的算术平均值。是不是我们在生活中经常用这个结论？以后在把一堆数做平均，就可以说使用了最小二乘估计的算法了。
不知道有人注意到了没有，全程没有量测误差V什么事，它是多余的吗。不是的，这涉及到估计精度分析。
三、最小二乘估计的精度分析
回过头来再看观测方程：
Z
=
H
X
+
V
Z=HX+V
Z=HX+V
这里的V是随机噪声，也是一个长度和Z相同的向量。虽然噪声的具体值不知道，但是噪声的方差却是可以知道的。量测噪声方差的大小就代表了量测精度的大小。当然这里的噪声的数学期望值是0。
于是，我们对于V向量引入了一个概念，方差矩阵R。R矩阵是一个m×m的方阵，对角线上的元素就是量测向量Z里每个测量值的方差，其他元素就是Z向量里不同量测之间的协方差。而如果Z向量里每个量测值都是不相关的（大多数情况都是这样的），意思就是每个测量值的经度不受其他测量值的影响。那么所有的协方差就都是0。R阵就可以写成：
R
=
(
r
1
0
…
0
0
r
2
…
0
⋮
⋮
⋱
⋮
0
0
…
r
m
)
R=\begin{pmatrix}r_1&0&…&0\\0&r_2&…&0\\\vdots&\vdots&\ddots&\vdots\\0&0&…&r_m\end{pmatrix}
R=⎝⎜⎜⎜⎛r10⋮00r2⋮0……⋱…00⋮rm⎠⎟⎟⎟⎞
那么用最小二乘法估计获得的
X
^
\hat X
X^的方差，也就是
X
^
\hat X
X^的精度是多少呢。直观上感觉，量测的经度越高，估计的经度就越高，事实也确实是这样的。可以利用方差的定义进行推导，就是化简下面这个东西
E
[
X
~
X
~
T
]
E[\widetilde X\widetilde X^T]
E[X
X
T]其中
X
~
\widetilde X
X
表示
X
−
X
^
X-\hat X
X−X^。把最小二乘的估值结果带进去，再用到的定义
R
=
E
[
Z
~
Z
~
T
]
R=E[\widetilde Z\widetilde Z^T]
R=E[Z
Z
T]，
Z
~
=
Z
−
Z
^
\widetilde Z=Z-\hat Z
Z
=Z−Z^。进行一系列化简，就会得到
E
[
X
~
X
~
T
]
=
(
H
T
H
)
−
1
H
T
R
H
(
H
T
H
)
−
1
E[\widetilde X\widetilde X^T]=(H^ TH)^{-1}H^TRH(H^ TH)^{-1}
E[X
X
T]=(HTH)−1HTRH(HTH)−1
这个式子就是最小二乘估计的方差，也就是最小二乘估计的精度。
公式不直观，还是用上面那个求平均值的例子进行说明，不是说对X进行了r次测量吗。如果那个测量仪器的测量方差是s，那么方差矩阵R就可以写成：
R
=
(
s
0
…
0
0
s
…
0
⋮
⋮
⋱
⋮
0
0
…
s
)
R=\begin{pmatrix}s&0&…&0\\0&s&…&0\\\vdots&\vdots&\ddots&\vdots\\0&0&…&s\end{pmatrix}
R=⎝⎜⎜⎜⎛s0⋮00s⋮0……⋱…00⋮s⎠⎟⎟⎟⎞
对角线上都是s。代入上面那个公式就会得到：
E
[
X
~
2
]
=
s
r
E[\widetilde X^2]=\frac{s}{r}
E[X
2]=rs
可以看到，多次测量求平均值的方差是测量方差的r分之1，也就是说测量的次数越多，测量的精度就越高，这个结论在数理统计中也同样出现过。
四、用途——曲线拟合
其实，最小二乘估计会经常被用到的，比如，你是不是做过这么一件事，曲线拟合。曲线拟合说白了就是最小二乘估计，比如二次曲线拟合，要拟合的曲线是：
y
=
a
x
2
+
b
x
+
c
y=ax^2+bx+c
y=ax2+bx+c
拟合的目的就是要通过大量的y和x来确定a，b，c。这时，我们就可以定义
Z
=
(
y
1
y
2
⋮
y
n
)
,
X
=
(
a
b
c
)
,
H
=
(
x
1
2
x
1
1
x
2
2
x
2
1
⋮
x
n
2
x
n
1
)
Z=\begin{pmatrix}y_1\\y_2\\\vdots\\y_n\end{pmatrix},X=\begin{pmatrix}a\\b\\c\end{pmatrix},H=\begin{pmatrix}x_1^2&x_1&1\\x_2^2&x_2&1\\\vdots\\x_n^2&x_n&1\end{pmatrix}
Z=⎝⎜⎜⎜⎛y1y2⋮yn⎠⎟⎟⎟⎞,X=⎝⎛abc⎠⎞,H=⎝⎜⎜⎜⎛x12x22⋮xn2x1x2xn111⎠⎟⎟⎟⎞
然后就可以用最小二乘估计了。其他曲线用这种方法也可以拟合。
最小二乘估计算法简单，用处广，但它有它的缺陷。就是估值精度不高。这是因为在
X
^
\hat X
X^的估值公式中并没有出现量测精度的信息。下一篇文章就会讲一下这个问题，并尝试解决。
1670
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
