扩展卡尔曼滤波在目标跟踪中的应用研究_张爱民
合集下载
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
2 2
^
^
^
[
σＲ 0
2
0 σβ
2
]
， 在求解卡尔曼增益
0 0 s[ n － 1 ] + ux [ n ] uy [ n ]
（ 10 ）
矩阵的时候会用到。那么式 （ 3 ） 的观测方程可以写 成为：
^ Ｒ ［ n］ = h( s[ n] ) + w[ n] Z[ n] = ^ β［ n］
DOI:10.13274/j.cnki.hdzj.2013.10.015
文章编号： 1009 － 2552 （ 2013 ） 10 － 0095 － 03
中图分类号： TN911． 72
文献标识码： A
扩展卡尔曼滤波在目标跟踪中的应用研究
张爱民
（ 总参谋部通信训练基地 ，河北 宣化 075100 ）
摘
要： 扩展卡尔曼滤波在非平稳矢量信号和噪声环境下具有广泛的应用 ， 针对机动目标运动 模型的特点，采用基于扩展卡尔曼滤波的算法对运动目标进行跟踪处理 ， 该算法首先建立了运 动目标的状态模型和观测模型， 然后对观测数据进行滤波和误差估计处理， 最后通过计算机的 蒙特卡洛仿真得到了滤波轨迹和运动目标的距离和角度误差 ， 仿真结果表明， 扩展卡尔曼滤波
Abstract： Extended kalman filter has great importance in practicality under nonstationary vector signal and noise circumstance，aim to motion model of maneuvering target， a method of maneuvering target tracking based on extended Kalman filter algorithm has adopted． Firstly， the motion model of maneuvering target has been constructed， and then filter and error estimate is operated by use of observational datum． Finally， the experiment result has been presented by MonteCarlo simulation， including track of maneuvering target after filtering， distance error and direction error． The simulation result shows that extended Kalman filter algorithm has good performance of target track． Key words： extended Kalman filter； target track； MonteCarlo simulation
^
（ 5） （ 6）
－ 1 ］ HT ［ n ］ （ 7）
n］ M［ n | n － 1］ H T［ n］) － 1 ( C [ n ] + H［
扩展卡尔曼滤波初始化 ［6 － 7 ］ 。 由于实 在进行扩展滤波时， 需指定初值 ， 际中通常无法得到目标的初始状态 可以利用前几 个观测值建立状态的初始估计， 比如采用前两个观 测值， 初始滤波值为： zx （ 2） [ z （ 2） － z （ 1） ] / Δ ^ x x X（ 2 / 2 ） = zy （ 2） [ zy （ 2） － zy （ 1） ] / Δ
rx [ n ] r2 [ n ] + r2 [ n ] y 槡x － ry [ n ] 2 2 rx [ n ] + ry [ n ] 1． 3
ry [ n ] r [ n] + r 槡
2 x 2 x 2 y
[ n]
rx [ n ] r [ n ] + r2 y [ n]
^
收稿日期： 2013 － 03 － 25 作者简介： 张爱民（ 1977 － ） ， 男， 硕士， 讲师， 研究方向为军用无线 通信与网络， 通信抗干扰技术。
— 95 —
分量为 ： v x［ n］= v x［ n － 1］+ u x［ n］ v y［ n］= v y［ n － 1］+ u y［ n］ （ 1） u x［ n］ ， u y［ n］ 其中， 为 x 和 y 方向的噪声扰动。得到 了速度方程， 根据直线运动方程可以得到 n 时刻飞 行器的位置为： r x［ n］= r x［ n － 1］+ v x［ n － 1］ Δ r y［ n］= r y［ n － 1］+ v y［ n － 1］ Δ （ 2） 其中， Δ 为样本之间的时间间隔。 此时便可以得到 飞行器的距离和角度在噪声存在条件的观测模型 ： Ｒ[ n] =
h( s[ n] ) = 其中， w[ n] = 1． 2 rx [ n ] + ry [ n ] 槡
2 2
（ 4） ，
2 n］ ， u y［ n］ 这里对 u x［ 赋予相同的方差， 都用 σ u 表示， 并且假定它们是相互独立的， 于是可以得到如 下观测模型参数矩阵： 0 0 0 0 1 0 Δ 0 0 0 0 0 0 1 0 Δ A= Q= ， 2 0 0 σu 0 0 0 1 0 2 0 0 0 1 0 0 0 σu
PX （ 2 / 2 ） = E { X（ 2 / 2 ） X T （ 2 / 2 ） } = 珘 珘 X σx σ2 / Δ x 0 0
2
在极坐标下， 目标距观测者的真实距离和角度 、 测量距离和角度和经过滤波后距离和角度比较如图 2 所示， 经过扩展卡尔曼滤波器处理后的距离和角 度的误差均值和误差标准差分别如图 3 － 4 所示。 上述仿真结果都说明， 由于观测噪声和系统扰动噪 使得观测数据与真实数据之间存在着很 声的存在， 大的误差， 经过扩展卡尔曼滤波器对观测数据进行 滤波使滤波误差标准差远远小于观测噪声 处理后， 经过滤波达到了平滑数据目的， 同时体现了 标准差， 。 目标跟踪的良好性能
2 2 σ y = 1600 ， 飞行受到轻微扰动， 系统扰动方差为 σ u 为 0． 002 ， 在极坐标 下， 测量噪声的协方差分别为 2 2 σ Ｒ = 2000 ， σ β = 0 ． 05 ， 雷达通过测距 Ｒ 和测角 β 来 间接地确定目标位置， 并假设 Ｒ 和 β 独立地进行观
σx / Δ Δ σ + 2 σ2 x /Δ 4 0 0
2 2 u
2
0 0 σy
2 2
σy / Δ
0 2 σy / Δ 2 2 Δ σu + 2 σ2 x /Δ 4 0
2 扩展卡尔曼滤波目标跟踪算法仿真 分析
仿真情景假设： 目标沿某直线匀速运动， 用一雷 x 和 y 独立 雷达扫描周期 T = 1 秒， 达对目标观测， 信 号 矢 量 的 起 始 状 态 为： s [ － 1 ] = 地进 行 观 测， 2 [ 48002600 － 105 ] T ， 观 测 噪 声 的 标 准 差 均 为 σx =
0
引言
卡尔曼 （ Kalman ） 滤波器是对维纳滤波器的重 要推广， 适合于非平稳的矢量信号和噪声。 可将卡 尔曼滤波器看成噪声中序贯最小， 均方误差为估计 量， 其中信号利用动态或状态方程来描述。 克服了 维纳滤波必须用到无限过去的数据， 不适用于实时 处理的缺点， 卡尔曼滤波理论的基本思想是： 采用信 号与噪声的状态空间模型， 利用前一时刻的估计值 和现时刻的观测值来更新对状态变量的估计 ， 求出 现时刻的估计值。目标跟踪是其重要的应用领域之 一， 在目标跟踪中由于测量误差、 系统扰动噪声的影 响， 利用卡尔曼滤波建立的状态方程和观测方程经 这时经典的卡尔曼滤波不再实用 ， 这 常是非线性的， 时就要用到扩展的卡尔曼滤波来处理 。扩展卡尔曼
arctan
r [ n] r [ n]
2 y 2 x
在（ 7 ） 式中根据扩展卡尔曼滤波经典理论可以 求得： H[ n] = h s [ n ]
^
[ w [ n] ]
wＲ [ n ]
β
s [ n ]= s [ n | n － 1 ]
^
= 0 0ቤተ መጻሕፍቲ ባይዱ0 0
扩展卡尔曼滤波方程和观测参数矩阵 针对上述扩展卡尔曼滤波解决飞行器跟踪的问
利用一阶泰勒级数展开把非线性问题变为线性 题， ［4 － 5 ］ ： 问题得到如下公式 n | n － 1］= As［ n － 1 | n － 1］ 预测： s［ 最小预测 MSE 矩阵： M［ n | n － 1］= AM［ n － 1 | n － 1］ AT + Q 卡尔曼增益矩阵： K ［n ］ = M ［n | n
算法具有很好的目标跟踪性能。 关键词： 扩展卡尔曼滤波； 目标跟踪； 蒙特卡洛仿真
Ｒesearch on target tracking of extended Kalman filter and its application
ZHANG Aimin
（ Communication Training Base of PLA General Staff Headquarters，Xuanhua 075100 ， Hebei Province， China）
滤波实质是利用一阶泰勒级数展开化非线性为线 ［1 － 2 ］ 。 性， 然后再利用矢量的卡尔曼递推进行滤波
1
1． 1
扩展卡尔曼滤波目标跟踪算法分析
飞行器跟踪观测模型建立
这里用扩展卡尔曼滤波来跟踪飞行器的位置 ， 假定飞行器飞行的速度和方向是确定的 ， 同时飞行 器作匀速直线运动， 在飞行过程中难免会受到阻力 和轻微的速度修正等产生的扰动， 通常把这些扰动 建立直角坐标系 看成是一 种 噪 声 的 输 入 来 处 理， （ 假定飞行器在 xy 平面上运动 ） 来对飞行的状态进 行分析， 那么该飞行器 n 时刻在 x 和 y 方向的速度
（ 3）
n］ n］ 上式中Ｒ［ 表示 n 时刻的距离观测值， β［ 表示 n ， w ［ n ］ n 时刻的角度观测值 Ｒ 表示 时刻距离观测误 w β［n］表 示 n 时 刻 角 度 观 测 误 差， 差， 并且假设 w Ｒ［ n］ ， w β［ n］ 为均值为 0 高斯白噪声， 方差分别为 σＲ ， σβ ， 通常假定估计误差都是独立的， 同时也是不 变的。因此令C [ n ] =
rx [ n ] + ry [ n ] + wＲ [ n ] 槡
2 2
^
这里选择的信号矢量模型是由位置和速度分量 令： 构成的， rx [ n ] r [ n] y 由式（ 1 ） － （ 2 ） 可以推出： 令： s [ n ] = vx [ n ] vy [ n ] 1 0 s[ n] = 0 0 0 1 0 0 Δ 0 1 0 0 Δ 0 1
［3 ］
修正： s［ n | n］= s［ n | n － 1］+ K［ n］ n | n － 1］ ( Z [ n ] － h ( s［ )) 最小 MSE 矩阵： M［ n | n］= ( I － K [ n ] H [ n ] ) M［ n | n － 1］
^
^
^
（ 8） （ 9）
^
r2 y [ n] + wβ [ n ] β [ n ] = arctan 2 rx [ n ]
— 96 —
此时， 估计误差为： － vx （ 2 ） v （ 1） Δ ·u x （ 1 ） + x 2 珘 X （ 2 /2） = － vx （ 2 ） v （ 1） Δ ·u x （ 1 ） + x 2 而估计误差协方差矩阵为：
～ ～
－ vx （ 2 ） Δ － vx （ 2 ） Δ
^
^
^
[
σＲ 0
2
0 σβ
2
]
， 在求解卡尔曼增益
0 0 s[ n － 1 ] + ux [ n ] uy [ n ]
（ 10 ）
矩阵的时候会用到。那么式 （ 3 ） 的观测方程可以写 成为：
^ Ｒ ［ n］ = h( s[ n] ) + w[ n] Z[ n] = ^ β［ n］
DOI:10.13274/j.cnki.hdzj.2013.10.015
文章编号： 1009 － 2552 （ 2013 ） 10 － 0095 － 03
中图分类号： TN911． 72
文献标识码： A
扩展卡尔曼滤波在目标跟踪中的应用研究
张爱民
（ 总参谋部通信训练基地 ，河北 宣化 075100 ）
摘
要： 扩展卡尔曼滤波在非平稳矢量信号和噪声环境下具有广泛的应用 ， 针对机动目标运动 模型的特点，采用基于扩展卡尔曼滤波的算法对运动目标进行跟踪处理 ， 该算法首先建立了运 动目标的状态模型和观测模型， 然后对观测数据进行滤波和误差估计处理， 最后通过计算机的 蒙特卡洛仿真得到了滤波轨迹和运动目标的距离和角度误差 ， 仿真结果表明， 扩展卡尔曼滤波
Abstract： Extended kalman filter has great importance in practicality under nonstationary vector signal and noise circumstance，aim to motion model of maneuvering target， a method of maneuvering target tracking based on extended Kalman filter algorithm has adopted． Firstly， the motion model of maneuvering target has been constructed， and then filter and error estimate is operated by use of observational datum． Finally， the experiment result has been presented by MonteCarlo simulation， including track of maneuvering target after filtering， distance error and direction error． The simulation result shows that extended Kalman filter algorithm has good performance of target track． Key words： extended Kalman filter； target track； MonteCarlo simulation
^
（ 5） （ 6）
－ 1 ］ HT ［ n ］ （ 7）
n］ M［ n | n － 1］ H T［ n］) － 1 ( C [ n ] + H［
扩展卡尔曼滤波初始化 ［6 － 7 ］ 。 由于实 在进行扩展滤波时， 需指定初值 ， 际中通常无法得到目标的初始状态 可以利用前几 个观测值建立状态的初始估计， 比如采用前两个观 测值， 初始滤波值为： zx （ 2） [ z （ 2） － z （ 1） ] / Δ ^ x x X（ 2 / 2 ） = zy （ 2） [ zy （ 2） － zy （ 1） ] / Δ
rx [ n ] r2 [ n ] + r2 [ n ] y 槡x － ry [ n ] 2 2 rx [ n ] + ry [ n ] 1． 3
ry [ n ] r [ n] + r 槡
2 x 2 x 2 y
[ n]
rx [ n ] r [ n ] + r2 y [ n]
^
收稿日期： 2013 － 03 － 25 作者简介： 张爱民（ 1977 － ） ， 男， 硕士， 讲师， 研究方向为军用无线 通信与网络， 通信抗干扰技术。
— 95 —
分量为 ： v x［ n］= v x［ n － 1］+ u x［ n］ v y［ n］= v y［ n － 1］+ u y［ n］ （ 1） u x［ n］ ， u y［ n］ 其中， 为 x 和 y 方向的噪声扰动。得到 了速度方程， 根据直线运动方程可以得到 n 时刻飞 行器的位置为： r x［ n］= r x［ n － 1］+ v x［ n － 1］ Δ r y［ n］= r y［ n － 1］+ v y［ n － 1］ Δ （ 2） 其中， Δ 为样本之间的时间间隔。 此时便可以得到 飞行器的距离和角度在噪声存在条件的观测模型 ： Ｒ[ n] =
h( s[ n] ) = 其中， w[ n] = 1． 2 rx [ n ] + ry [ n ] 槡
2 2
（ 4） ，
2 n］ ， u y［ n］ 这里对 u x［ 赋予相同的方差， 都用 σ u 表示， 并且假定它们是相互独立的， 于是可以得到如 下观测模型参数矩阵： 0 0 0 0 1 0 Δ 0 0 0 0 0 0 1 0 Δ A= Q= ， 2 0 0 σu 0 0 0 1 0 2 0 0 0 1 0 0 0 σu
PX （ 2 / 2 ） = E { X（ 2 / 2 ） X T （ 2 / 2 ） } = 珘 珘 X σx σ2 / Δ x 0 0
2
在极坐标下， 目标距观测者的真实距离和角度 、 测量距离和角度和经过滤波后距离和角度比较如图 2 所示， 经过扩展卡尔曼滤波器处理后的距离和角 度的误差均值和误差标准差分别如图 3 － 4 所示。 上述仿真结果都说明， 由于观测噪声和系统扰动噪 使得观测数据与真实数据之间存在着很 声的存在， 大的误差， 经过扩展卡尔曼滤波器对观测数据进行 滤波使滤波误差标准差远远小于观测噪声 处理后， 经过滤波达到了平滑数据目的， 同时体现了 标准差， 。 目标跟踪的良好性能
2 2 σ y = 1600 ， 飞行受到轻微扰动， 系统扰动方差为 σ u 为 0． 002 ， 在极坐标 下， 测量噪声的协方差分别为 2 2 σ Ｒ = 2000 ， σ β = 0 ． 05 ， 雷达通过测距 Ｒ 和测角 β 来 间接地确定目标位置， 并假设 Ｒ 和 β 独立地进行观
σx / Δ Δ σ + 2 σ2 x /Δ 4 0 0
2 2 u
2
0 0 σy
2 2
σy / Δ
0 2 σy / Δ 2 2 Δ σu + 2 σ2 x /Δ 4 0
2 扩展卡尔曼滤波目标跟踪算法仿真 分析
仿真情景假设： 目标沿某直线匀速运动， 用一雷 x 和 y 独立 雷达扫描周期 T = 1 秒， 达对目标观测， 信 号 矢 量 的 起 始 状 态 为： s [ － 1 ] = 地进 行 观 测， 2 [ 48002600 － 105 ] T ， 观 测 噪 声 的 标 准 差 均 为 σx =
0
引言
卡尔曼 （ Kalman ） 滤波器是对维纳滤波器的重 要推广， 适合于非平稳的矢量信号和噪声。 可将卡 尔曼滤波器看成噪声中序贯最小， 均方误差为估计 量， 其中信号利用动态或状态方程来描述。 克服了 维纳滤波必须用到无限过去的数据， 不适用于实时 处理的缺点， 卡尔曼滤波理论的基本思想是： 采用信 号与噪声的状态空间模型， 利用前一时刻的估计值 和现时刻的观测值来更新对状态变量的估计 ， 求出 现时刻的估计值。目标跟踪是其重要的应用领域之 一， 在目标跟踪中由于测量误差、 系统扰动噪声的影 响， 利用卡尔曼滤波建立的状态方程和观测方程经 这时经典的卡尔曼滤波不再实用 ， 这 常是非线性的， 时就要用到扩展的卡尔曼滤波来处理 。扩展卡尔曼
arctan
r [ n] r [ n]
2 y 2 x
在（ 7 ） 式中根据扩展卡尔曼滤波经典理论可以 求得： H[ n] = h s [ n ]
^
[ w [ n] ]
wＲ [ n ]
β
s [ n ]= s [ n | n － 1 ]
^
= 0 0ቤተ መጻሕፍቲ ባይዱ0 0
扩展卡尔曼滤波方程和观测参数矩阵 针对上述扩展卡尔曼滤波解决飞行器跟踪的问
利用一阶泰勒级数展开把非线性问题变为线性 题， ［4 － 5 ］ ： 问题得到如下公式 n | n － 1］= As［ n － 1 | n － 1］ 预测： s［ 最小预测 MSE 矩阵： M［ n | n － 1］= AM［ n － 1 | n － 1］ AT + Q 卡尔曼增益矩阵： K ［n ］ = M ［n | n
算法具有很好的目标跟踪性能。 关键词： 扩展卡尔曼滤波； 目标跟踪； 蒙特卡洛仿真
Ｒesearch on target tracking of extended Kalman filter and its application
ZHANG Aimin
（ Communication Training Base of PLA General Staff Headquarters，Xuanhua 075100 ， Hebei Province， China）
滤波实质是利用一阶泰勒级数展开化非线性为线 ［1 － 2 ］ 。 性， 然后再利用矢量的卡尔曼递推进行滤波
1
1． 1
扩展卡尔曼滤波目标跟踪算法分析
飞行器跟踪观测模型建立
这里用扩展卡尔曼滤波来跟踪飞行器的位置 ， 假定飞行器飞行的速度和方向是确定的 ， 同时飞行 器作匀速直线运动， 在飞行过程中难免会受到阻力 和轻微的速度修正等产生的扰动， 通常把这些扰动 建立直角坐标系 看成是一 种 噪 声 的 输 入 来 处 理， （ 假定飞行器在 xy 平面上运动 ） 来对飞行的状态进 行分析， 那么该飞行器 n 时刻在 x 和 y 方向的速度
（ 3）
n］ n］ 上式中Ｒ［ 表示 n 时刻的距离观测值， β［ 表示 n ， w ［ n ］ n 时刻的角度观测值 Ｒ 表示 时刻距离观测误 w β［n］表 示 n 时 刻 角 度 观 测 误 差， 差， 并且假设 w Ｒ［ n］ ， w β［ n］ 为均值为 0 高斯白噪声， 方差分别为 σＲ ， σβ ， 通常假定估计误差都是独立的， 同时也是不 变的。因此令C [ n ] =
rx [ n ] + ry [ n ] + wＲ [ n ] 槡
2 2
^
这里选择的信号矢量模型是由位置和速度分量 令： 构成的， rx [ n ] r [ n] y 由式（ 1 ） － （ 2 ） 可以推出： 令： s [ n ] = vx [ n ] vy [ n ] 1 0 s[ n] = 0 0 0 1 0 0 Δ 0 1 0 0 Δ 0 1
［3 ］
修正： s［ n | n］= s［ n | n － 1］+ K［ n］ n | n － 1］ ( Z [ n ] － h ( s［ )) 最小 MSE 矩阵： M［ n | n］= ( I － K [ n ] H [ n ] ) M［ n | n － 1］
^
^
^
（ 8） （ 9）
^
r2 y [ n] + wβ [ n ] β [ n ] = arctan 2 rx [ n ]
— 96 —
此时， 估计误差为： － vx （ 2 ） v （ 1） Δ ·u x （ 1 ） + x 2 珘 X （ 2 /2） = － vx （ 2 ） v （ 1） Δ ·u x （ 1 ） + x 2 而估计误差协方差矩阵为：
～ ～
－ vx （ 2 ） Δ － vx （ 2 ） Δ