U-卡尔曼滤波在状态估计中的应用
合集下载
相关主题
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
G 外 - ( 和 0( 还满足 2 )% d 8 9 0(- 43 ( 4 给定量测 / 时 的 迭 代 估 计 可 以 表示成以下形 y ( (
式%
第 7期增刊 "
卡尔曼滤波在状态估计中的应用 K#
7 -
的最佳预测& 的最佳预 ’( ! * * ! #$ % # #) % #+ % # % , &
! !
$1 4 6 . $1 4 % +! & % +! &6 . ! 8 ! ! ! f f f f f f f
G
"
"
[
"G
$1 4 $4 6 !6 ! ! f f ff
G f G f
"
f z8 | 8 4 % & % &6 $ g 8$1 !+ ! !+ ! f f { } 8 y 对任意 # 计算 a 点3 ~o . k. !p . b c dG
然后 +,只是简单地将所有非线性模 型 线 性 化 d 再利用线性卡尔曼滤波方法 d +,给出的是 最 佳 估 计 的 一阶近似 *一 +,在实际使用中存在 明 显 的 缺 陷 d 二是 是线性化有可能产生极不稳定的滤波 ! +,需计 算" 这在多数情况下不是一件容易 H s E #矩 阵 的 导 数 d 的事 *本文介绍一种新的推广卡尔曼滤波 (( )x 卡尔 曼滤波# 通 过 仿 真 实 例 说 明 有 效 地 克 服 ’ d d )+, )+, 了它给出的至少是最佳估计的二阶近 +,的 缺 陷 d 似*
K % 1
仪
* ,
器
仪
表
学
报
第1 卷 J
&’( + !"# ) ) " # " $% " $% " $%
1 2
/$
!3 . " & 0 53 + " # " $% 3 &4
1 2
量测更新 6
88 ( $. ( $. !3 !3 7 ""..& 0 5 3 + " # " $% + " # " $% 3 &4
第1 :卷第 :期增刊
仪
器
仪
表
学
报
1 % % ;年 &月
卡尔曼滤波在状态估计中的应用 " !
周桃庚 郝 群
北京
沙定国
北京理工大学信息科学技术学院 # 摘要
$ % % % & $ ’
本文介绍了新的滤波估计方法 () 卡尔曼滤波 *)+,能给出最佳估计的至少二阶近似 *本文通过一个时间序列的仿真 ) 变换 状态估计
测& &
假设 ! 的先验估计 " 和当前量测 * 为高斯 随 ! # # +# 机变量. 此 迭 代 得 到 的 解 是 //0 这里不需假设 1的 . 模型是线性的 2 此最佳迭代解可由下式给出 3 4 % & 6 ! 5 ! # $1 # ++- 9 + 99 $8 $* +" ( * * # !*8 **. # # #
b
% l &
"+
+ #
! # $ n gb e b . # " # +b $f
% d& !
万方数据
! b +_
‘ _
gb $i ‘ % _ ’h & b $. k. ‘ _
4 +" 4 +" 8 $ n gb e ! e ! b . # #6 b . # #6 " # +" # +! + ! +
[
b $f
"G
G f
"G
[
^和 8 就是这些变换点的 函数得到变换后的点 . 而且 * * * 均值和协方差 2 均值为 \ !和 协 方 差 为 8 的 _维 随 机 变 量 !可 以
! !
$4 e ! # +# +-
G
"G
"G
’# ! +# 6 ! # +# +-
"G
G 式中 . $j % _ ’h & 8 # # +-
"
G [ [ [ [
L M & N变换 % M O P Q R O S R TN U V O P W X U YV S Z X O
K[ 变换是一种计算一个随机变量 的 非 线 性 变 换 的统计量的方法 2 假设 !是一随机变量 . 其均值 \ !的协 它 同 !成 非 线 性 关 系 . 方差 8 . *是 另 一 随 机 变 量 . ! ! ^ 需计算 的均值 和协方差 $] % & . 2 * ! * * 8 * * 变换是选择一些点使得其样本 均 值和样本协 K[ \ 方差分别为 !和 8 . 将这些点中的每个点代入非线性
N q H q J
~ 引
言
个假设d 卡尔曼滤波给出了状态的均值和方差的最佳 的可能估计 * 要求均值和方差是可测量的在实际应用 中没有困难d 但是要求所有的量测和状态模型是线性 在实际应用中是不现实的 * 的d 为了将卡尔曼滤波应用到非线性情况d 就产生了 推广卡尔曼滤波 # ’ * +, +,不能说是卡尔曼滤波的 下面简 +,对所有的非线性信息用线性近似来代替 * 要介绍一下推广卡尔曼滤波以及其存在的缺陷 * 考虑的系统可用下面的非线性离散时间的状态转 移方程来表示 %
"+
M 卡 尔 曼 滤 波 % M O P Q R O S R T uV v YV O & w Z v S R U Z O x t
现考虑非线性卡尔曼滤波 % 和% & ‘ & 2 将状态和状 态噪声变量和量测噪声变量合成一个变量考虑 . 即3 $4 ! ! g # y#6 # # G 对! 应用 K[ 变换 2 可得到 K=>的计算公式如下 3 # g 初始值 3
用‘ 点近似表示 3 ’-个加权 a _ b c dG \ e$! . g $h i % _ ’h &
f f
时间更新 3 $5 4 . 6 e e e # " # +# +# +‘ _
! ! g
\ j% e $! ’% _ ’h & 8 & b $. k_ b ! b \ j% e $! +% _ ’h & 8& . b $_ ’. k. ‘ _
此算法具有以下性质 3
但对非线性模型 . 1 =>得到这些值的近似值 3
"+ "+
99 % & . ?8 ! ( ! # ?5 # +# !*8 **.
# # # #
"
+-
因为 !的均值和协方差可以准确到二阶 . 这样 % & 从这可 *的 均 值 和 协 方 差 的 计 算 值 也 可 以 达 到 二 阶 . 看出 . 的均值的计算值的准确度要比 1 =>要高 . K=> 而协方差的准确度至少一样 2 这些 a % ‘ & b c dG点 俘 获 到 的 均 值 和 协 方 差 不 会 因 不同的平方根方法而改变 2 因此可以采用效率高 q 数值 稳定的 C ; I r s a # *方法 2 可 用 标 准 的向量和矩阵运算来计算均值和协 % , & 方 差. 实 现 速 度 很 快. 因不需计算 1 => 中 需 计 算 的 F G H I J矩阵 2
% * ! # $; #& 协 方 差 的 确 定 首 先 将 状 态 模 型 线 性 化. ! # ’ -? 然后对此线性系 统 确 定 其 后 ’A . ?C ’D . @! B * ! E # # # # # 此后验协方差矩阵就认为是非线性模 验协方差矩阵. 型的协方差 2 换句话说 . 在1 状态分布近似为高 =>中 . 他 通 过 非 线 性 系 统 的 一 阶 线 性 化 传 递. 也就 斯 分 布. 是. 虽 然. 1 => 给 出 的 是 最 佳 估 计 的 一 阶 近 似 . 1 => 也可做到二阶. 但由于其计算的复杂性使其实现比较 因此在实际应用中基本不予考虑二阶的情况 2 一 困难 . 阶近似可能将很大的误差引入到变换后的随机变量的 特别是在局部线性化的假设不 真实均值和协方差中. 成立时 . 线性化会产生极不稳定的滤波 2另外 . 1 > >需 计算 F 这在大多数情况下不是一件 G H I J矩 阵 的 导 数 . 简单的事 2 而 K=>可以克服 1 =>这些缺陷 2
其中 . 是矩阵% 的平方根 %j% _ ’h & 8 ’h & _ 8 b ! ! !& 矩阵的第 b 行或第 b 列. 点的权 2变换过程如 g b是第 b 下3 将‘ % & ’ -个 a _ b c dG点 代 入 非 线 性 函 数 得 到 一 系列变换点 . $] % & mb e b
‘ _
完全推广 d 只是一个用线性去近似非线性的粗糙方法 d
来自百度文库
$ 推广卡尔曼滤波方法及其缺陷
对于线性系 统d 卡尔曼滤波在保证状态分布的前 两阶矩 # 均值和协方差 ’ 的一致性上提供了最佳解决方 卡尔曼滤波利用了以下事实 % 法* & 只给出一个分布的 均值和方差# 协方差’ 对此分布的最保守的假设是此 d 分布为高斯分布 其均值和方差为给出的均值和方差 * d 万方数据 ’ 高斯分布经过线性运算后还是高斯分布 * 基于这两
# # # #
"+
"
% 7 & % : & % < &
^ 求出变换点的均值 . $ n gb % ‘ & * mb
b $f
求出变换点的协方差 . % , &
‘ _
[ ^ ^ $ n gb o +* & p o +* p mb mb 8 * * b $f
"+
4 % 6 ; ! * # $1 #&
"+
" + 和 " + 分别为 这里 . 和* 的最佳预测 2 ! * ! # # # # 在线性情况下 . 卡尔曼滤波可以准确计算这些量 2
U UG H qu J n H n qn J s E K rE p r J pH o o p E y P vH q P E KE w J n q P vH q P E K n zKJ y H vo u JE w q P vJn J p P J nn q H q JJ n q P vH q P E KP nI P t J K D J
实例说明 )+,比 +,的估计精度要高 * 关键词 推广卡尔曼滤波 ) 卡尔曼滤波
. / / 0 1 2 3 4 1 5 6 7 5 8 ! 6 7 2 9 6 4 9 :;3 0 <3 6= 1 0 4 9 > 1 6 ?5 64 @ 9A 4 3 4 9B 7 4 1 <3 4 1 5 6
C D E FG H E I J K I LH E MF K N D HO P K I I F E
论
88 &7 7 7 " " $: ..:
本文首先对推广卡尔曼滤波的缺陷进行了分析+ 在此基础上介绍了 X 变换 + 然后给出了 X 卡尔曼滤波 的计算公式 O 最后给出了一个时间序列的状态估计的 例子 O 仿真结果表明 + XVW比 U VW的精度要高 O
" "
/$
/$
9
1 2
( $* ( $. 7 ) !3 *.& 0 5 3 3 + " # " $% "+ " # " $% "3 &4
" "
/$
/$
9
图 1 状态协方差的估计
88 :&7 *.7 ..
" "
$%
" "
/
&* $/ * . . " ;: = "> " "
< $ $ 9
" "
/$
_ 结
)* # ’ ,- ( # $ ’ y y ( ( +$ 其中 d *是状态模型 d - ( 是 .维系统噪声 * 观测值和系统状态矢量的关系有下面方程给出 %
)D # ’ ,0( # 1 ’ / y ( ( 其 中d 观测矢量d D是 观 测 模 型 d / 0( 是 量 测 噪 声 * 假 (
设噪声 - ( 和 0( 都是不相关的零均值的高斯白噪 声 d G 且 对任意 ( 和另 d 2 )% d 2 )5 d 7% ! 0(3 0(043 6 5 ( ( 4 (
# U de de j j j k i dl ’ QR S T V WX Y W V Z [\ T ] V Y^ R _ ‘ Y V a V b c R ] f ] Y bX Y g T ] T h T RV W^ R _ ‘ Y V a V b c R ] f ] Y bi ‘ ] Y \
x U . m 7 4 > 3 2 4 G D P no H o J pP K q p E r F s J rHK E t J u vJ q D E rw E pw P u q p P K IJ n q P vH q P E K F K n s J K q J r+H u vH Kw P u q J p P K I )+,I P t J n U n P vF u H q P E Kn D E {nq D H q )+,u J H r nq EvE p JH s s F p H q Jp J n F u q n ;9 |} 5 > : 7 y q J K r J r +H u vH Kw P u q J p P K I J n q P vH q P E K )K n s J K q J r +H u H vK w P u q J p P K I )K n s J K q J rG p H K n w E p vH q P E K
式%
第 7期增刊 "
卡尔曼滤波在状态估计中的应用 K#
7 -
的最佳预测& 的最佳预 ’( ! * * ! #$ % # #) % #+ % # % , &
! !
$1 4 6 . $1 4 % +! & % +! &6 . ! 8 ! ! ! f f f f f f f
G
"
"
[
"G
$1 4 $4 6 !6 ! ! f f ff
G f G f
"
f z8 | 8 4 % & % &6 $ g 8$1 !+ ! !+ ! f f { } 8 y 对任意 # 计算 a 点3 ~o . k. !p . b c dG
然后 +,只是简单地将所有非线性模 型 线 性 化 d 再利用线性卡尔曼滤波方法 d +,给出的是 最 佳 估 计 的 一阶近似 *一 +,在实际使用中存在 明 显 的 缺 陷 d 二是 是线性化有可能产生极不稳定的滤波 ! +,需计 算" 这在多数情况下不是一件容易 H s E #矩 阵 的 导 数 d 的事 *本文介绍一种新的推广卡尔曼滤波 (( )x 卡尔 曼滤波# 通 过 仿 真 实 例 说 明 有 效 地 克 服 ’ d d )+, )+, 了它给出的至少是最佳估计的二阶近 +,的 缺 陷 d 似*
K % 1
仪
* ,
器
仪
表
学
报
第1 卷 J
&’( + !"# ) ) " # " $% " $% " $%
1 2
/$
!3 . " & 0 53 + " # " $% 3 &4
1 2
量测更新 6
88 ( $. ( $. !3 !3 7 ""..& 0 5 3 + " # " $% + " # " $% 3 &4
第1 :卷第 :期增刊
仪
器
仪
表
学
报
1 % % ;年 &月
卡尔曼滤波在状态估计中的应用 " !
周桃庚 郝 群
北京
沙定国
北京理工大学信息科学技术学院 # 摘要
$ % % % & $ ’
本文介绍了新的滤波估计方法 () 卡尔曼滤波 *)+,能给出最佳估计的至少二阶近似 *本文通过一个时间序列的仿真 ) 变换 状态估计
测& &
假设 ! 的先验估计 " 和当前量测 * 为高斯 随 ! # # +# 机变量. 此 迭 代 得 到 的 解 是 //0 这里不需假设 1的 . 模型是线性的 2 此最佳迭代解可由下式给出 3 4 % & 6 ! 5 ! # $1 # ++- 9 + 99 $8 $* +" ( * * # !*8 **. # # #
b
% l &
"+
+ #
! # $ n gb e b . # " # +b $f
% d& !
万方数据
! b +_
‘ _
gb $i ‘ % _ ’h & b $. k. ‘ _
4 +" 4 +" 8 $ n gb e ! e ! b . # #6 b . # #6 " # +" # +! + ! +
[
b $f
"G
G f
"G
[
^和 8 就是这些变换点的 函数得到变换后的点 . 而且 * * * 均值和协方差 2 均值为 \ !和 协 方 差 为 8 的 _维 随 机 变 量 !可 以
! !
$4 e ! # +# +-
G
"G
"G
’# ! +# 6 ! # +# +-
"G
G 式中 . $j % _ ’h & 8 # # +-
"
G [ [ [ [
L M & N变换 % M O P Q R O S R TN U V O P W X U YV S Z X O
K[ 变换是一种计算一个随机变量 的 非 线 性 变 换 的统计量的方法 2 假设 !是一随机变量 . 其均值 \ !的协 它 同 !成 非 线 性 关 系 . 方差 8 . *是 另 一 随 机 变 量 . ! ! ^ 需计算 的均值 和协方差 $] % & . 2 * ! * * 8 * * 变换是选择一些点使得其样本 均 值和样本协 K[ \ 方差分别为 !和 8 . 将这些点中的每个点代入非线性
N q H q J
~ 引
言
个假设d 卡尔曼滤波给出了状态的均值和方差的最佳 的可能估计 * 要求均值和方差是可测量的在实际应用 中没有困难d 但是要求所有的量测和状态模型是线性 在实际应用中是不现实的 * 的d 为了将卡尔曼滤波应用到非线性情况d 就产生了 推广卡尔曼滤波 # ’ * +, +,不能说是卡尔曼滤波的 下面简 +,对所有的非线性信息用线性近似来代替 * 要介绍一下推广卡尔曼滤波以及其存在的缺陷 * 考虑的系统可用下面的非线性离散时间的状态转 移方程来表示 %
"+
M 卡 尔 曼 滤 波 % M O P Q R O S R T uV v YV O & w Z v S R U Z O x t
现考虑非线性卡尔曼滤波 % 和% & ‘ & 2 将状态和状 态噪声变量和量测噪声变量合成一个变量考虑 . 即3 $4 ! ! g # y#6 # # G 对! 应用 K[ 变换 2 可得到 K=>的计算公式如下 3 # g 初始值 3
用‘ 点近似表示 3 ’-个加权 a _ b c dG \ e$! . g $h i % _ ’h &
f f
时间更新 3 $5 4 . 6 e e e # " # +# +# +‘ _
! ! g
\ j% e $! ’% _ ’h & 8 & b $. k_ b ! b \ j% e $! +% _ ’h & 8& . b $_ ’. k. ‘ _
此算法具有以下性质 3
但对非线性模型 . 1 =>得到这些值的近似值 3
"+ "+
99 % & . ?8 ! ( ! # ?5 # +# !*8 **.
# # # #
"
+-
因为 !的均值和协方差可以准确到二阶 . 这样 % & 从这可 *的 均 值 和 协 方 差 的 计 算 值 也 可 以 达 到 二 阶 . 看出 . 的均值的计算值的准确度要比 1 =>要高 . K=> 而协方差的准确度至少一样 2 这些 a % ‘ & b c dG点 俘 获 到 的 均 值 和 协 方 差 不 会 因 不同的平方根方法而改变 2 因此可以采用效率高 q 数值 稳定的 C ; I r s a # *方法 2 可 用 标 准 的向量和矩阵运算来计算均值和协 % , & 方 差. 实 现 速 度 很 快. 因不需计算 1 => 中 需 计 算 的 F G H I J矩阵 2
% * ! # $; #& 协 方 差 的 确 定 首 先 将 状 态 模 型 线 性 化. ! # ’ -? 然后对此线性系 统 确 定 其 后 ’A . ?C ’D . @! B * ! E # # # # # 此后验协方差矩阵就认为是非线性模 验协方差矩阵. 型的协方差 2 换句话说 . 在1 状态分布近似为高 =>中 . 他 通 过 非 线 性 系 统 的 一 阶 线 性 化 传 递. 也就 斯 分 布. 是. 虽 然. 1 => 给 出 的 是 最 佳 估 计 的 一 阶 近 似 . 1 => 也可做到二阶. 但由于其计算的复杂性使其实现比较 因此在实际应用中基本不予考虑二阶的情况 2 一 困难 . 阶近似可能将很大的误差引入到变换后的随机变量的 特别是在局部线性化的假设不 真实均值和协方差中. 成立时 . 线性化会产生极不稳定的滤波 2另外 . 1 > >需 计算 F 这在大多数情况下不是一件 G H I J矩 阵 的 导 数 . 简单的事 2 而 K=>可以克服 1 =>这些缺陷 2
其中 . 是矩阵% 的平方根 %j% _ ’h & 8 ’h & _ 8 b ! ! !& 矩阵的第 b 行或第 b 列. 点的权 2变换过程如 g b是第 b 下3 将‘ % & ’ -个 a _ b c dG点 代 入 非 线 性 函 数 得 到 一 系列变换点 . $] % & mb e b
‘ _
完全推广 d 只是一个用线性去近似非线性的粗糙方法 d
来自百度文库
$ 推广卡尔曼滤波方法及其缺陷
对于线性系 统d 卡尔曼滤波在保证状态分布的前 两阶矩 # 均值和协方差 ’ 的一致性上提供了最佳解决方 卡尔曼滤波利用了以下事实 % 法* & 只给出一个分布的 均值和方差# 协方差’ 对此分布的最保守的假设是此 d 分布为高斯分布 其均值和方差为给出的均值和方差 * d 万方数据 ’ 高斯分布经过线性运算后还是高斯分布 * 基于这两
# # # #
"+
"
% 7 & % : & % < &
^ 求出变换点的均值 . $ n gb % ‘ & * mb
b $f
求出变换点的协方差 . % , &
‘ _
[ ^ ^ $ n gb o +* & p o +* p mb mb 8 * * b $f
"+
4 % 6 ; ! * # $1 #&
"+
" + 和 " + 分别为 这里 . 和* 的最佳预测 2 ! * ! # # # # 在线性情况下 . 卡尔曼滤波可以准确计算这些量 2
U UG H qu J n H n qn J s E K rE p r J pH o o p E y P vH q P E KE w J n q P vH q P E K n zKJ y H vo u JE w q P vJn J p P J nn q H q JJ n q P vH q P E KP nI P t J K D J
实例说明 )+,比 +,的估计精度要高 * 关键词 推广卡尔曼滤波 ) 卡尔曼滤波
. / / 0 1 2 3 4 1 5 6 7 5 8 ! 6 7 2 9 6 4 9 :;3 0 <3 6= 1 0 4 9 > 1 6 ?5 64 @ 9A 4 3 4 9B 7 4 1 <3 4 1 5 6
C D E FG H E I J K I LH E MF K N D HO P K I I F E
论
88 &7 7 7 " " $: ..:
本文首先对推广卡尔曼滤波的缺陷进行了分析+ 在此基础上介绍了 X 变换 + 然后给出了 X 卡尔曼滤波 的计算公式 O 最后给出了一个时间序列的状态估计的 例子 O 仿真结果表明 + XVW比 U VW的精度要高 O
" "
/$
/$
9
1 2
( $* ( $. 7 ) !3 *.& 0 5 3 3 + " # " $% "+ " # " $% "3 &4
" "
/$
/$
9
图 1 状态协方差的估计
88 :&7 *.7 ..
" "
$%
" "
/
&* $/ * . . " ;: = "> " "
< $ $ 9
" "
/$
_ 结
)* # ’ ,- ( # $ ’ y y ( ( +$ 其中 d *是状态模型 d - ( 是 .维系统噪声 * 观测值和系统状态矢量的关系有下面方程给出 %
)D # ’ ,0( # 1 ’ / y ( ( 其 中d 观测矢量d D是 观 测 模 型 d / 0( 是 量 测 噪 声 * 假 (
设噪声 - ( 和 0( 都是不相关的零均值的高斯白噪 声 d G 且 对任意 ( 和另 d 2 )% d 2 )5 d 7% ! 0(3 0(043 6 5 ( ( 4 (
# U de de j j j k i dl ’ QR S T V WX Y W V Z [\ T ] V Y^ R _ ‘ Y V a V b c R ] f ] Y bX Y g T ] T h T RV W^ R _ ‘ Y V a V b c R ] f ] Y bi ‘ ] Y \
x U . m 7 4 > 3 2 4 G D P no H o J pP K q p E r F s J rHK E t J u vJ q D E rw E pw P u q p P K IJ n q P vH q P E K F K n s J K q J r+H u vH Kw P u q J p P K I )+,I P t J n U n P vF u H q P E Kn D E {nq D H q )+,u J H r nq EvE p JH s s F p H q Jp J n F u q n ;9 |} 5 > : 7 y q J K r J r +H u vH Kw P u q J p P K I J n q P vH q P E K )K n s J K q J r +H u H vK w P u q J p P K I )K n s J K q J rG p H K n w E p vH q P E K