第五章 非线性滤波(含粒子滤波)

1/ 2 T 1 ˆ ˆ exp x x Σ 1 x x d x 2





ˆ ˆ y n n-1 y n y1:n 1 , y n p x n y1:n 1 dx n y n N x n ; x n n-1PnXX dx n n-1
XX ˆ ˆ ˆn PnYY 1 R h(x n )hT (x n ) N x n ; x n n-1 , p n n-1 dx n y n|n 1y T|n 1 n XX ˆ ˆ ˆn PnXY1 x n hT (x n ) N x n ; x n n-1 , p n n-1 dx n x n|n 1y T|n 1 n
现代数字信号处理
非线性信号滤波
滤波的信号模型
• 统计状态转换方程
– 联系当前状态与以前 状态
x n f (x n 1 , w n 1 )
噪声
• 统计观察/测量方程
– 联系观察数据与当前 状态
yn h(x n , vn )
滤波方法
信号模型 滤波方法
• 线性，加性高斯噪声 • 非线性，加性高斯噪声
• 先验预测互相关矩阵
XX T PnXY1 p n n-1 H n n
XX ˆ 初始估计：x0|0 P0|0
非线性卡尔曼滤波
XX ˆ ˆ x n n-1 f (x n-1 ) N x n-1 ; x n-1 n-1 , p n-1 n-1 dx n-1

XX ˆ ˆ ˆn PnXX1 f (x n-1 ) f T (x n-1 ) N x n-1 ; x n-1 n-1 , p n-1 n-1 dx n-1 x n|n 1xT|n 1 Q n
n 1:n
遗憾的是，上式在很多场合下（非线性非高斯）没有可分解的计算方法。 因此常常采用一些近似的方法求解上面的积分。
两种可分解情况
在两种情况下有可分解的计算方法： 1。离散状态空间 2。线性模型，高斯噪声。（Kalman filter）
p xn | y1:n 1 x p xn | xn 1 , y1:n 1 p xn 1 | y1:n 1


XX ˆ 初始估计：x0|0 P0|0
卡尔曼滤波（线性模型）
xn Fn xn1 wn1
y n Hn xn v n
如果信号模型为线性，噪声为加性高斯噪声，则前面几个 假设真实成立。并且如果已知n-1时刻的后验均值和方差， 则先验和n时刻的后验均值和方差可以轻松算出。
线性卡尔曼滤波过程
x n y1:n 1 x n p x n y1:n 1 dx n x n N x n ; f (x n-1 ), Q p x n-1 y1:n 1 dx n-1 dx n x n N x n ; f (x n-1 ), Q dx n p x n-1 y1:n 1 dx n-1
n1
p x n | y1:n

p y n | x n p x n | y1:n 1
xn
p y n | x n p x n | y1:n 1
g (x n )
p ( xn | y1:n )
x g (xn ) p(xn | y1:n )
n
（二）卡尔曼滤波器
一个非线性随机系统可以由一个统计的状态转换方程 (1) xn fn xn 1 , wn 1 和一个统计的观察/测量方程 y n h n xn , v n (2)
共同定义。
贝叶斯框架下， 公式（1）确定了预测当前状态的条件转换概率(给定前一 时刻的状态和所有的观测值)： (*1) p x n x n-1 , y1:n 1 公式（2）确定了预测当前观测值的似然概率(给定当前状 态)： (*2) p y n xn
-SISPF
粒子退化问题；Rao-Blackwellasation PF; 粒子滤波器应用
• 高 斯 积 分 卡 尔 曼 滤 波 器
无重采样粒子滤波器 基于高斯分布的 粒子滤波器
Bootstrap Particle Filter
-BPF
•高斯积分粒子滤波器
•无色粒子滤波器 •MC粒子滤波器
（一） 贝叶斯滤波
ˆ ˆ y n n-1 y n N x n ; x n n-1PnXX n-1

XX ˆ ˆ ˆn PnYY 1 h(x n )hT (x n ) N x n ; x n n-1 , p n n-1 dx n y n|n 1y T|n 1 R n XX ˆ ˆ ˆn PnXY1 x n hT (x n ) N x n ; x n n-1 , p n n-1 dx n x n|n 1y T|n 1 n
XX ˆ p x n-1 y1:n 1 N x n-1 ; x n-1 n-1 , Pn-1 n-1 设 n-1时刻后验概率为高斯分布：
f (x n-1 ) p x n-1 y1:n 1 dx n-1
XX ˆ ˆ x n n-1 f (x n-1 ) N x n-1 ; x n-1 n-1 , p n-1 n-1 dx n-1
• 卡尔曼滤波
• 扩展卡尔曼滤波；基于 高斯积分，无色变换的 卡尔曼滤波
• 非线பைடு நூலகம்，非高斯非加性 噪声
• 粒子滤波器
非线性滤波
通用贝叶斯非线性滤波 加性高斯噪声 • 扩 展 卡 尔 曼 滤 波 器 •MC 卡 尔 曼 滤 波 器 • 无 色 卡 尔 曼 滤 波 器
非加性高斯噪声
重采样粒子滤波器
Sequential Importance Sampling Particle Filter
通用卡尔曼滤波过程
预测
• 状态预测（先验均值）和预测误差功 率（先验方差）
XX ˆ ˆ xn n-1 f (x n-1 ) N x n-1 ; x n-1 n-1 , p n-1 n-1 dx n-1
更新
XX ˆ ˆ ˆn PnXX1 Q f (x n-1 ) f T (x n-1 ) N x n-1 ; x n-1 n-1 , p n-1 n-1 dx n-1 x n|n 1xT|n 1 n • • 观察值预测和预测方差 ˆ ˆ y n n-1 y n N xn ; xn n-1PnXX dx n n-1




