第二讲近似高斯滤波

张永安 非线性/非高斯滤波讲义

第二讲 近似高斯滤波

2.1 泰勒线性化和推广卡尔曼滤波

给定随机系统的动态滤波问题，系统包括两个过程： (1) 状态过程（信号过程）：具有初始分布0~()0x p x ,转移核为()1|k k p x x −的马尔科夫过程； (2) 观测过程：观测量与状态量k z k x 有概率关联()|k k p z x 。 若系统具有

设系统具有线性、高斯性，亦即具有以下性质： SSM Σ∈S S (A1) 可以写成线性状态空间模型形式： S

⎩⎨⎧+=+=Σ−k k k k

k

k k k

v x C z w x A x 1LSSM :(A2) 和服从高斯分布，即k w k v GSSM Σ∈S ； (A3) 状态初始分布为高斯分布：

000ˆ~(;,0)x x x

P N

以上(A2)与(A3)合称高斯假设，三个假设合起来线性高斯假设，具有线性高斯假设的模型称为线性高斯模型，其全体记为。则可以证明，若服从高斯分布： LGSSM Σ)|(1:11−−k k z x p

11:111|11|1ˆ(|)~(;,k k k k k k k p x z x x

P −−−−−−−)N

)|(1:1−k k z x p 和也是高斯的，

)|(:1k k z x p 1:1|1|1ˆ(|)~(;,)k k k k k k k p x z x x P −−−N

1:||ˆ(|)~(;,)k k k k k k k p x z x x

P N 且这三个高斯分布的参数（状态的均值和协方差阵）满足卡尔曼滤波递推公式，类似于贝叶

斯递推滤波公式，卡尔曼滤波分两部分： 一步预测和测量修正。其算法如下：

算法2.1 (卡尔曼滤波): (1) 给定

0|00|0,P x (2) 递推计算：其中

",1,0=k (a) 一步预测： k k k k k k q x A x

+=−−−1|11|ˆˆ

k T k k k k k k Q A P A P +=−−−1|11|(b) 测量修正：

11|1|)(−−−+=k T

k k k k T k k k k R C P C C P K )ˆ(ˆˆ1|1||k k k k k k k k k k r x C z K x x

−−+=−−

T

k k k T k k k k k k k k K R K C K I P C K I P +−−=−)()(1||

由于状态的条件分布完全由其均值和协方差阵完全表征，因此卡尔曼滤波是递推贝叶斯最优滤波的显式解，在给定的线性高斯假设条件下与贝叶斯最优滤波完全等价。

若系统是非线性的，且具有加性高斯输入噪声，即GSSM S ∈Σ，则由第一讲可知，系统

可表示为

S

1()()k k k k k k k

k

x f x w z g x v −=+⎧⎨

=+⎩

其中，，与分为独立同分布(i.i.d)白噪

声输入，且相互独立，它们具有高斯分布，且均值和协方差阵 x w x

n n n k f ℜ→ℜ×ℜ

:z v x n n n k g ℜ→ℜ×ℜ:k v k w

k k r Ev =，k k q Ew =，

[][]

{}T

k k k k k E v r v r R −−=， [][]{}T k

k

k

k

k

E w q w q Q −−=

ASSM Σ∈S 可以写为如下形式：

11GSSM (|)[;(),]

:(|)[;(),]

k k k k k k k k k k k k k k p x x x f x q Q p z x z g x r R −−=+⎧Σ⎨

=+⎩N N 人们通过各种非线性近似，来获得近似解。最基本的近似方法是泰勒近似法，其思路是：当状态的先验分布可以用高斯分布近似时，状态的条件分布完全由其均值和协方差阵表征，若在状态的滤波值和预测值周围分别将状态方程和测量方程展开泰勒级数：

k k k k k k k k k k k k w x x x x A x

f x +−Δ+−+=−−−−−−−−)ˆ()ˆ()ˆ(1|1111|111|1

|1|12|1ˆˆˆ()()()k k k k k k k k k k k k z g x

C x x x x v −−=+−+Δ−−+这里和为二阶以上被截去掉的高阶项， )ˆ(1|111−−−−Δk k k x

x )ˆ(1|2−−Δk k k x x

1|11ˆ1

1)

(−−−=−−∂∂=

k k k x

x k k k k x x f A ，|1ˆ()

k k k k k k x k

g x C x x

−=∂=

∂

为线性化的雅可比阵，则可得GSSM Σ∈S 的局部线性化系统

⎩⎨⎧++=++=Σ−k

k k k k k

k k k k v d x C z w c x A x ~

~:1LSSM

其中 1|11|1ˆ)ˆ(−−−−−=k k k k k k k x A x

f c

1|1|ˆ)ˆ(−−−=k k k k k k k x C x

g d

在分别给定和后为确定性分量，而 1|1ˆ−−k k x

1|ˆ−k k x k k k k k w x x w +−Δ=−−−)ˆ(~1|111

k k k k

k v x x v +−Δ=−)ˆ(~1|2

为随机输入，注意它们包含了线性化误差，因而它已经是非高斯的，我们分别称它们为虚拟

过程噪声和虚拟测量噪声。忽略线性化带来的误差时，k

k k k v v w w ≈≈~,~，此时局部线性化系统具有线性、高斯模型，从而可以利用线性系统的卡尔曼滤波公式，递推的求解

，即我们可以近似假定状态的条件分布是高斯的：

)|(:1k k z x p

11:111|11|11:1|1|11:||ˆ(|)(;,ˆ(|)(;,)ˆ(|)(;,)k k k k k k k k k k k k k k k k k k k k k p x z x x

P p x z x x

P p x z x x

P −−−−−−−−−−⎧≈⎪

≈⎨⎪≈⎩N N N )而中的参数的递推公式为当前最常用的非线性滤波公式——推广卡尔曼滤波(EKF)。

)|(:1k k z x p

算法2.2 (EKF)：

(1) 给定先验高斯分布的均值和协方差估计： 00,ˆP x

(2) 递推计算：其中

",1,0=k (a) 一步预测状态估计：

1|11ˆ1

−−−=−=

k k k x

x k k k dx df A

k k k k k k q x f x

+=−−−)ˆ(ˆ1|11|

k T

k k k k k k Q A P A P +=−−−1|11|(b) 测量修正：

|1ˆk k k k

k x x

k

dg C dx −==