《卡尔曼滤波教学》PPT课件
合集下载
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
AS ˆ((k k 1 ) )H(K C )[(X k S ˆ(()k k A 1))(]
(6-61) 由（6－56）～（6－61）可以画出卡尔 曼滤波对 S (k )进行估计的递推模型，如 图6.13所示
• 输入为观测值X(k)，输出为信号估计 值 Sˆ (k) 。
X(k) X~(k) H(k)
X ~(k)X(k X ˆ()k) (6-60)
显然，新息的产生是由于我们前面忽略 了w1(k)与 w(k)所引起的
• 用新息X~(k)乘以一个修正矩阵 H(k )，用 它来代替式（6－56）的w1(k来) 对S (k )进 行估计：
S ˆ(k A )S ˆ( k 1 )H ) X ~ ((k k))
令 Cε ((kk))τ C R (k (k S ) τ )S ，Uε(k)C(kτ ) 代入上式化简：
ε(k ) ε(k H ) (τK U)H τ U H (k (τ ) H k)τ(S kS
ε ( k U )τ ) ( 1 U S τ [S H U (τ ) k 1 ( ]S ) [S H U τ ( ) 1 ( ] k τS
Xˆ (k) C(k)
Sˆ (k)
z 1
A(k ) Sˆ (k 1)
图6.13 卡尔曼滤波的一步递推法模型
６.２.２ 卡尔曼滤波的递推公式 从图6.13容易看出，要估计出 Sˆ (k) 就必须 要先找到最小均方误差下的修正矩阵
H (k )，结合式（6－61）、（6－56）、 （6－57）得：
S ˆ(k A )S ˆ( ( k k 1 H ) ) (K (k w )) [ ( C C k()S ˆ k ( k ) 1S A )
z w1(k ) S(k1) 1
S (k ) C(k)
w(k )
X(k)
A(k1)
• 图6.12 卡尔曼滤波的信号模型
• 【例6-1】设卡尔曼滤波中量测方程为
• X(k S) (k w ) (k，) 已知信号的自相关 函数的z变换为R ss (z)(10.8z0 .1 3 )1 ( 60.8z),0.8z1.25
把（1）代入（2）、（3）式，消去 ε(k，) 再
把（2）和（3）联立，得到
ε(k0 ).6ε4( k1 )0.36 H(k) 0.6ε4( k1 )1.36
（5）
初始条件为 Sˆ(1)0,(0)1，k＝0开始观
测，利用等式（4），（5）进行递推得： • k＝0，(0) 1.0000，H(0) 1.0000，Sˆ(0)X(0) • k＝1，(1) 0.5000，H（1） 0.5000，S ˆ(1)0.4S ˆ(0)0.5X(1) • k＝2，(2) 0.4048，H(2) 0.4048，S ˆ(2)0.47S ˆ(1 6)2 0.40X(4 2) • k＝3，(3) 0.3824，H(3) 0.3824，S ˆ(3)0.49S ˆ(2 4)1 0.38X(2 3) • k＝4，(4) 0.3768，H(4) 0.3768，S ˆ(4)0.49S ˆ(38 )0 5.37X(4 6)8 • k＝5，(5) 0.3755，H(5) 0.3755，S ˆ(5)0.49S ˆ(4 9)60.37X(5 5)5 • k＝6，(6) 0.3751，H(6) 0.3751，S ˆ(6)0.49S ˆ(5 9 )0 9 .37 X(6 5) 1 • k＝7，(7) 0.3750，H(7) 0.3750，S ˆ(7)0.50S ˆ(6 0 )0 0.37X(7 5)0 • 上面是递推过程，还没有达到稳态的情况。
声统计，已知
Sˆ(1)0,(在0)k1 ＝0时刻
开始观测信号。试用卡尔曼滤波的公式
求 和Sˆ (k) ，ε(k＝) 0,1,2,3,4,5,6,7；以及
稳态时的 和Sˆ (k) 。ε(k)
解：由例6-6的结果知，A(k)0.8 C(k) 1
Q(k)w 21 0.36 R( v ka w )(k r)( )1
(6-68) 要使得均方误差最小，则必须 H(k U )Sτ ()S 10
求得最小均方误差下的修正矩阵为： H (εk (k ))τ[C C ε( ((k k k))τ ) C R( (1 k k (6) -)69]) 把上式代入(6-61)即可得均方误差最小条 件下的 Sˆ (k) 递推公式。 最小均方误差为：
X(k C ) (k) Sw (k(k) ()6-55)
上式中的称为量测矩阵，它的引入原因 是，量测矢量的维数不一定与状态矢量 的维数相同，因为我们不一定能观测到 所有需要的状态参数。
６.１.２ 信号模型
根据状态方程 S( A k)(k 1) )w S 1 ((k k 1 和量测方程 X(k C ) (k) Sw (k(k)，)卡 尔曼滤波的信号模型，如图6.12所示。
(6-62)
根据上式来求最小均方误差下的 H(k)，然后把 求到的H (k )代入（6－61）则可以得到估计 值Sˆ (k) 。
• 设真值和估计值之间的误差为：
S ~(k)S( kS ˆ()k)
误差是个矢量，因而均方误差是一个矩
阵，用 ε(k )表示。把式（6－62）代入得
S ~(k)S( kS ˆ()k)
• 把式（6－66）代入（6－67）得
ε(k) [ IH(Kε()k C )H ([k I()k]τ)H C(k)k ]R τ) (
ε ( k H ) (ε K ( k ε ( ))k C τ ) H ( C τ k ( H ( k )k ( )ε ) k (k )τ [ ) R C C( ( (τ k k k
６.２ 卡尔曼滤波方法
（The method of Kalman filtering）
把状态方程和量测方程重新给出： S(A k)(k 1) )w S 1((k k 1(6)-56) X(k C ) (k) Sw (k(k) ) (6-57)
假设信号的上一个估计值 Sˆ(k 1)已知， 现在的问题就是如何来求当前时刻的 估计值 。Sˆ (k)
• 用上两式得到的和分别用和表示，得： S ˆ(k)A(S ˆk (k )1) (6-58)
X ˆ(k)C(S ˆk (k ))C(kS ˆ)(A k 1(k )(6-)59) • 必然，观测值 X(k)和估计值Xˆ (k) 之间有
误差，它们之间的差 X~(k)称为新息 （innovation）：
• 噪声的自相关函数为Rww (m)(m) ，
信号和噪声统计。求卡尔曼滤波信号模 型中的 A和(k) C(k。)
• 解：根据等式 R ss (z)w 21A (0z).3A 6(zz 11z)
可以求得
(10.8z1)(10.8z)
z1
S(z)
A(z)
10.8z1 W1(z)
变换到时域得：s (k 1 ) 0 .8 s (k ) w 1 (k ) 因此 A(k)0.8 又因为X(k S) (k w ) (，k) 所以C(k) ＝1。
《卡尔曼滤波教学》PPT课件
本课件PPT仅供学习使用 本课件PPT仅供学习使用 本课件PPT仅供学习使用
学习完毕请自行删除
第一节 卡尔曼滤波信号模型 第二节 卡尔曼滤波方法
第三节 卡尔曼滤波的应用
６.１ 信号模型
• 维纳滤波的模型：信号 s(n)可以认为是
由白噪声 w1(n)激励一个线性系统 A( z)的
S(A k)(k 1) )w S 1((k k 1)(6-53)
上式表示的含义就是在k时刻的状态 S (k ) 可以由它的前一个时刻的状态 S (k1)来 求得，即认为k－1时刻以前的各状态都 已记忆在状态 S(k1)中了
卡尔曼滤波是根据系统的量测数据（即 观测数据）对系统的运动进行估计的， 所以除了状态方程之外，还需要量测方 程。
ε(k ) ε(k)U(τS )1S Uτ
[IH(k)Cε(k))]
(6-70)
• 综上所述，得到卡尔曼滤波的一步递推 公式：
ε(k) Aε (k ( k 1 ) )A τQ (k ()1k) (6-71)
H (εk (k ))τ[C C ε( ((k k k))τ ) C R( (1 k (6k -)7)2)]
(6-63)
[ I H(K A( ) k 1 C S ˆ ) (( [ k 1 k S w )) 1 ( ]] k 1 [ H )] (k
均方误差矩阵：
ε( k E)S ~([k S ~()k τ])
(6-64)
表示对向量取共轭转置。
为了计算方便，令 ε(kE ) [( S S ˆ((k k))) (S ˆS (k (τ]k )))(6-65) 找到和均方误差矩阵的关系：
ε ( E k) [ 1 ( w 1 A ( ) 1 k A ( ) S ˆ ( k 1 k () k ) k S ) 1 ) w ( ( 1 ( ) A k 1 k A ) S ( ˆ ( k 1 k ( τ ] k
A 1 ( S ˆ ( k ) 1 k ) ) 1 E S ˆ ) ( ) 1 ( k [ τ ] S S ) τ A E ) ( 1 ( k ( 1 k k [ Байду номын сангаас ( k w ) 1 k τ ] ) w
• 把上式代入式(6-71)～ (6-74)得
ε(k) 0.6ε4( k 1 )0.36
（1）
H (ε k (k )ε()k [1 ]) 1
（2）
ε(k ) [1H(k ε()k])
（3）
Sˆ (k) 0.8S ˆ(k 1 )H(K )0[.8S X ˆ(k (1k （))4]）
求逆 H(ε k (k )ε()k [ 1 ]) 1 ε(k/[ε)(k1 )]
A S ˆ ( ( k 1 k H ))( A K S ˆ ( ( k 1 ) k w 1 ) [ ( ) C k 1 w )( ] C k (k ) S ( ˆ ( [ k ) k 1) )
A S ˆ (( k 1 k H )) [I ( H k)(C ( K S ˆ k ( ( k 1 ) k w C ) 1 () k 1 ] ( H ) k ]) (
Aε (k ( k 1 ) )A τQ (k ()1k)
(6-66)
把式（6－63）代入式（6－64），最后 化简得：
ε( k E)S ~([k S ~()k τ])
[ I H(K Aε )( C (k 1 k ) ) (τ A k Q )( ] 1 k ([k ) )H ][( Ik τ H )C (k (k k τ ))R
响应，假设响应和激励的时域关系可以 用下式表示：
s (n ) a(n s 1 ) w 1 (n 1 ) (6-52) 上式也就是一阶AR模型。
• 在卡尔曼滤波中信号 s(n)被称为是状态 变量，用矢量的形式表示为 S (k )，激励 信号w1(n) 也用矢量表示为w1 (k)，激励和 响应之间的关系用传递矩阵 A(k)来表示， 得出状态方程：
在卡尔曼滤波中，用表示量测到的信号 矢量序列，表示量测时引入的误差矢量， 则量测矢量与状态矢量之间的关系可以 写成
X(k S) (k w ) (k) (6-54)
X(k S) (k w ) (k)
上式和维纳滤波的概念上是一致的，也 就是说卡尔曼滤波的一维信号模型和维 纳滤波的信号模型是一致的。
把式(6-55)推广就得到更普遍的多维量 测方程
• 假设到了某一时刻k－1，前后时刻的均方误差
相等，也就是误差不再随着递推增加而下降，
达到最小的均方误差了，即稳态情况，式（5）
中的误差
(k)(k 代入1)（5）式可以计算
到稳态时的均方误差为： (k)(k1) 即0.稳375
态时的修正矩阵 H(k)0，.3代75入式4得稳态 时的信号估计： Sˆ (k) 0.5S ˆ(k1 )0.37X5(k)
ε(k ) [IH(k)Cε((kk))]
(6-73)
Sˆ (k) AS ˆ((k k 1 ) )H(K C )[(X k S ˆ(()k k A 1))(]k
(6-74)
【例6-2】设卡尔曼滤波中量测方程为
X(k S) (k w ) (已k知) 信号的自相关函数的z
变换为 Rss(z)(10.8z0.13 )1( 60.8z),0.8z1.25 ，噪声 的自相关函数为 Rww (m)(m)，信号和噪
(6-61) 由（6－56）～（6－61）可以画出卡尔 曼滤波对 S (k )进行估计的递推模型，如 图6.13所示
• 输入为观测值X(k)，输出为信号估计 值 Sˆ (k) 。
X(k) X~(k) H(k)
X ~(k)X(k X ˆ()k) (6-60)
显然，新息的产生是由于我们前面忽略 了w1(k)与 w(k)所引起的
• 用新息X~(k)乘以一个修正矩阵 H(k )，用 它来代替式（6－56）的w1(k来) 对S (k )进 行估计：
S ˆ(k A )S ˆ( k 1 )H ) X ~ ((k k))
令 Cε ((kk))τ C R (k (k S ) τ )S ，Uε(k)C(kτ ) 代入上式化简：
ε(k ) ε(k H ) (τK U)H τ U H (k (τ ) H k)τ(S kS
ε ( k U )τ ) ( 1 U S τ [S H U (τ ) k 1 ( ]S ) [S H U τ ( ) 1 ( ] k τS
Xˆ (k) C(k)
Sˆ (k)
z 1
A(k ) Sˆ (k 1)
图6.13 卡尔曼滤波的一步递推法模型
６.２.２ 卡尔曼滤波的递推公式 从图6.13容易看出，要估计出 Sˆ (k) 就必须 要先找到最小均方误差下的修正矩阵
H (k )，结合式（6－61）、（6－56）、 （6－57）得：
S ˆ(k A )S ˆ( ( k k 1 H ) ) (K (k w )) [ ( C C k()S ˆ k ( k ) 1S A )
z w1(k ) S(k1) 1
S (k ) C(k)
w(k )
X(k)
A(k1)
• 图6.12 卡尔曼滤波的信号模型
• 【例6-1】设卡尔曼滤波中量测方程为
• X(k S) (k w ) (k，) 已知信号的自相关 函数的z变换为R ss (z)(10.8z0 .1 3 )1 ( 60.8z),0.8z1.25
把（1）代入（2）、（3）式，消去 ε(k，) 再
把（2）和（3）联立，得到
ε(k0 ).6ε4( k1 )0.36 H(k) 0.6ε4( k1 )1.36
（5）
初始条件为 Sˆ(1)0,(0)1，k＝0开始观
测，利用等式（4），（5）进行递推得： • k＝0，(0) 1.0000，H(0) 1.0000，Sˆ(0)X(0) • k＝1，(1) 0.5000，H（1） 0.5000，S ˆ(1)0.4S ˆ(0)0.5X(1) • k＝2，(2) 0.4048，H(2) 0.4048，S ˆ(2)0.47S ˆ(1 6)2 0.40X(4 2) • k＝3，(3) 0.3824，H(3) 0.3824，S ˆ(3)0.49S ˆ(2 4)1 0.38X(2 3) • k＝4，(4) 0.3768，H(4) 0.3768，S ˆ(4)0.49S ˆ(38 )0 5.37X(4 6)8 • k＝5，(5) 0.3755，H(5) 0.3755，S ˆ(5)0.49S ˆ(4 9)60.37X(5 5)5 • k＝6，(6) 0.3751，H(6) 0.3751，S ˆ(6)0.49S ˆ(5 9 )0 9 .37 X(6 5) 1 • k＝7，(7) 0.3750，H(7) 0.3750，S ˆ(7)0.50S ˆ(6 0 )0 0.37X(7 5)0 • 上面是递推过程，还没有达到稳态的情况。
声统计，已知
Sˆ(1)0,(在0)k1 ＝0时刻
开始观测信号。试用卡尔曼滤波的公式
求 和Sˆ (k) ，ε(k＝) 0,1,2,3,4,5,6,7；以及
稳态时的 和Sˆ (k) 。ε(k)
解：由例6-6的结果知，A(k)0.8 C(k) 1
Q(k)w 21 0.36 R( v ka w )(k r)( )1
(6-68) 要使得均方误差最小，则必须 H(k U )Sτ ()S 10
求得最小均方误差下的修正矩阵为： H (εk (k ))τ[C C ε( ((k k k))τ ) C R( (1 k k (6) -)69]) 把上式代入(6-61)即可得均方误差最小条 件下的 Sˆ (k) 递推公式。 最小均方误差为：
X(k C ) (k) Sw (k(k) ()6-55)
上式中的称为量测矩阵，它的引入原因 是，量测矢量的维数不一定与状态矢量 的维数相同，因为我们不一定能观测到 所有需要的状态参数。
６.１.２ 信号模型
根据状态方程 S( A k)(k 1) )w S 1 ((k k 1 和量测方程 X(k C ) (k) Sw (k(k)，)卡 尔曼滤波的信号模型，如图6.12所示。
(6-62)
根据上式来求最小均方误差下的 H(k)，然后把 求到的H (k )代入（6－61）则可以得到估计 值Sˆ (k) 。
• 设真值和估计值之间的误差为：
S ~(k)S( kS ˆ()k)
误差是个矢量，因而均方误差是一个矩
阵，用 ε(k )表示。把式（6－62）代入得
S ~(k)S( kS ˆ()k)
• 把式（6－66）代入（6－67）得
ε(k) [ IH(Kε()k C )H ([k I()k]τ)H C(k)k ]R τ) (
ε ( k H ) (ε K ( k ε ( ))k C τ ) H ( C τ k ( H ( k )k ( )ε ) k (k )τ [ ) R C C( ( (τ k k k
６.２ 卡尔曼滤波方法
（The method of Kalman filtering）
把状态方程和量测方程重新给出： S(A k)(k 1) )w S 1((k k 1(6)-56) X(k C ) (k) Sw (k(k) ) (6-57)
假设信号的上一个估计值 Sˆ(k 1)已知， 现在的问题就是如何来求当前时刻的 估计值 。Sˆ (k)
• 用上两式得到的和分别用和表示，得： S ˆ(k)A(S ˆk (k )1) (6-58)
X ˆ(k)C(S ˆk (k ))C(kS ˆ)(A k 1(k )(6-)59) • 必然，观测值 X(k)和估计值Xˆ (k) 之间有
误差，它们之间的差 X~(k)称为新息 （innovation）：
• 噪声的自相关函数为Rww (m)(m) ，
信号和噪声统计。求卡尔曼滤波信号模 型中的 A和(k) C(k。)
• 解：根据等式 R ss (z)w 21A (0z).3A 6(zz 11z)
可以求得
(10.8z1)(10.8z)
z1
S(z)
A(z)
10.8z1 W1(z)
变换到时域得：s (k 1 ) 0 .8 s (k ) w 1 (k ) 因此 A(k)0.8 又因为X(k S) (k w ) (，k) 所以C(k) ＝1。
《卡尔曼滤波教学》PPT课件
本课件PPT仅供学习使用 本课件PPT仅供学习使用 本课件PPT仅供学习使用
学习完毕请自行删除
第一节 卡尔曼滤波信号模型 第二节 卡尔曼滤波方法
第三节 卡尔曼滤波的应用
６.１ 信号模型
• 维纳滤波的模型：信号 s(n)可以认为是
由白噪声 w1(n)激励一个线性系统 A( z)的
S(A k)(k 1) )w S 1((k k 1)(6-53)
上式表示的含义就是在k时刻的状态 S (k ) 可以由它的前一个时刻的状态 S (k1)来 求得，即认为k－1时刻以前的各状态都 已记忆在状态 S(k1)中了
卡尔曼滤波是根据系统的量测数据（即 观测数据）对系统的运动进行估计的， 所以除了状态方程之外，还需要量测方 程。
ε(k ) ε(k)U(τS )1S Uτ
[IH(k)Cε(k))]
(6-70)
• 综上所述，得到卡尔曼滤波的一步递推 公式：
ε(k) Aε (k ( k 1 ) )A τQ (k ()1k) (6-71)
H (εk (k ))τ[C C ε( ((k k k))τ ) C R( (1 k (6k -)7)2)]
(6-63)
[ I H(K A( ) k 1 C S ˆ ) (( [ k 1 k S w )) 1 ( ]] k 1 [ H )] (k
均方误差矩阵：
ε( k E)S ~([k S ~()k τ])
(6-64)
表示对向量取共轭转置。
为了计算方便，令 ε(kE ) [( S S ˆ((k k))) (S ˆS (k (τ]k )))(6-65) 找到和均方误差矩阵的关系：
ε ( E k) [ 1 ( w 1 A ( ) 1 k A ( ) S ˆ ( k 1 k () k ) k S ) 1 ) w ( ( 1 ( ) A k 1 k A ) S ( ˆ ( k 1 k ( τ ] k
A 1 ( S ˆ ( k ) 1 k ) ) 1 E S ˆ ) ( ) 1 ( k [ τ ] S S ) τ A E ) ( 1 ( k ( 1 k k [ Байду номын сангаас ( k w ) 1 k τ ] ) w
• 把上式代入式(6-71)～ (6-74)得
ε(k) 0.6ε4( k 1 )0.36
（1）
H (ε k (k )ε()k [1 ]) 1
（2）
ε(k ) [1H(k ε()k])
（3）
Sˆ (k) 0.8S ˆ(k 1 )H(K )0[.8S X ˆ(k (1k （))4]）
求逆 H(ε k (k )ε()k [ 1 ]) 1 ε(k/[ε)(k1 )]
A S ˆ ( ( k 1 k H ))( A K S ˆ ( ( k 1 ) k w 1 ) [ ( ) C k 1 w )( ] C k (k ) S ( ˆ ( [ k ) k 1) )
A S ˆ (( k 1 k H )) [I ( H k)(C ( K S ˆ k ( ( k 1 ) k w C ) 1 () k 1 ] ( H ) k ]) (
Aε (k ( k 1 ) )A τQ (k ()1k)
(6-66)
把式（6－63）代入式（6－64），最后 化简得：
ε( k E)S ~([k S ~()k τ])
[ I H(K Aε )( C (k 1 k ) ) (τ A k Q )( ] 1 k ([k ) )H ][( Ik τ H )C (k (k k τ ))R
响应，假设响应和激励的时域关系可以 用下式表示：
s (n ) a(n s 1 ) w 1 (n 1 ) (6-52) 上式也就是一阶AR模型。
• 在卡尔曼滤波中信号 s(n)被称为是状态 变量，用矢量的形式表示为 S (k )，激励 信号w1(n) 也用矢量表示为w1 (k)，激励和 响应之间的关系用传递矩阵 A(k)来表示， 得出状态方程：
在卡尔曼滤波中，用表示量测到的信号 矢量序列，表示量测时引入的误差矢量， 则量测矢量与状态矢量之间的关系可以 写成
X(k S) (k w ) (k) (6-54)
X(k S) (k w ) (k)
上式和维纳滤波的概念上是一致的，也 就是说卡尔曼滤波的一维信号模型和维 纳滤波的信号模型是一致的。
把式(6-55)推广就得到更普遍的多维量 测方程
• 假设到了某一时刻k－1，前后时刻的均方误差
相等，也就是误差不再随着递推增加而下降，
达到最小的均方误差了，即稳态情况，式（5）
中的误差
(k)(k 代入1)（5）式可以计算
到稳态时的均方误差为： (k)(k1) 即0.稳375
态时的修正矩阵 H(k)0，.3代75入式4得稳态 时的信号估计： Sˆ (k) 0.5S ˆ(k1 )0.37X5(k)
ε(k ) [IH(k)Cε((kk))]
(6-73)
Sˆ (k) AS ˆ((k k 1 ) )H(K C )[(X k S ˆ(()k k A 1))(]k
(6-74)
【例6-2】设卡尔曼滤波中量测方程为
X(k S) (k w ) (已k知) 信号的自相关函数的z
变换为 Rss(z)(10.8z0.13 )1( 60.8z),0.8z1.25 ，噪声 的自相关函数为 Rww (m)(m)，信号和噪