三维纯角度被动跟踪定位的最小二乘_卡尔曼滤波算法
合集下载
相关主题
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
Abstract : Bearing2only location is a nonlinear state estimation in essence. Classical extended Kalman fil2 ter is erratic. In this paper , expressions for the least squares estimation of target location and its variances have been derived by the static estimation theory. Then Kalman filter is used in x , y , z directions respectively to improve the location accuracy. Simulation shows that it is simple and efficient .
i =1
i =1
i =1
N
N
∑ ∑ F =
( l2i + n2i ) yi - mi li xi - mi nizi G =
( m2i + l2i ) zi - ni li xi - mi ni yi
i =1
i =1
由公式 (2) 可求得 li 、mi 、ni , ( xi , yi , zi ) 是已知观测站 i 的坐标 ,因此 ,由上式可求得目标位置的估计值 。同理 ,
图 1 目标的最小二乘估计 Fig. 1 Least square estimation of target location
xli
xi
=
y - yi mi
=
z - zi ni
(1)
式中 ( xi , yi , zi ) ———观测站 i 的坐标 ; li 、mi 和 ni ———定位线 Li 的方向余弦 ,它们分别为 :
Keywords : Bearing2only measurement ; Least squares ; Kalman filter ; Passive target tracking and location
1 引 来自百度文库言
被动式探测器 (如红外 、声纳等) 只能得到角度信息 ,利用角度信息估计目标的距离和速度实质上是一个非 线性状态估计问题 。经典的扩展卡尔曼滤波 ( EKF) 算法性能很不稳定 ,在非线性 、噪声较大时估计误差大 ,甚 至发散 。在多个观测站的情况下 ,希望尽可能利用所得到的方向角数据 ,文中给出的算法 ,首先采用最小二乘 法对目标位置进行粗估计 ,然后进行线性的卡尔曼滤波 ,从而实现了对目标较高精度的定位和跟踪 。
就有 N 条定位线 。在没有观测误差的情况下 ,这 N 条定位线应交于
一点 ,这个交点就是目标的位置 ,但是在有观测误差的情况下 ,这 N
条定位线并不一定交于一点 。应用最小二乘法原理 ,可以认为与 N
条定位线距离和最短的点就是目标的估计位置 ,如 1 图所示 。Li 表 示由观测站 i 得到的定位线 , T ( xT , yT , zT) 是目标的位置 , Ai ( xi0 , yi0 , zi0 ) 表示目标到定位线 Li 的垂足 ,则定位线 Li 的公式为 :
(6)
·84 ·
© 1994-2007 China Academic Journal Electronic Publishing House. All rights reserved. http://www.cnki.net
邱 玲 :三维纯角度被动跟踪定位的最小二乘2卡尔曼滤波算法
式中 D = LMN + 2 TRS - S2 M - R2 L - T2 N
i =1
N
∑ -
mi ni
i =1
N
∑ yT =
[ ( l2i + n2i ) yi - mi li xi - mi nizi \ 〗 (5)
i =1
N
∑ -
li ni
i =1
N
∑ -
mi ni
i =1
N
∑( m2i + l2i )
i =1
zT
N
∑[ ( m2i + l2i ) zi - ni li xi - mi ni yi \ 〗
可求得对目标位置估计的方差 :
σ xT
=
MN D
R2 ,σyT
=
NL D
S2 ,σzT
=
LM D
T2
(7)
3 卡尔曼滤波
前面利用某一时刻各观测站的测量数据 ,根据静态估计理论 ,得到了目标位置的一个最小二乘估计 ,下面 采用卡尔曼滤波作进一步的数据处理 ,提高定位精度 。为了避免测量误差的相关性 ,在 x , y , z 方向上分别进 行卡尔曼滤波 ,这样可以大大简化算法 ,提高系统的定位精度 。下面仅给出 x 方向上的滤波算法 。
文章编号 :100722276 (2001) 0220083204
三维纯角度被动跟踪定位的最小二乘2卡尔曼滤波算法
邱 玲 ,沈振康
(长沙国防科技大学 ATR 重点实验室 ,湖南 长沙 410073) 摘要 :利用角度信息估计出目标的距离和速度实质上是一个非线性状态估计问题 ,经典的扩展 卡尔曼滤波算法性能很不稳定 。文中首先根据静态估计理论推导出了在某一时刻目标位置的最小 二乘解 ,然后将其作为卡尔曼滤波的测量值进行滤波 ,作进一步的数据处理 ,以提高估计精度 。为 了避免测量误差的相关性 ,分别在 x , y , z 方向上进行滤波 ,简化了算法 ,提高系统的定位精度 。仿 真结果表明这一算法是简单而有效的 。 关 键 词 : 纯角度测量 ; 最小二乘 ; 卡尔曼滤波 ; 目标被动跟踪定位 中图分类号 :TP39 文献标识码 :A
收稿日期 :2000207228 ; 修订日期 :2000210230 作者简介 :邱玲 (19732) ,女 ,浙江金华人 ,博士 ,从事通信与信息工程方面的研究工作 。
·83 · © 1994-2007 China Academic Journal Electronic Publishing House. All rights reserved. http://www.cnki.net
ui = ( xT - xi ) l2i + ( yT - yi ) mi li + ( zT - zi ) li ni + xi - xT
vi = ( xT - xi ) mi li + ( yT - yi ) m2i + ( zT - zi ) mi ni + yi - yT
wi = ( xT - xi ) li ni + ( yT - yi ) mi ni + ( zT - zi ) n2i + zi - zT
第 30 卷第 2 期 红外与激光工程 2001 年 4 月 Vol . 30No. 2 Infrared and Laser Engineering Apr. 2001
邱 玲 :三维纯角度被动跟踪定位的最小二乘2卡尔曼滤波算法
2 最小二乘估计
假设每个观测站 i 的位置分别为 ( xi , yi , zi ) , i = 1 ,2 …, N ,目标的位置为 ( xT , yT , zT) ,则由每个观测站 i 测 得的方位角 αi 、俯仰角 βi 可以确定一条空间的定位线 , N 个观测站
定义矢量 Xk 为 :
Xk = xk xk T
( 8)
式中 xk ———时刻 k 目标在 x 方向上的位置 ; xk ———时刻 k 目标在 x 方向上的速度 。
则在假设目标作匀速直线运动的情况下 ,目标的动力学公式为 :
< Γ Xk + 1 = X k + 1 , k k + W k + 1 , k k
·85 ·
© 1994-2007 China Academic Journal Electronic Publishing House. All rights reserved. http://www.cnki.net
邱 玲 :三维纯角度被动跟踪定位的最小二乘2卡尔曼滤波算法
所以可以写出 x 方向上的卡尔曼滤波公式如下 :
4 实验仿真
不失一般性 ,我们考虑两个观测站的情况 。观测站 1 的坐标为 ( 0 , 0 , 0) , 观测站 2 的坐标为 ( 0 , 8km , 0) , 目标的初始位置为 ( 50km , 50km , 8km) ,目标的速度 vx = - 340mΠs , vy = - 340mΠs , vz = 0 。测角精度为 0. 1s 。 实验结果表明 ,最小二乘估计已使定位具有一定的精度 , 卡尔曼滤波进一步提高了定位精度 , 距离估计误差在 0. 5m 之内 ,且收敛很快 。图 2 为 x 方向距离估计误差曲线 , 图 3 是 x 方向距离估计方差收敛曲线 , 图 4 为 y 方向目标速度估计误差曲线 ,图 5 为 y 方向目标速度估计方差收敛曲线 。
状态公式预测 : Xk+ 1Πk = < X k+ 1Πk kΠk
方差预测 < < Γ Γ : P = P + q k+ 1Πk
T k + 1 , k kΠk k + 1 , k
T k+1, k k k+1, k
状态估计 : Xk + 1Πk + 1 = Xk + 1Πk + Kk + 1 zk + 1 - H X k + 1 k + 1Πk
(4)
则分别令 9 d
xT
=
0
、9 d
yT
=
0
、9 d
zT
=
0
,可得如下一组公式
:
N
∑( m2i + n2i )
i =1
N
∑ -
mi li
i =1
N
xT
∑ -
li ni
i =1
N
∑[ ( m2i + n2i ) xi - mi li yi - li nizi ]
i =1
N
∑ -
mi li
i =1
N
∑( m2i + l2i )
LS2Kalman algorithm for passive target location and tracking with bearing2only measurements
QIU Ling , SHEN Zhen2kang
(ATR Lab. , University of National Defense Technology , Changsha 410073 , China)
li = sinβi cosαi , mi = sinβi sinαi , ni = cosβi
(2)
由几何关系并经过一定的数学变换 ,可得目标相对于 N 条定位线 Li 的距离的平方和为 :
N
N
∑ ∑ d =
| Ai T | 2 =
( u2i + v2i + w2i )
(3)
i =1
i =1
式中
( 9)
式中 <k + 1 , k = 1
t ;
01
Γ = k + 1 , k
t2Π2 t
式中 t > tk - 1 - tk ———样本的时间间隔 ;
Wk = ¨xk ,是 x 方向上的加速度分量干扰 ,其统计特性为 :
E[ Wk ] = 0 E[ W2k ] = q
测量公式 : zk = xk + vk , xk 由公式 ( 6) 计算而得 。
N
N
N
N
∑ ∑ ∑ ∑ L =
( m2i + n2i ) M =
( l2i + n2i ) N =
( l2i + m2i ) R = -
mi ni
i =1
i =1
i =1
i =1
N
N
N
∑ ∑ ∑ S = -
li ni T = -
mi li E =
( m2i + n2i ) xi - mi li yi - li ni zi
方差估计 : Pk+ 1Πk+ 1 = I - Kk+ 1 Hk+ 1 Pk+ 1Πk
滤波增益 : Kk+ 1
=
P HT k + 1Πk k + 1
H P HT k + 1 k + 1Πk k + 1
+
Rk+ 1
-1
( 11) ( 12) ( 13) ( 14) ( 15)
对于 y 和 z 方向上的滤波估计 ,原理也如上分析 ,这里不再赘述 。
或写成 :
zk = Hk Xk + vk
( 10)
式中 Hk = [ 1 0 ] ; vk ———x 方向上时刻 k 的位置测量误差 , vk 的方差由公式 ( 7) 计算而得 。
通过上述分析 ,得到了目标在 x 方向上的动力学公式 ( 9) 、测量公式 ( 10) 及位置误差 vk 的方差公式 ( 7) ,
i =1
公式 (5) 的解就是目标位置的最小二乘估计值 ,最后的解如下 :
^x T = ( EMN + FRS + TRG - GMS - TFN - R2 E) ΠD
^y T = ( L FN + TGS + ERS - S2 F - GRL - TEN) ΠD
^z T = ( LMG + TR E + TFS - SM E - R FL - T2 G) ΠD
i =1
i =1
i =1
N
N
∑ ∑ F =
( l2i + n2i ) yi - mi li xi - mi nizi G =
( m2i + l2i ) zi - ni li xi - mi ni yi
i =1
i =1
由公式 (2) 可求得 li 、mi 、ni , ( xi , yi , zi ) 是已知观测站 i 的坐标 ,因此 ,由上式可求得目标位置的估计值 。同理 ,
图 1 目标的最小二乘估计 Fig. 1 Least square estimation of target location
xli
xi
=
y - yi mi
=
z - zi ni
(1)
式中 ( xi , yi , zi ) ———观测站 i 的坐标 ; li 、mi 和 ni ———定位线 Li 的方向余弦 ,它们分别为 :
Keywords : Bearing2only measurement ; Least squares ; Kalman filter ; Passive target tracking and location
1 引 来自百度文库言
被动式探测器 (如红外 、声纳等) 只能得到角度信息 ,利用角度信息估计目标的距离和速度实质上是一个非 线性状态估计问题 。经典的扩展卡尔曼滤波 ( EKF) 算法性能很不稳定 ,在非线性 、噪声较大时估计误差大 ,甚 至发散 。在多个观测站的情况下 ,希望尽可能利用所得到的方向角数据 ,文中给出的算法 ,首先采用最小二乘 法对目标位置进行粗估计 ,然后进行线性的卡尔曼滤波 ,从而实现了对目标较高精度的定位和跟踪 。
就有 N 条定位线 。在没有观测误差的情况下 ,这 N 条定位线应交于
一点 ,这个交点就是目标的位置 ,但是在有观测误差的情况下 ,这 N
条定位线并不一定交于一点 。应用最小二乘法原理 ,可以认为与 N
条定位线距离和最短的点就是目标的估计位置 ,如 1 图所示 。Li 表 示由观测站 i 得到的定位线 , T ( xT , yT , zT) 是目标的位置 , Ai ( xi0 , yi0 , zi0 ) 表示目标到定位线 Li 的垂足 ,则定位线 Li 的公式为 :
(6)
·84 ·
© 1994-2007 China Academic Journal Electronic Publishing House. All rights reserved. http://www.cnki.net
邱 玲 :三维纯角度被动跟踪定位的最小二乘2卡尔曼滤波算法
式中 D = LMN + 2 TRS - S2 M - R2 L - T2 N
i =1
N
∑ -
mi ni
i =1
N
∑ yT =
[ ( l2i + n2i ) yi - mi li xi - mi nizi \ 〗 (5)
i =1
N
∑ -
li ni
i =1
N
∑ -
mi ni
i =1
N
∑( m2i + l2i )
i =1
zT
N
∑[ ( m2i + l2i ) zi - ni li xi - mi ni yi \ 〗
可求得对目标位置估计的方差 :
σ xT
=
MN D
R2 ,σyT
=
NL D
S2 ,σzT
=
LM D
T2
(7)
3 卡尔曼滤波
前面利用某一时刻各观测站的测量数据 ,根据静态估计理论 ,得到了目标位置的一个最小二乘估计 ,下面 采用卡尔曼滤波作进一步的数据处理 ,提高定位精度 。为了避免测量误差的相关性 ,在 x , y , z 方向上分别进 行卡尔曼滤波 ,这样可以大大简化算法 ,提高系统的定位精度 。下面仅给出 x 方向上的滤波算法 。
文章编号 :100722276 (2001) 0220083204
三维纯角度被动跟踪定位的最小二乘2卡尔曼滤波算法
邱 玲 ,沈振康
(长沙国防科技大学 ATR 重点实验室 ,湖南 长沙 410073) 摘要 :利用角度信息估计出目标的距离和速度实质上是一个非线性状态估计问题 ,经典的扩展 卡尔曼滤波算法性能很不稳定 。文中首先根据静态估计理论推导出了在某一时刻目标位置的最小 二乘解 ,然后将其作为卡尔曼滤波的测量值进行滤波 ,作进一步的数据处理 ,以提高估计精度 。为 了避免测量误差的相关性 ,分别在 x , y , z 方向上进行滤波 ,简化了算法 ,提高系统的定位精度 。仿 真结果表明这一算法是简单而有效的 。 关 键 词 : 纯角度测量 ; 最小二乘 ; 卡尔曼滤波 ; 目标被动跟踪定位 中图分类号 :TP39 文献标识码 :A
收稿日期 :2000207228 ; 修订日期 :2000210230 作者简介 :邱玲 (19732) ,女 ,浙江金华人 ,博士 ,从事通信与信息工程方面的研究工作 。
·83 · © 1994-2007 China Academic Journal Electronic Publishing House. All rights reserved. http://www.cnki.net
ui = ( xT - xi ) l2i + ( yT - yi ) mi li + ( zT - zi ) li ni + xi - xT
vi = ( xT - xi ) mi li + ( yT - yi ) m2i + ( zT - zi ) mi ni + yi - yT
wi = ( xT - xi ) li ni + ( yT - yi ) mi ni + ( zT - zi ) n2i + zi - zT
第 30 卷第 2 期 红外与激光工程 2001 年 4 月 Vol . 30No. 2 Infrared and Laser Engineering Apr. 2001
邱 玲 :三维纯角度被动跟踪定位的最小二乘2卡尔曼滤波算法
2 最小二乘估计
假设每个观测站 i 的位置分别为 ( xi , yi , zi ) , i = 1 ,2 …, N ,目标的位置为 ( xT , yT , zT) ,则由每个观测站 i 测 得的方位角 αi 、俯仰角 βi 可以确定一条空间的定位线 , N 个观测站
定义矢量 Xk 为 :
Xk = xk xk T
( 8)
式中 xk ———时刻 k 目标在 x 方向上的位置 ; xk ———时刻 k 目标在 x 方向上的速度 。
则在假设目标作匀速直线运动的情况下 ,目标的动力学公式为 :
< Γ Xk + 1 = X k + 1 , k k + W k + 1 , k k
·85 ·
© 1994-2007 China Academic Journal Electronic Publishing House. All rights reserved. http://www.cnki.net
邱 玲 :三维纯角度被动跟踪定位的最小二乘2卡尔曼滤波算法
所以可以写出 x 方向上的卡尔曼滤波公式如下 :
4 实验仿真
不失一般性 ,我们考虑两个观测站的情况 。观测站 1 的坐标为 ( 0 , 0 , 0) , 观测站 2 的坐标为 ( 0 , 8km , 0) , 目标的初始位置为 ( 50km , 50km , 8km) ,目标的速度 vx = - 340mΠs , vy = - 340mΠs , vz = 0 。测角精度为 0. 1s 。 实验结果表明 ,最小二乘估计已使定位具有一定的精度 , 卡尔曼滤波进一步提高了定位精度 , 距离估计误差在 0. 5m 之内 ,且收敛很快 。图 2 为 x 方向距离估计误差曲线 , 图 3 是 x 方向距离估计方差收敛曲线 , 图 4 为 y 方向目标速度估计误差曲线 ,图 5 为 y 方向目标速度估计方差收敛曲线 。
状态公式预测 : Xk+ 1Πk = < X k+ 1Πk kΠk
方差预测 < < Γ Γ : P = P + q k+ 1Πk
T k + 1 , k kΠk k + 1 , k
T k+1, k k k+1, k
状态估计 : Xk + 1Πk + 1 = Xk + 1Πk + Kk + 1 zk + 1 - H X k + 1 k + 1Πk
(4)
则分别令 9 d
xT
=
0
、9 d
yT
=
0
、9 d
zT
=
0
,可得如下一组公式
:
N
∑( m2i + n2i )
i =1
N
∑ -
mi li
i =1
N
xT
∑ -
li ni
i =1
N
∑[ ( m2i + n2i ) xi - mi li yi - li nizi ]
i =1
N
∑ -
mi li
i =1
N
∑( m2i + l2i )
LS2Kalman algorithm for passive target location and tracking with bearing2only measurements
QIU Ling , SHEN Zhen2kang
(ATR Lab. , University of National Defense Technology , Changsha 410073 , China)
li = sinβi cosαi , mi = sinβi sinαi , ni = cosβi
(2)
由几何关系并经过一定的数学变换 ,可得目标相对于 N 条定位线 Li 的距离的平方和为 :
N
N
∑ ∑ d =
| Ai T | 2 =
( u2i + v2i + w2i )
(3)
i =1
i =1
式中
( 9)
式中 <k + 1 , k = 1
t ;
01
Γ = k + 1 , k
t2Π2 t
式中 t > tk - 1 - tk ———样本的时间间隔 ;
Wk = ¨xk ,是 x 方向上的加速度分量干扰 ,其统计特性为 :
E[ Wk ] = 0 E[ W2k ] = q
测量公式 : zk = xk + vk , xk 由公式 ( 6) 计算而得 。
N
N
N
N
∑ ∑ ∑ ∑ L =
( m2i + n2i ) M =
( l2i + n2i ) N =
( l2i + m2i ) R = -
mi ni
i =1
i =1
i =1
i =1
N
N
N
∑ ∑ ∑ S = -
li ni T = -
mi li E =
( m2i + n2i ) xi - mi li yi - li ni zi
方差估计 : Pk+ 1Πk+ 1 = I - Kk+ 1 Hk+ 1 Pk+ 1Πk
滤波增益 : Kk+ 1
=
P HT k + 1Πk k + 1
H P HT k + 1 k + 1Πk k + 1
+
Rk+ 1
-1
( 11) ( 12) ( 13) ( 14) ( 15)
对于 y 和 z 方向上的滤波估计 ,原理也如上分析 ,这里不再赘述 。
或写成 :
zk = Hk Xk + vk
( 10)
式中 Hk = [ 1 0 ] ; vk ———x 方向上时刻 k 的位置测量误差 , vk 的方差由公式 ( 7) 计算而得 。
通过上述分析 ,得到了目标在 x 方向上的动力学公式 ( 9) 、测量公式 ( 10) 及位置误差 vk 的方差公式 ( 7) ,
i =1
公式 (5) 的解就是目标位置的最小二乘估计值 ,最后的解如下 :
^x T = ( EMN + FRS + TRG - GMS - TFN - R2 E) ΠD
^y T = ( L FN + TGS + ERS - S2 F - GRL - TEN) ΠD
^z T = ( LMG + TR E + TFS - SM E - R FL - T2 G) ΠD