多传感器状态融合

m
w (k ) R r 是输入白噪声， vi (k ) R mi 是第 i 个传感器的观测白噪声， A R nn , B R ns ，
C R nr , H i R mi n 是已知矩阵。
假设 1： w ( k ) 、 vi ( k )(i 1, , L) 是均值为零方差阵为 Qw 、 Qvi (i 1, , L) 的互不相 关（正交）白噪声，即
i 1 j 1
并且有关系式：
trP0s (k | k ) trPi (k | k ), i 1, , L
其 中
s (k ) a s (k ) a1s (k ) aL T
Ptr (k | k ) 1 e eT Ptr (k | k ) 1 e
，
e 1 1
确定。 定理 3.3.3 多传感器系统(3.1.1)和(3.1.2)在假设 1、2 下，按对角阵加权最优状态融合 Kalman 滤波器为：
1 d d d T j ˆ0 ˆ x 0 j ( k | k ) e ( P ( k | k ) e , j 1, , L j ( k | k ) aij ( k ) xij ( k | k ) ， P 1
L
i 1
并且有关系式：
P0dj (k | k ) Piij (k | k ), i 1, , L
误差平方和为 trP0 ( k | k )
d
P
j 1
n
d 0j
(k | k ) ，并且有关系式：
trP0d (k | k ) trPi (k | k ), i 1, , L
3
Kalman 滤波器为：
m ˆ0 ˆ (k | k ) ， P0m (k | k ) (eT P(k | k ) 1 e ) 1 (k | k ) (e T P(k | k ) 1 e ) 1 eT P(k | k ) 1 x x
并且有关系式：
trP0m (k | k ) trPi (k | k ), i 1, , L
记
ˆ i (l | k ), i 0,1, , L ， i (l | k ) x (l ) x x
i (l | k ) x j (l | k )T ], i, j 0,1, , L Pij (l | k ) E[ x
i (l | k ) x i (l | k )T ], i 0,1, , L Pi (l | k ) Pii (l | k ) E[ x
并且局部滤波误差互协方差阵为( i j )：
Pij (k | k ) K iA (k ) Pij (k 1| k 1) K jA (k )T K iC (k )Qw K jC (k )T
其中 K iA ( k ) [ I K i ( k ) H i ] A ， K iC ( k ) [ I K i ( k ) H i ]C ，
因此由假设 1 得到(3.2.4)和(3.2.5)。 再利用第四章定理 3.3.1 马上得到局部 Kalman 预报器： 定理 3.2.2(局部 Kalman 预报器) 系统(3.2.1)和(3.2.2)在假设 1、2 下的递推 Kalman 预 报器为：
ˆ i (k 1| k ) K iA (k ) x ˆ i (k | k 1) K ip (k ) yi (k ) x
T T
证明：只需证明(3.2.7)和(3.2.8)。 将(来自百度文库.2.2)代入(3.2.6)得到，
ˆ i ( k 1 | k ) K iA x ˆ ( k | k 1) AK i ( k )[ H i x ( k ) v i ( k )] x
(3.2.1)减去上式，得到
2
ˆ i ( k | k 1) AK i ( k )[ H i x ( k ) v i ( k )] i ( k 1 | k ) A x ( k ) C w ( k ) K iA x x i ( k | k 1) C w ( k ) K ip ( k ) v i ( k ) K iA x
(3.2.1)减去上式，得到
i ( k | k ) A x ( k 1) C w ( k 1) x ˆ i ( k 1 | k 1) K i ( k )[ H i A x ( k 1) H i C w ( k 1) v i ( k )] K iA x i ( k 1 | k 1) K iC w ( k 1) K i ( k ) v i ( k ) K iA x
1
ˆ i ( k | k ) K iA ( k ) x ˆ i ( k 1 | k 1) K i ( k ) yi ( k ) x
(3.2.3)
Pi (k | k ) [ I K i (k ) H i ]Pi (k | k 1)
或
Pi (k | k ) K iA (k ) Pi (k 1| k 1) K iA (k )T K iC ( k )Qw K iC (k )T K i ( k )Qvi K i ( k )T (3.2.4)
(3.2.6)
Pi (k 1| k ) K iA (k ) Pi (k | k 1) AT CQw C T
或
Pi (k 1| k ) K iA (k ) Pi (k | k 1) K iA (k )T CQw C T K ip (k )Qvi K ip (k )T
§3
3．1
多传感器状态融合 Kalman 滤波器和预报器
问题描述
考虑如下不含控制变量的多传感器线性离散定常随机系统：
x (k 1) Ax (k ) Cw (k )
yi (k ) H i x (k ) vi (k ), i 1, , L
n
(3.1.1) (3.1.2)
其中 x ( k ) R 是随机状态变量， yi (k ) R i 是第 i 个传感器对状态的随机观测信号，
因此由假设 1 得到(3.2.7)和(3.2.8)。
3.3
状态融合 Kalman 滤波器
首先，利用定理 3.2.1 并根据第二节定理 2.2.1-2.2.3 得到最优状态融合 Kalman 滤波器。 定理 3.3.1 多传感器系统(3.1.1)和(3.1.2)在假设 1、2 下，按矩阵加权最优状态融合
ˆ i (k | k )(i 1, , L) 和 Pij (k | k )(i, j 1, , L) 由定理 3.2.1 确定。 x
定理 3.3.2 多传感器系统(3.1.1)和(3.1.2)在假设 1、2 下，按标量加权最优状态融合
L
L L
Kalman 滤波器为：
s ˆ0 ˆ i (k | k ) ， P0s (k | k ) ais (k )a sj (k ) Pij (k | k ) x (k | k ) ais (k ) x i 1
A
(3.2.8)
Qεi (k ) H i Pi (k | k 1) H iT Qvi ， K i ( k ) Pi ( k | k 1) H i T Qε1 ( k )
i
ˆ (1 | 0) A 0 B u (0) ， P (1| 0) AP0 A CQC 。 初值 x
证明：只需证明(3.2.4)和(3.2.5)。 由(3.2.1)和(3.2.2)知， yi ( k ) H i x ( k ) vi ( k ) H i ( Ax ( k 1) Cw ( k 1)) vi ( k ) ，代 入(3.2.3)得到，
ˆ i ( k | k ) K iA x ˆ ( k 1 | k 1) K i ( k )[ H i A x ( k 1) H i C w ( k 1) v i ( k )] x
其中 kl
1, k l 。 0, k l
假设 2： x (0) 均值为 0 ，方差阵为 P0 ，并且与 w ( k ) 和 vi ( k )(i 1, , L) 不相关（正 交） ，即
E ( x (0)) 0 , E[( x (0) 0 )( x (0) 0 )T ] P0 E[( x (0) 0 ) w (k )T ] 0 ， E[( x (0) 0 )vi (k )T ] 0(i 1, , L) ˆ i (k | k ) 和预报器 问题是求状态 x (k ) 的基于第 i 个传感器的局部最优 Kalman 滤波器 x ˆ i (k 1| k )(i 1, , L) ，并求在三种加权融合估计准则下的状态融合 Kalman 滤波器 x ˆ 0 (k | k ) 和预报器 x ˆ 0 (k 1| k ) 。 x
并且局部预报误差互协方差阵为( i j )：
(3.2.7)
Pij (k 1| k ) K iA (k ) Pij (k 1| k ) K jA (k )T CQw C T
其中 K i ( k ) A[ I K i ( k ) H i ] ， K ip ( k ) AK i ( k ) ，
3.2 局部 Kalman 滤波器和预报器
首先对第 i 个传感器：
x (k 1) Ax (k ) Cw (k ) yi (k ) H i x (k ) vi (k )
利用第四章推论 3.2.1 马上得到局部 Kalman 滤波器：
(3.2.1) (3.2.2)
定理 3.2.1(局部 Kalman 滤波器) 系统(3.2.1)和(3.2.2)在假设 1、2 下的递推 Kalman 滤 波器为：
T
，
trP trP 11 ( k | k ) 1L ( k | k ) ˆ Ptr (k | k ) ，xi (k | k )(i 1, , L) 和 Pij (k | k ) 由定理 3.2.1 trP ( k | k ) trP ( k | k ) LL L1
其
中
d d ad aLj (k ) j ( k ) a1 j ( k ) T
( P j (k | k )) 1 e eT ( P j (k | k )) 1 e
，
e 1 1
T
，
jj jj P P 11 ( k | k ) 1L ( k | k ) jj P j (k | k ) ， Pit (k | k ) 为 Pit (k | k ) 的 第 (j,j) 元 素 ， jj PLjj 1 (k | k ) PLL (k | k )
ˆ1 ( k | k ) P I P x 11 ( k | k ) 1L ( k | k ) ˆ 其 中 e ， P (k | k ) ， x (k | k ) ， I P (k | k ) P (k | k ) x LL L1 ˆ L (k | k )
E ( w (k )) 0, E ( w (k ) w (l )T ) Qw kl
E (vi (k )) 0, E (vi (k )vi (l )T ) Qvi kl
E ( w (k )vi ( j )T ) 0 ， E (vi (k )v j (l )T ) 0(i j )
(3.2.5)
Qεi (k ) H i Pi (k | k 1) H iT Qvi ， K i ( k ) Pi ( k | k 1) H i T Qε1 ( k )
i
Pi (k | k 1) APi (k 1| k 1) A CQw C
T
T
ˆ (0 | 0) 0 ， P (0 | 0) P0 。 并且初值 x