中文第二章卡尔曼滤波器共20页

式
f a1cG
s ˆ n n a s ˆ n 1 n 1 G n x n a s ˆ n 1 c n 1
新息
一步预测: a s ˆn 1 n 1 s ˆn n 1
第二步预测: x ˆ n n 1 c s ˆ n n 1 a s ˆ n c 1 n 1
用测量模型来更新先验概率分布，从而获得需要的滤波结果：
pxk|z1:kpzkp |x k zkp |z 1 x :kk 1 |z1:k1
(2)
p z k |z 1 :k 1 p z k|x k p x k|z 1 :k 1 d x k
两个步骤递归计算就构成了最优的贝叶斯估计。遗憾的是，式和在很多场合
下没有可分解的计算方法，所以它们只是一个理论上的解。基于特定分布的
假设，如高斯分布可以获得最优估计的解析的计算方法 。
卡尔曼滤波
卡尔曼滤波器认为后验概率在任何时刻都是高斯分布的，这样由均
值和方差就可以完全确定其概率分布。可以证明，如果 pxk1|z1:k1 是高 斯的，那么要使 pxk |z1:k 也是高斯的话，隐含了下面的假设：
信号矢量 噪声矢量
a1 0 0
A
0
a2
0
0 0 a q
sn As n 1 w n
参数矩阵 信号模型的矩阵形式
信号矢量：例2
s1 n as1 n 1 bs2 n 1 wn s2 n s1 n 1
s
n
s1 s2
n n
wn
wn
0
sn Asn 1 wn
号为k时的观测矢量。观测量和系统状态之间的关系为：
zk hkxk,nk
pzk xk k0
v和n分别为方差为Q和R的高斯白噪声 需要注意的是：这里x表示信号状态，z表示观察/测量值。
贝叶斯估计
假设需要计算的后验分布 pxk1|z1:k1在时刻k-1已经得到，那
么我们利用状态模型可以获得时刻k状态的先验概率分布：
a b A 1 0
观察/测量矢量
xi n cisi n vi n i 1, 2, , k (k q)
xn x1n x2 n xk nT
vn v1n v2 n vk nT
Hale Waihona Puke c1 0 C0 0
c2 0
0 0
0 0
ck
0
xn Cs n vn
测量模型的矩阵形式
p x k |z 1 : k 1 p x k |x k 1p x k 1 |z 1 : k 1 d x k 1
(1)
注意：做了如下假设（即认为状态模型为一阶马尔科夫过程）：
px k|x k 1 ,z1 :k 1px k|x k 1
在k时刻可以获得新的观测矢量Zk，基于贝叶斯准则可以利
•R.E. Kalman (1960)
•Optimal? formulating the MMSE linear filtering problem (causal IIR Wiener filter)
•Recursive? The time-recursive processing of the input data
新息（Innovation）: n x n x ˆ n n 1 x n a s ˆ n 1 c n 1
卡尔曼增益：
Gn R ccP 2 n PnPR ncc2
预测误差功率：
PnEe12na2n1Q
e1nsnsˆnn1 预测误差
估计误差功率和预测误差功率关系：
Initiation s ˆ 0 0 , 0 P 1 G 1 1 ,s ˆ 1 1
信号矢量：例1
（同时估计若干个信号）
s i n a i s i n 1 w i n ,i 1 ,2 , ,q
sn s1 n s2 n sq n T w n w1 n w2 n wq n T
内容
2.1 卡尔曼滤波器 2.2 由因果IIR维纳滤波器看卡尔曼滤波器 2.3 从bayes滤波角度看卡尔曼滤波器 2.4 卡尔曼滤波器的扩展
2.1 卡尔曼滤波器
• What is Kalman filter? An optimal recursive data processing algorithm.
v和n都是参数已知的高斯分布
fk xk1,vk 是 x k 1 和 v k 的线性函数 hk xk,nk 是 x k 和n k 的线性函数
xk Fkxk1vk
zk Hkxknk
(1) (2)
px k 1 |z 1 :k 1 N x k 1 ;m k 1 |k 1 ,P k 1 |k 1
2.2 维纳滤波器的迭代实现
信号模型和测量模型： snasn1wn xncsnvn
因果IIR维纳滤波器 （前面推导结果）：
s ˆn n ,s ˆn n 1 ,x ˆn n 1
分别代表用n时刻以及n-1时刻及以 前所有数据对s(n)和x(n)的估计值
迭
代
差分方程
形
s ˆ n n fs ˆ n 1 n 1 G n x n
nE e2n Een sns ˆnn Eensn
PncG nPn
1cG nPnPn ensnsˆnn 估计误差
结构框图
计算步骤
P na2 n 1 Q
Gn RccP2nPn
n 1 cn G P n
s ˆ n n a s ˆ n 1 n 1 G n x n a s ˆ n 1 c n 1
矢量卡尔曼滤波器的计算公式
snAsn1wn xnCsnvn
标量算术 矢量算术
ab aba2 a2 b 1ab A BAB ATA AB T A A B T
PnAnn1n1ATnQn
GnPnCTnCnPnCTnRn1
nnIGnCnPn
sˆnnAnsˆn1n1GnxnAnCnsˆn1n1
2.3卡尔曼滤波的统计原理
状态模型和观察信号模型 贝叶斯滤波 卡尔曼滤波
状态模型和观测模型
假设实际系统的状态序列为xk ,k¥ ，其中k为时间序列标
号，xk ¡ nx 表示时间标号为k时的状态矢量，nx ¥ 为状态矢量的
维数。状态间的转移关系为
xkfk xk1,vk
pxk xk1 k0
系统观测到的序列为zk,k¥，其中 zk ¡ nz 表示时间标