没有合适的资源?快使用搜索试试~ 我知道了~
基于扩展卡尔曼滤波的目标跟踪系统的设计
需积分: 0 2 下载量 126 浏览量
2024-05-06
18:51:38
上传
评论
收藏 1.25MB DOCX 举报
温馨提示
基于扩展卡尔曼滤波的目标跟踪系统的设计
资源推荐
资源详情
资源评论
基于扩展卡尔曼滤波的目标跟踪系统的设计
目录
1 扩展卡尔曼滤波的原理..................................................................................1
1.1 标准卡尔曼滤波 ...................................................................................1
1.1.1 卡尔曼滤波的基本公式 ..............................................................1
1.1.2 已知条件 ......................................................................................1
1.1.3 预测阶段 ......................................................................................1
1.1.4 更新阶段 ......................................................................................2
1.1.5 卡尔曼滤波的推导 ......................................................................3
1.2 扩展卡尔曼滤波 ...................................................................................4
1.2.1 符号定义 ......................................................................................4
1.2.2 非线性方程线性化 ......................................................................4
1.2.3 扩展卡尔曼滤波的公式 ..............................................................6
2 扩展卡尔曼滤波的算法..................................................................................7
2.1 扩展卡尔曼滤波计算流程 ...................................................................7
2.2 扩展卡尔曼滤波的函数 .......................................................................8
2.2.1 使用语法 ......................................................................................8
2.2.2 输入参数 ......................................................................................9
2.2.3 属性 ..............................................................................................9
2.2.4 输出参数 ....................................................................................14
3 目标跟踪系统的模型....................................................................................14
3.1 系统模型 .............................................................................................14
3.1.1 状态方程 ....................................................................................14
3.1.2 观测方程 ....................................................................................15
3.1.3 线性化观测方程 ........................................................................15
4 代码分析........................................................................................................17
5 仿真实验结果................................................................................................20
5.1 基本结果展示 .....................................................................................20
5.2 误差分析 .............................................................................................23
5.3 参数调整 .............................................................................................25
6 讨论与总结....................................................................................................28
6.1 技术亮点 .............................................................................................28
6.2 扩展卡尔曼滤波的缺点 .....................................................................29
6.3 问题及解决 .........................................................................................29
7 参考文献........................................................................................................35
8 附录................................................................................................................35
1
1 扩展卡尔曼滤波的原理
1.1 标准卡尔曼滤波
卡尔曼滤波是利用线性系统状态方程,通过系统输入输出观测数据,来对当
前状态进行最优估计的一种算法。由于它排除了系统中的噪声以及观测中存在的
干扰,通过“预测”与“更新”两个过程来对系统的状态进行最优估计,这种最
优估计又可以称之为滤波。这里说的系统状态,是总结系统所有过去的输入和扰
动对系统的作用的最小参数的集合。简而言之,卡尔曼滤波器是最优化的线性滤
波器。卡尔曼滤波器提供了一种有效的递归方法来估计过程的状态,在某种程度
上使平方误差的平均值最小化。该滤波器非常强大,可以对过去、现在甚至未来
的状态进行估计,即使模型系统的精确性质是未知的。
在实际应用中我们可以在任何含有不确定信息的动态系统中使用卡尔曼滤
波,对系统下一步的走向做出有根据的预测,即使伴随着各种干扰,卡尔曼滤波
总是能指出真实发生的情况。
在连续变化的系统中使用卡尔曼滤波是非常理想的,它具有占用内存小的优
点(除了前一个状态量外,不需要保留其它历史数据),并且速度很快,很适合
应用于实时问题和嵌入式系统。
1.1.1 卡尔曼滤波的基本公式
$
( )
$
) ( )1| ( |x x k kk k A k Bu+ = +
$
( )
$
( )
$
( )
1| 1 1| ( ( 1) 1| )k k k k Kx z k H kx x k+ + = + + + - +
( 1| 1) ( ) ( 1| )P k k I KH P k k+ + = - +
1
( 1| ) ( ( 1| ) )
T T
K P k k H HP k k H R
-
= + + +
( 1| ) ( | )
T
P k k AP k k A Q+ = +
1.1.2 已知条件
假设一个离散线性动态系统的模型如下所示:
① 状态估计方程
( ) ( ) ( ) ( )1x xA k B k w kk u++ = +
( )u k
为输入信号、
( )x k
为状态变量
A为状态转移矩阵
B为控制输入矩阵,作用在控制器向量
( )u k
的输入控制模型。
( )
w k
表示过程噪声,且为高斯白噪声,其中
( ) ( )cov w k Q k=
表示协方差矩
阵,即
( ) ~ (0, )p w N Q
。
② 观测方程
( ) ( ) ( ) ( )H xz k k kvk = +
( )z k
为观测变量,表示k时刻真实状态
( )x k
的一个测量
H为状态观测矩阵,它把真实状态空间映射成观测空间
( )
v k
表示观测噪声,也为高斯白噪声,其中
( ) ( )cov w k R k=
表示协方差矩
阵,
( ) ~ (0, )p v N R
初始状态以及每一时刻的噪声都认为是相互独立的
1.1.3 预测阶段
① 状态估计方程:
$
( )
$
) ( )1| ( |x x k kk k A k Bu+ = +
(黄金一条),表示预测下一步状态。
2
( )x k
表示状态真实值
$
( )
1|x k k+
表示状态预测值,在第k时刻,对
( )
1x k +
的估计(在第k时刻的时
候,对
1k +
时刻的任何值,都只能是估计(预测未来值),也称为先验状态
估计值)
$
( | )x k k
表示状态的最优估计值,也称为后验状态估计值。
状态估计误差
( ) ( )
$
( )
1| 1 1|k k x k k ke x+ -+ = +
$
,表示先验状态误差
( ) ( )
$
( )
| ke k x k x k= -
,表示后验状态误差
观测估计误差
( ) ( ) ( )
1 1 1|k z k z k ke + = + - +
$$
观测估计方程:
$
( )
1| ) 1|(k k H k kz x+ = +
$
② 预测估计协方差方程
( 1| ) ( | )
T
P k k AP k k A Q+ = +
,用于计算先验概率
后验估计误差协方差矩阵:
( ) (| ) ( ) ][
T
P kE e e kk k =
,为k时刻的后验估计误
差协方差矩阵,度量估计值的精确程度。表示真实值与最优估计值之间的
协方差
估计误差协方差矩阵
1( ) ( ) ( ) ]1| [ | 1|
T
P k k kk E e k e k+ = + +
$ $
,为k时刻到k+1时
刻的估计误差协方差矩阵,表示真实值与预测值之间的协方差。
Q
为过程噪声协方差矩阵,越大说明越不相信预测。实际工程中可自己调
参,也可以自适应(AKF)。
1.1.4 更新阶段
① 卡尔曼增益方程
1
( 1| ) ( ( 1| ) )
T T
K P k k H HP k k H R
-
= + + +
K
为最优卡尔曼增益
R
为测量噪声协方差矩阵,越大说明越不相信观测。实际工程中测量噪声由
厂商提供,也可以自己调参,也可以自适应(AKF)。
② 更新状态估计方程
$
( )
$
( ) ( )
$
( )
$
( )
1| 1 1| 1 1| ( ( 1) 1| )x x e xk k k k K k k k K z k H k kx+ + = + + + + += + + -
$
卡尔曼滤波最终目的,是得到一个基于误差能够不断修正的迭代式估计表达
式,可以由状态更新方程得到(黄金二条)
可以使用测量残差来简化表达公式
$
$
( )
( 1) 1|y z k H k kx= + - +
,观测误差协方
差矩阵:
)cov1) 1( (S ek k+ +=
$
,
( 1) ( 1| )
T
S k HP k k H R+ = + +
则卡尔曼增益可以简化为
1
( 1| ) ( 1)
T
K P k k H S k
-
= + +
更新状态估计方程可以简化为
$
( )
$
( )
$
1| 1 1|x x yk k k k K+ + = + +
③ 更新估计协方差方程
( 1| 1) ( ) ( 1| )P k k I KH P k k+ + = - +
,计算可以得到后验概率
$
( )
$
( ) ( )
$
( )
$
( )
1| 1 1| 1 1| ( ( 1) 1| )x x e xk k k k K k k k K z k H k kx+ + = + + + + += + + -
$
计算卡尔曼增益K(Kalman Gain) 之后便能最优修正当前状态值。
3
图1 标准卡尔曼滤波的工作流程
[2]
1.1.5 卡尔曼滤波的推导
这里需要得到一些关键的递推形式
① 通过状态估计更新方程得到从
( | )P k k
到
( 1| )P k k+
的递推形式(黄金五条)
$
$
$
$
( 1| ) cov[ ( 1) ( 1| )]
cov[ ( ) ( ) ( ) ( | ) ( )]
cov[ ( ) ( | ) ( )]
cov ( ) ( | ) cov ( )
( | )
T
T
P k k x k x k k
Ax k Bu k w k Ax k k Bu k
Ax k x k k w k
A x k x k k A w k
AP k k A Q
+ = + - +
= + + - -
= - +
= - +
= +
② 从状态估计更新方程推
( 1| 1)P k k+ +
$
$
$
$
( 1| 1) cov ( 1) ( 1| 1)
cov ( 1) [ ( 1| ) ( 1)]
cov ( 1) ( 1| ) [ ( 1) ( 1| )]
cov[( )( ( 1) ( 1| ) ( )]
( ) ( 1| )( ) ( 1)
T T
P k k x k x k k
x k x k k Ke k
x k x k k K z k z k k
I KH x k x k k Kw k
I KH P k k I KH KR k K
+ + = + - + +
= + - + + +
= + - + + + - +
= - + - + -
= - + - + +
$
$
③ 推导最优卡尔曼增益
卡 尔 曼 滤 波 器 是 最 小 均 方 误 差 估 计 器 , 后 验 状 态 误 差 估 计 是 指
( ) (| ) ( ) ][
T
P kE e e kk k =
,当矩阵
|( )P k k
的均方误差为最小时,即可求出最优卡尔
曼增益,在这里可以使用P的迹
对上式展开可以得到:
( 1| 1) ( 1| ) ( 1| ) ( 1| ) ( ( 1| )
T T T T
P k k P k k KHP k k P k k H K K HP k k H RK+ + = + - + - + + + +
然后使用P的迹对K求导,令其为0得到最优的W:
1
( ( 1| 1))
2( ( 1| )) 2 0
T
k
trace P k k
HP k k KS
K
+
¶ + +
= - + + =
可以得到:
1
( 1| ) ( ( 1| ) )
T T
K P k k H HP k k H R
-
= + + +
(黄金四条)
④ 利用最优卡尔曼增益化简
( 1| 1)P k k+ +
再将K带回
( 1| 1)P k k+ +
的式子中:
4
可以得到估计误差矩阵为
( 1| 1) ( ) ( 1| )P k k I KH P k k+ + = - +
(黄金三条)
在这里得到了K的最优表达式,只能在最优卡尔曼增益时才成立。如果算术
精度总是很低而导致数值稳定性出现问题,或者特意使用非最优卡尔曼增益,则
必须使用上面未简化后的后验误差协方差公式:
( 1| 1) ( 1| ) ( 1| ) ( 1| ) ( ( 1| )
T T T T
P k k P k k KHP k k P k k H K K HP k k H RK+ + = + - + - + + + +
在这里得到了一个完整的递推链:
( | ) ( 1| ) ( 1| 1)P k k P k k P k k® + ® + +
通过这个递推链,只要给定一个初始估
计
(0 | 0)P
,就可按照递推连来进行状态估计
(0 | 0) (1| 0) (1|1)P P P® ® ® L
1.2 扩展卡尔曼滤波
卡尔曼滤波的应用前提是系统需为线性系统,且系统的噪声(过程噪声、测
量噪声)均为高斯白噪声。但实际上,几乎所有的系统是非线性的,即系统状态
变量的差分方程是非线性的。非线性系统的分析复杂,很多情况下依然会选择线
性化处理,使其等效为一个线性系统。经过线性化处理后,卡尔曼滤波的适用条
件便可满足,这也就是扩展卡尔曼滤波(Extern Kalman Filter, EKF)。
无论是线性还是非线性,卡尔曼滤波里面存在的所有误差都是符合正态分布
的。扩展卡尔曼滤波(Extended Kalman Filter,EKF)是标准卡尔曼滤波在非线
性情形下的一种扩展形式,EKF算法是将非线性函数进行泰勒展开,省略高阶项,
保留展开项的一阶项,以此来实现非线性函数线性化,最后通过卡尔曼滤波算法
近似计算系统的状态估计值和方差估计值,对信号进行滤波。
1.2.1 符号定义
为了简化公式长度,在这里续上文定义:
( )
k
x k or x
和
( ) z
k
z k or
为真实的状态和测量输出值
$ $
( | )
k
x k k or x
和
( | )
k
z k k or z
$ $
是估计状态和测量输出值,为
k
时间点的后验状
态值。
%
k
x
和
%
k
y
是计算得到的近似状态和测量输出值
$
( )
$
o| r 1
k
x k k x
-
-
为 k 时 间 点 的 先 验 状 态 值 , 先 验 估 计 公 式 :
$ $
1
( , , 0)
k k
k
x f x u
-
-
=
,
k
u
其中是已知量,
$
1k
x
-
是上一时刻的后验估计。
1( | )P k k+
or
µ
k
P
-
,表示真实值与预测值之间的协方差。
( )P k
or
k
P
,表示真实值与最优估计值之间的协方差
1.2.2 非线性方程线性化
对于非线性系统,一个正态分布的随机变量通过非线性系统之后就不再是正
态的,EKF只是一种特殊的状态估计值,通过线性化逼近贝叶斯规则的最优性。
在这里我们假设非线性差分方程为:
( ) ( ( 1), ( 1), ( 1))
( ) ( ( ), ( ))
x k f x k u k w k
z k h x k v k
= - - -
ì
í
=
î
当不知道噪声的具体大小的时候,可以通过以下式子得到近似状态值以及测
量值:
%
$
%
( | ) ( ( 1| 1), ( 1),0)
( ) ( ( | ),0)
x k k f x k k u k
z k h x k k
ì
= - - -
ï
í
=
ï
î
%
其中,
$
( | )x k k
是后验状态估计,下面对其进行推导求解
剩余41页未读,继续阅读
资源评论
偕笑满
- 粉丝: 108
- 资源: 1
上传资源 快速赚钱
- 我的内容管理 展开
- 我的资源 快来上传第一个资源
- 我的收益 登录查看自己的收益
- 我的积分 登录查看自己的积分
- 我的C币 登录后查看C币余额
- 我的收藏
- 我的下载
- 下载帮助
最新资源
资源上传下载、课程学习等过程中有任何疑问或建议,欢迎提出宝贵意见哦~我们会及时处理!
点击此处反馈
安全验证
文档复制为VIP权益,开通VIP直接复制
信息提交成功