最优估计之线性连续系统卡尔曼滤波

t 0 取极限
黎卡提微分方程：
t 0
lim P(t t, t ) P(t, t ) P(t )
(t ) A(t ) P(t ) P(t ) AT (t ) G(t )Q(t )G T (t ) P P(t ) H T (t ) R 1 (t ) H (t ) P(t )
t
G (t ) E[ w(t )~ z T ( )]R 1 ( )~ z ( )d
t0
t
~ ˆ A(t ) X (t ) K (t )Z (t )
ˆ (t ) K (t )[ Z (t ) H (t ) X ˆ (t )] A(t ) X ˆ (t ) K (t )Z (t ) [ A(t ) K (t )H (t )] X
•
步骤3：确定增益阵 K(t)
K (t ) E[ x(t )~ z T (t )]R 1 (t )
ˆ (t )][ H (t ) ~ E{x(t )~ z T (t )} E{[~ x (t ) x x (t ) v(t )]T } ˆ (t ) ~ E{~ x (t ) ~ x T (t ) H T (t )} E{x x T (t ) H T (t )} P(t ) H T (t )
P(t ) H T (t ) R 1 (t ) H (t ) P(t )
注：连续系统的卡尔曼滤波估计问题归结为求解微分方程问题；
矩阵黎卡提微分方程很难求解。
11
线性连续系统卡尔曼滤波方程
12
两点说明：
ˆ (t ) 是 X (t ) 在 Z t Z ( ), t t 条件下的均值,即 1、X t0 0 ˆ (t | t ) E X (t ) | Z t X t0 是线性最小方差估计。
线性连续系统 (t ) A(t ) x(t ) G (t ) w(t ) x z (t ) H (t ) x(t ) v(t ) 框图如下：
x (t0 )
w(t )
G (t )
v (t ) x (t )
H (t )
+源自文库
+ +
1 s
A(t )
+
z (t )
滤波方程： ˆ (t ) A(t ) x ˆ (t ) K (t )~ x z (t ) 可视为一个 K (t )~ z (t ) 作用下的线性系统，其 结构图如下：


2、 由线性最小方差估计的 正交投影性质， 估计误差正交于测量量 ， 也正交于估计量， 即 ~ ~ ˆT T E[ X (t ) Z (t )] E[ X (t ) X (t )] 0 ~ ~ ˆT T E[ Z (t ) Z (t )] E[ Z (t ) Z (t )] 0
13
将其代入• (8.16)，得：
P(t t , t t ) P(t , t ) [ A(t ) P(t , t ) P(t , t ) AT (t ) t K (t t ) H (t t ) P(t , t ) T G (t )Q(t )G (t )] t
(8.1.1)
噪声统计特性： E[ w(t )] 0 E[ w(t ) wT ( )] Q(t ) (t ) E[v(t )] 0 E[v(t )vT ( )] R(t ) (t ) E[ w(t )vT ( )] 0，E[ x(t ) wT (t ) • ] 0，E[ x(t )vT (t ) • ] 0 t , t0 E[ x(t0 )] x (t0 )，Var[ x(t0 )] P x (t0 )
ˆ (t t ) [ I A(t )t ]x ˆ (t ) K (t t )z (t t ) x ˆ (t ) H (t t )[ I A(t )t ]x K (t ) ˆ (t )， 将上式两端同减x 并除以 t， 得：
ˆ (t t ) x ˆ (t ) x K (t t ) ˆ (t ) A(t ) x [ z (t t ) t t ˆ (t )] H (t t )[ I At ]x
z (t t ) H (t t ) x(t ) v n (t t )
其中，(t t , t ) I n A(t )t (t t , t ) G (t )t
Qk Cov[W (t ),W ( )] kj t Rk n n Cov[V (t ),V ( )] kj t
最优估计
第8章 线性连续系统 卡尔曼滤波

离散系统取极限的推导方法 卡尔曼滤波方程新息推导法 线性连续系统滤波器的一般形式 滤波的稳定性及误差分析
• •
研究连续系统的必要性：实际的物理系统往往是连续的，故离散 系统的描述不能完全代替连续时间系统。 线性连续系统模型：
(t ) A(t ) x(t ) G (t ) w(t ) x z (t ) H (t ) x(t ) v(t )
得等效离散线性系统的卡尔曼滤波方程：
ˆ (t t ) (t t , t ) x ˆ (t ) K (t t )[ z (t t ) x ˆ (t )] H (t t )(t t , t ) x
(8.1.3)
K (t t ) P(t t , t ) H T (t t )[H (t t ) P(t t , t ) (8.1.4) R(t t ) 1 T H (t t ) ] t P(t t , t ) (t t , t ) P(t , t )T (t t , t ) Q(t ) T (t t , t ) (t t , t ) t
方差估计， 定义 ~ ˆ (t ) z (t ) z (t ) H (t ) x 为新息过程。 新息中包含 z (t ) 的新成份。
•
新息的性质：新息是一个与测量噪声有相同统计值的白噪 声过程。
16
推导过程
•
步骤1：构造估计量的函数形式 ˆ (t ) 是 ~ 假定 x z (t ) 的线性函数：
t0
* (t , s) R( s)
估计与测量的正交性
ˆ(t )~ E[ x z T (s)] E[ x(t )~ x T (s)]
* (t , ) E[ x(t )~ z T (s)]R 1 (s)
17
ˆ(t ) E[ x(t )~ x z T ( )]R 1 ( )~ z ( )d
ˆ (t0 ) x
z (t )

+ +
K (t )
1 s
A(t )
ˆ (t ) x
ˆ (t ) z
H (t )
8.2 卡尔曼滤波方程新息推导法
•
系统模型：
(t ) A(t ) x(t ) G (t ) w(t ) x z (t ) H (t ) x(t ) v(t )
•
新息：设 x ˆ (t ) 为由 z (t ) 在 t0 ~ t 区间的 Z tt0 得到的 X (t ) 最小
k , k 1 (t t , t ) k , k 1 (t t , t ) H k H (t t ) Pk |k P (t t , t t )
Pk 1|k 1 P (t t , t ), K k K (t t ) Q(t ) R (t t ) Qk 1 , Rk t t
•
问题： 给定测量 Z (t ) (t t )， 使 0 求式(8.1.1)状态估计 X (t )， ~ ~ Pt E[ X (t ) X T (t )]
最小的线性估计。
8.1 离散系统取极限的推导方法
推导方法思想：当采样稠密或采样间隔趋于零时，取离散系统
的极限，将离散系统的结果转化为连续系统的公式。
T 1
t
P(t t , t ) H (t t ) H (t t ) P(t t , t ) H (t t )t R(t t )
T T


1
t 0
取极限
----------- 增益矩阵
K (t ) P(t ) H T (t ) R 1 (t )
推导方法步骤：
• • •
步骤1：建立(8.1.1)的等效离散线性系统数学描述 步骤2：求等效离散模型的卡尔曼滤波方程
当 t 0 时 对离散卡尔曼滤波公式取极限 步骤3：
4
•
步骤1：建立(8.1.1)的等效离散线性系统数学描述 由 5.3 知，等效模型： x(t t ) (t t , t ) x(t ) (t t , t ) wn (t )
t 0 取极限
---(8.1.8) 最优滤波方程
ˆ(t ) A(t ) x ˆ(t ) K (t )[ z(t ) H (t ) x ˆ(t )] x
线性连续系统的卡尔曼滤波方程，是一个一阶微分方程。
K (t t ) K (t ) t R(t t ) T P(t t , t ) H (t t ) H (t t ) P(t t , t ) H (t t ) t
----------- 估计误差方差
线性连续系统卡尔曼滤波求解公式
最优滤波方程： ˆ(t ) A(t ) x ˆ(t ) K (t )[ z(t ) H (t ) x ˆ(t )] x
滤波增益方程：
K (t ) P(t ) H T (t ) R 1 (t )
滤波误差方差矩阵黎卡提方程： (t ) A(t ) P(t ) P(t ) AT (t ) F (t )Q(t ) F T (t ) P
将 (t t , t ) I n A(t )t，(t t ) G(t )t 代入 (8.1.5) 式，得： Q (t ) T T P (t t , t ) [ I A(t )t ]P (t t , t )[ I A(t )t ] G (t )t G (t )t t P (t , t ) [ A(t ) P (t , t ) P (t , t ) AT (t ) G (t )Q (t )G T (t )]t
ˆ (t ) * (t , )~ x z ( )d
t0
t
选择 * (t , )， 以得到 x(t ) 的最小方差估计。
t T ~ ˆ (t ) z ( s)] * (t , )E[~ E[ x z ( )~ z T ( s)]d t0 t
* (t , )R( ) ( s )d
P(t t , t t ) [ I K (t t ) H (t t )]P(t t , t )
(8.1.5)
(8.1.6)
•
当 t 0 时 对离散卡尔曼滤波公式取极限 步骤3：
将 (t t , t ) I n A(t )t 代入滤波方程(8.1.3) 式，得：
t0
t
•
步骤2：对上述函数关于时间求导
ˆ (t ) E[ x(t )~ x z T (t )]R 1 (t )~ z (t ) (t )~ E{x z T ( )}R 1 ( )~ z ( )d
t0
t ~ K (t ) z (t ) A(t ) E[ x(t )~ z T ( )]R 1 ( )~ z ( )d t0
n n
t t0 kt , t0 jt
•
步骤2：求等效离散模型的卡尔曼滤波方程
利用离散线性系统卡尔曼滤波方程(132页)及下列等效关系：
xk x(t t ), z k z (t t ), xk 1 x(t ), Pk |k 1 P (t t , t ),