1、最小二乘的性能——估计方差
定量描述估计的误差,估计方差是一个很关键的评价指标。本节给出几种最小二乘方法滤波增益和估计方差的求解方法,这些也是卡尔曼滤波器的理论基础。
估计方差的定义为
P
(
k
)
=
E
[
(
θ
−
θ
^
(
k
)
)
(
θ
−
θ
^
(
k
)
)
T
]
\pmb P(k) = E\left[(\pmb\theta-\hat{\pmb\theta}(k))(\pmb\theta-\hat{\pmb\theta}(k))^T\right]
P(k)=E[(θ−θ^(k))(θ−θ^(k))T]
式中, θ \pmb\theta θ 为待估计参数的真值, θ ^ ( k ) \hat{\pmb\theta}(k) θ^(k) 为第 k k k 步的估计值, E ( ⋅ ) E(\cdot) E(⋅) 表示求均值。一般可以认为,所得的估计方差越小,估计方法就越好。
接下来以递推最小二乘方法为例,研究一下最小二乘方法的估计方差。前面已经讲过,权值
W
(
k
)
W(k)
W(k) 一般取为测量噪声方差
R
(
k
)
R(k)
R(k) 的倒数,如果权值
W
(
k
)
W(k)
W(k) 为一个矩阵的话则取为
R
(
k
)
R(k)
R(k) 的逆。从现在开始都用
R
−
1
(
k
)
R^{-1}(k)
R−1(k) 来代替
W
(
k
)
W(k)
W(k)。
θ
^
(
k
)
=
θ
^
(
k
−
1
)
+
M
k
H
T
(
k
)
W
(
k
)
[
Z
(
k
)
−
H
(
k
)
θ
^
(
k
−
1
)
]
M
k
−
1
=
M
k
−
1
−
1
+
H
T
(
k
)
W
(
k
)
H
(
k
)
(1)
\boxed{\begin{aligned} \hat{\pmb\theta}(k)&=\hat{\pmb\theta}(k-1)+\pmb M_k\pmb H^T(k)\pmb W(k)\left[Z(k)-\pmb H(k)\hat{\pmb\theta}(k-1) \right]\\ \pmb M_{k}^{-1}&=\pmb M_{k-1}^{-1} + \pmb H^T(k)\pmb W(k)\pmb H(k) \end{aligned}\tag{1}}
θ^(k)Mk−1=θ^(k−1)+MkHT(k)W(k)[Z(k)−H(k)θ^(k−1)]=Mk−1−1+HT(k)W(k)H(k)(1)
我们令
K
(
k
)
=
M
k
H
T
(
k
)
R
−
1
(
k
)
\pmb K(k)=\pmb M_k\pmb H^T(k)\pmb R^{-1}(k)
K(k)=MkHT(k)R−1(k),称其为滤波增益,将上式代入估计方差的定义,可以得到
P
(
k
)
=
E
[
(
θ
−
θ
^
(
k
)
)
(
θ
−
θ
^
(
k
)
)
T
]
=
E
[
(
θ
−
θ
^
(
k
−
1
)
−
K
(
k
)
(
Z
(
k
)
−
H
(
k
)
θ
^
(
k
−
1
)
)
)
(
θ
−
θ
^
(
k
−
1
)
−
K
(
k
)
(
Z
(
k
)
−
H
(
k
)
θ
^
(
k
−
1
)
)
)
T
]
(2)
\begin{aligned} \pmb P(k)&= E\left[(\pmb\theta-\hat{\pmb\theta}(k))(\pmb\theta-\hat{\pmb\theta}(k))^T\right]\\[1ex] &= E\Bigg[\bigg(\pmb\theta-\hat{\pmb\theta}(k-1)-\pmb K(k)\left(Z(k)-\pmb H(k)\hat{\pmb\theta}(k-1) \right)\bigg)\\[1ex] &\quad\ \ \ \ \ \ \ \bigg(\pmb\theta-\hat{\pmb\theta}(k-1)-\pmb K(k)\left(Z(k)-\pmb H(k)\hat{\pmb\theta}(k-1) \right)\bigg)^T\Bigg] \end{aligned}\tag{2}
P(k)=E[(θ−θ^(k))(θ−θ^(k))T]=E[(θ−θ^(k−1)−K(k)(Z(k)−H(k)θ^(k−1))) (θ−θ^(k−1)−K(k)(Z(k)−H(k)θ^(k−1)))T](2)
再将测量方程
Z
(
k
)
=
H
(
k
)
θ
+
N
(
k
)
Z(k)=\pmb H(k)\pmb\theta+N(k)
Z(k)=H(k)θ+N(k) 代入上式,考虑到待估计值与
N
(
k
)
N(k)
N(k) 不相关,整理后可以得到
P
(
k
)
=
[
I
−
K
(
k
)
H
(
k
)
]
P
(
k
−
1
)
[
I
−
K
(
k
)
H
(
k
)
]
T
+
K
(
k
)
R
(
k
)
K
T
(
k
)
(3)
\pmb P(k)=\Big[\pmb I-\pmb K(k)\pmb H(k)\Big]\pmb P(k-1)\Big[\pmb I-\pmb K(k)\pmb H(k)\Big]^T + \pmb K(k)\pmb R(k)\pmb K^T(k)\tag{3}
P(k)=[I−K(k)H(k)]P(k−1)[I−K(k)H(k)]T+K(k)R(k)KT(k)(3)
接下来讨论递推最小二乘方法的估计性能问题,其结果是最优的吗?
令式(3)
P
(
k
)
\pmb P(k)
P(k) 求偏导得到的子式等于 0,即
∂
P
(
k
)
∂
K
(
k
)
=
2
[
I
−
K
(
k
)
H
(
k
)
]
P
(
k
−
1
)
[
−
H
T
(
k
)
]
+
2
K
(
k
)
R
(
k
)
=
0
(4)
\frac{\partial\pmb P(k)}{\partial\pmb K(k)} = 2\Big[\pmb I-\pmb K(k)\pmb H(k)\Big]\pmb P(k-1)\Big[-\pmb H^T(k)\Big] + 2\pmb K(k)\pmb R(k)=0\tag{4}
∂K(k)∂P(k)=2[I−K(k)H(k)]P(k−1)[−HT(k)]+2K(k)R(k)=0(4)
整理可得
K
(
k
)
=
P
(
k
−
1
)
H
T
(
k
)
[
R
(
k
)
+
H
(
k
)
P
(
k
−
1
)
H
T
(
k
)
]
−
1
(5)
\pmb K(k)=\pmb P(k-1)\pmb H^T(k)\Big[\pmb R(k)+\pmb H(k)\pmb P(k-1)\pmb H^T(k)\Big]^{-1}\tag{5}
K(k)=P(k−1)HT(k)[R(k)+H(k)P(k−1)HT(k)]−1(5)
这个答案初看起来令人惊讶,和已经得到的滤波增益 K ( k ) = M k H T ( k ) R − 1 ( k ) \pmb K(k)=\pmb M_k\pmb H^T(k)\pmb R^{-1}(k) K(k)=MkHT(k)R−1(k) 是不同的,一个包含 M k \pmb M_k Mk,另一个包含 P ( k − 1 ) \pmb P(k-1) P(k−1)。但下面2个结论(证明方法见附录(有空再补充吧))却表明这两个公式是等价的。
- 若考虑 W ( k ) = R − 1 ( k ) \pmb W(k)=\pmb R^{-1}(k) W(k)=R−1(k),则 M k \pmb M_k Mk 实际上是估计方差 P ( k ) \pmb P(k) P(k),按照式(1),估计方差也满足 P − 1 ( k ) = P − 1 ( k − 1 ) + H T ( k ) R − 1 ( k ) H ( k ) \pmb P^{-1}(k)=\pmb P^{-1}(k-1) + \pmb H^T(k)\pmb R^{-1}(k)\pmb H(k) P−1(k)=P−1(k−1)+HT(k)R−1(k)H(k)。
- 若用 P ( k ) \pmb P(k) P(k) 来代替式(1)中的 M k \pmb M_k Mk,则式(5)所表达的滤波增益是等价的。
到这里,可以得到如下结论,利用最小二乘性能指标得到的估计方法:
θ
^
(
k
)
=
θ
^
(
k
−
1
)
+
K
(
k
)
[
Z
(
k
)
−
H
(
k
)
θ
^
(
k
−
1
)
]
(6)
\hat{\pmb\theta}(k)=\hat{\pmb\theta}(k-1)+\pmb K(k)\left[Z(k)-\pmb H(k)\hat{\pmb\theta}(k-1) \right]\tag{6}
θ^(k)=θ^(k−1)+K(k)[Z(k)−H(k)θ^(k−1)](6)
可以得到最小的估计方差
P
(
k
)
\pmb P(k)
P(k),其中滤波增益
K
(
k
)
\pmb K(k)
K(k) 有以下两种不同的计算方法:
K
(
k
)
=
M
k
H
T
(
k
)
R
−
1
(
k
)
K
(
k
)
=
P
(
k
−
1
)
H
T
(
k
)
[
R
(
k
)
+
H
(
k
)
P
(
k
−
1
)
H
T
(
k
)
]
−
1
(7)
\begin{aligned} \pmb K(k)&=\pmb M_k\pmb H^T(k)\pmb R^{-1}(k)\\[1ex] \pmb K(k)=\pmb P(k-1)\pmb H^T&(k)\Big[\pmb R(k)+\pmb H(k)\pmb P(k-1)\pmb H^T(k)\Big]^{-1} \end{aligned}\tag{7}
K(k)K(k)=P(k−1)HT=MkHT(k)R−1(k)(k)[R(k)+H(k)P(k−1)HT(k)]−1(7)
估计方差
P
(
k
)
\pmb P(k)
P(k) 可以使用如下两种求法来计算:
P
(
k
)
=
[
P
−
1
(
k
−
1
)
+
H
T
(
k
)
R
−
1
(
k
)
H
(
k
)
]
−
1
(8)
\pmb P(k)=\Big[\pmb P^{-1}(k-1) + \pmb H^T(k)\pmb R^{-1}(k)\pmb H(k)\Big]^{-1}\tag{8}
P(k)=[P−1(k−1)+HT(k)R−1(k)H(k)]−1(8)
P ( k ) = [ I − K ( k ) H ( k ) ] P ( k − 1 ) [ I − K ( k ) H ( k ) ] T + K ( k ) R ( k ) K T ( k ) (9) \pmb P(k)=\Big[\pmb I-\pmb K(k)\pmb H(k)\Big]\pmb P(k-1)\Big[\pmb I-\pmb K(k)\pmb H(k)\Big]^T + \pmb K(k)\pmb R(k)\pmb K^T(k)\tag{9} P(k)=[I−K(k)H(k)]P(k−1)[I−K(k)H(k)]T+K(k)R(k)KT(k)(9)
在附录中还提到了下面的估计方差
P
(
k
)
\pmb P(k)
P(k) 计算方法:
P
(
k
)
=
[
I
−
K
(
k
)
H
(
k
)
]
P
(
k
−
1
)
(10)
\pmb P(k)=\Big[\pmb I-\pmb K(k)\pmb H(k)\Big]\pmb P(k-1)\tag{10}
P(k)=[I−K(k)H(k)]P(k−1)(10)
在上面的三个计算 P ( k ) \pmb P(k) P(k) 的公式中,式(10)是一个比较简单的表达式,但是数值计算问题可能导致 P ( k ) \pmb P(k) P(k) 不是正定的,即使 P ( k − 1 ) \pmb P(k-1) P(k−1) 和 R ( k ) \pmb R(k) R(k) 都是正定的。式(9)虽然复杂,但是却可以保证 P ( k ) \pmb P(k) P(k) 是正定的,与理论解一致,而式(8)需要三次矩阵求逆计算,因此计算复杂,需要占用更多的计算资源,但在多传感器状态融合估计中可以使融合公式具有统一形式,所以经常在状态融合估计方法中使用。