• 计算卡尔曼增益
Kn P
XY n n 1 YY Pn n 1 1

• 先验预测互相关矩阵
XX ˆ ˆ ˆn PnYY 1 R h(x n )hT (x n ) N x n ; x n n-1 , p n n-1 dx n y n|n 1y T|n 1 n
YY T Pn n 1 R H n PnXX1 H n |n
• 使用观察值更新预测（后验 均值）和估计误差功率（后 验方差）
ˆ ˆ ˆ xn n xn n-1 K n y n y n|n1
YY PnXX PnXX1 K n Pn n 1K T n n n
p xn xn-1 , y1:n 1 N (xn ; f (xn-1 ), Q)
xn f (xn-1 )
先验概率： p xn | y1:n 1 N xn ; f (xn 1 ), Q p x n 1 | y1:n 1 dx n 1 当前状态的先验估计： xn y1:n 1 xn p xn y1:n 1 dxn

ˆ p x n y1:n 1 N x n ; x n n-1 , PnXX 设 n时刻先验概率为高斯分布： n-1
XX ˆ ˆ ˆn PnXX1 Q f (x n-1 ) f T (x n-1 ) N x n-1 ; x n-1 n-1 , p n-1 n-1 dx n-1 x n|n 1xT|n 1 n



使用观察值更新预测（后验 均值）和估计误差功率（后 验方差）
ˆ ˆ ˆ xn n xn n-1 K n y n y n|n1
YY PnXX PnXX1 K n Pn n 1K T n n n
ˆ ˆ ˆn PnXY1 xn hT (xn ) N xn ; xn n-1 , p nXX dx n x n|n 1yT|n 1 n n-1
K P
YY n n n 1
K
T n
Kn P
XY n n 1
P
YY n n 1

1
取后验均值作为状态的估计值。
卡尔曼滤波器
认为后验概率以及先验概率在任何时刻都是高斯分布的，这样由均值和 方差就可以完全确定其概率分布（注意前面的3个假设）。
[1] Peter S. Maybeck, Stochastic models, estimation and control, Academic Press, New York, San Francisco, London, 1979 [2]A.J.Haug, A tutorial on bayesian estimation and tracking techniques applicable to nonlinear and non-Gaussian Processes, MTR 05W0000004,July,2005
预测
• 状态预测（先验均值）和预测误差功 率（先验方差）
更新
ˆ ˆ xn|n 1 Fn xn 1|n 1
F
T k
• 计算卡尔曼增益
Kn P
XY n n 1 YY Pn n 1 1
P
XX n| n 1
Q Fn P
XX n 1| n 1
• 观察值预测和预测方差 ˆ ˆ y n n-1 H n xn n-1


dx


n


求解：
g ( x)
ˆ N ( x; x , Σ )
ˆ g (x) N (x; x, Σ)d x
（三）高斯积分的数值近似求解-----高斯-尔米特（Gauss-Hermite）积分
I g ( x) g ( x)
ΣS S
T
ˆ N ( x;x , Σ )
ˆ g ( x) N ( x; x, Σ) d x
贝叶斯滤波
假设n-1时刻状态的后验分布已经得到，那么我们利用条件 转移概率可以获得n时刻状态的先验分布：
p xn | y1:n 1 p xn | xn 1 , y1:n 1 p x n 1 | y1:n 1 dxn 1
在n时刻可以获得新的观测矢量 y n ，基于贝叶斯准则可以利 用似然模型来更新先验概率分布，从而得到n时刻状态的后 验概率： p y | x p x | y








设 n时刻后验概率也为高斯分布，则有（当加性高斯噪声，且线性模型
时可精确推得下面公式[1]；文献[2]推导了一般情况下，下面公式可用来近似后验概率为高斯分布）
ˆ ˆ ˆ xn n xn n-1 K n y n y n|n1
P
XX nn
P
XX n n 1
p x n | y1:n
n
p y n | y1:n 1
n
n
1:n 1
p yn | y1:n 1 p yn | yn p xn | y1:n 1 dxn
迭代滤波问题，通常就是在给定观测值 y1:n情况下计算当前 状态的某个函数的期望（如前两阶矩）。即： g (xn ) p ( x |y ) g (xn ) p(xn | y1:n )d x n
状态转换方程
观察/测量方程
xn fn xn 1 w n 1
y n hn xn v n
W,V为 互不相关的均值为0，方差为Q,R的高斯加性噪声； f(),h(), Q,R 已知且不随时间改变， 。
贝叶斯框架下， 状态方程确定了预测当前状态的条件转换概率为高斯分布：