一种新型自适应卡尔曼滤波算法_高勤
合集下载
相关主题
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
2001 年 12 月
现代雷达
第6期
一种新型自适应卡尔曼滤波算法
高 勤 李志强 都学新 ( 军械工程学院光学与电子工程系 石家庄 050003)
X
【 摘要】 主要完成 卡尔曼滤 波在准连续 波雷达数据 处理 ( 距离和速 度 ) 中 算法应用研 究 , 实 现一种新型 自适应 卡尔曼滤波算法。通 过计算机仿真 , 该算法能够适应 不同的机动目标 , 并对测量数 据中的有色噪声有较 强的抑制作 用 , 满足实时要求 , 提高雷达测量精度。 【 关键词】 卡尔曼滤波 , 自适应滤波 , 机动目标
可以看出, 状态矢量中加速度为 0, 加速度估计 的均方误差 1 非常大, 对滤波后的数据处理的精度 E 影响非常大, 甚至卡尔曼滤波数值发散, 导致不能工 作 , 后面的数据仿真分析也将证明这一点。 第二种方法: 它通过两个观测数据来启动滤波过程, 与第一 1 种方法比较 , 不同的是: 状态矢量中加速度为 ( v 2 T - v 1 ) , 加速度估计的均方误差为 12 R2 v , 它比第一种 T 方法有明显的改善。 第三种方法: 它实际上是第二种方法的简化形式, 状态矢量 的计算方法不变 , 对误差协方差矩阵只取为对角阵 形式 , 与第二种方法相比 , 只是令 P ( 2, 1) = P ( 1, 2) = 0。因为按照第一种计算方法 , P 的形式可为对角 阵 , 对角上的值根据实际测量情况来获得, 但在滤波 启动一段时间后影响就不大了, 所以做此简化, 数据 仿真分析也证明该方法运算最为简单 , 精度较高。
5 仿真模型的确定
根据准连续波雷达需要处理的距离和速度, 一 般采用如下的数据进行仿真: ( 1) 匀速直线运动 x 0 = 1000m , y 0= 1000m , 第一种速度较低 v 0= 10m/ s( 称为 M ode 1) 第二种速度较高 v 0= 400m/ s( 称为 M ode 2) ( 2) 匀加速直线运动 x 0 = 1000m , y 0= 1000m , v 0= 10m / s 第一种加速度较小 a0= 1m/ s ( 称为 M ode 3) 第一种加速度较高 a0= 20m/ s2( 称为 Mode 4) ( 3) 加速度阶跃运动 ( 称为 Mode 5) 初始时 x 0 = 1000m, y 0 = 1000m, v 0 = 10m / s, a0 = 1m / s2, 开始后 100 秒, 加速度跳变为 a0= 30m / s2。 ( 4) 转圈运动 ( 称为 M ode 6) x 0 = 6000m , y 0 = 8000m , R = 1500m, 角速 度为 2P / 50rad / s, 法向加速度为 23. 7m / s2 。 我们应该清楚 , 在 M ode 4 和 Mode 5 中 , 当进 行比较长时间后 , 速度非常大 , 超出目标本身的最高 速度 , 我们进行数据仿真主要是检测滤波器的跟踪 性能 , 没有过分约束数据的真实条件。
T ~ E〔 Z~ Z 〕 =
0 R
2 v
0 0 R
2 v
0 0
( 15)
0
式中 : Rr , Rv 分别为距离和速度测量误差的方差, 代
2 2
入上面的公式可得 R2 r 0 0 P= 0 Rv 0
2
P - 1 ( 1) = E I + H T R - 1( 1) H
( 8)
R ( 1) 为观测噪声的方差矩阵 , 须由经验假定获 得。H 为观测矩阵 , 则 P ( 1) = ( E I + H T R - 1 ( 1) H ) - 1 X = (E I + H R ( 1) H )
A N ew A daptive K alman Filtering Algorithm
GAO Qin LI Zhi-qiang DU Xue-xin ( Ordnance Engineering College Shijiazhuang 050003)
【 Abstract 】 T he applications of the K alman filtering algor ithm in data processing ( range and speed) of quasicontinuouswav e radar ar e studied. A new adaptive K alm an algor ithm is r ealized. It is sho wn by comput er simulation that this algor ithm is applicable for all kinds of mo ving targ et s, able to suppr ess coloured measurement noise larg ely , meets the requir ement for real-time pr ocessing , and impro ves the accuracy o f radar measurement. 【 Key words 】 K alman filter ing, adapt ive filt er ing, moving targ et
距离和速度测量噪声不相关, 则 R ( 1) 为 2 × 2 的对 角阵 , 设定 R ( 1) = 则 E+ P - 1 ( 1) = 0 0 1 R2 r E+ 0 0 1 R2 v 0 0 E ( 19) R2 r 0 0 Rv
2
~ 2 = D 1 ( v 2 - v 1 ) = 1 ( Rv 2 - Rv 1) ( 12) X T T 式中: Dr 2, Dv 2, Dv 1 是两次目标坐标测量时距离与速 度的测量误差。 ~ = B~ 设X Z , 式中定 义 B 为测量误差的变换矩 阵, 则
T T
( 11)
速度 , 观测向量 Z = 〔 r v 〕, 观测矩阵 H = 1 0 0 1 0 0 ( 17)
T
~X ~T 〕 这时初始估值的协方差矩阵为 : P = E 〔 X , ~ 式中 X 是误差矢量。
T ~ = 〔 ~ 0 X ~ 1 X ~ 2〕 X X ~ 0 = Dr 2, X ~ 1 = Dv 2 X
1 0 0
T 1 0
(A T - 1+ e (1 - e e
- A T
- A T
)/A
2
- A T
/A
Z ( k ) = H ( k ) X ( k) + V ( k )
( 2)
H ( k) =
1 0 0 1
0 0
为标准的卡尔曼滤波方程。 时间传播方程 d( k / k - 1) = 5 ( k + 1, k ) X d( k - 1/ k - 1) X -( k ) + U ( k) A ( 3) P ( k / k - 1) = 5 ( k + 1, k ) P ( k - 1/ k - 1) 5 T ( k + 1, k ) + Q( k - 1) 根据均值自适应算法 , 状态值预测计算时采用 d( k / k - 1) = 5 ( k + 1, k ) X d( k - 1/ k - 1) ( 5) X 测量修正方程 ( 4)
T - 1 - 1
( 9) ( 10)
2
1 2 1 2 0 Rv - 2 Rv T T
2
1 T 2 R2 v
( 16)
H R
T
- 1
( 1) Z
由此可以看出 , 在实际的卡尔曼滤波算法启动 初始值计算时, Rr , Rv 的值需要由经验获得。 ( 3) 第三种计算方法 初始向量的计算与第二种方法相同 , 初始估值 的协方差矩阵对滤波的影响不大 , 可以给出一个对 角阵作为它的初始值。 几种方法的分析比较 : 第一种方法, 通过一次观测后 , 就可启动滤波。 在准连续波雷达数据处理时, 观测量同时有距离和
式中 : Z 为观测矢量。这样给出的 X , P ( 1) 可以作为 一般卡尔曼滤波的初始条件来用。 ( 2) 第二种计算方法 在平面内跟踪运动目标, 可以通过初始的两次 观测来启动这个滤波过程。 做两次测量, 得到两组观 测数据 ( r 1 , v 1) , ( r 2 , v 2) , 则初始的状态向量 X 为 X = 〔 x 0 x 1 x 2 〕 = r2 v 2 1 ( v 2 - v 1 ) T
1 引 言
标准卡尔曼滤波在理论上是非常完美的 , 也就 是说卡尔曼滤波只有在其数学模型确定的情况下才 是最佳的。 但要将它应用于实际系统中, 有许多问题 需要解决, 其中最主要的是 : 滤波数学模型和数值发 散问题。 我们知道在实际应用中 , 要精确地建立滤波 数学模型是相当困难的 , 甚至是不可能的。 在滤波数 学模型与实际过程的数学规律不匹配时, 会使滤波 精度下降, 严重时会导致滤波发散, 这是实际的状态 估计误差趋向无穷大。还有容易出现的问题是数值 发散。 产生发散现象主要原因有 : 系统的数学模型和 噪声的统计模型不准确; 另外, 计算机字长有限 , 因 而存在着计算误差。本文提出了一种自适应卡尔曼 滤波的方法对滤波发散和计算发散都有较好的抑制 作用, 从而提高了滤波的处理精度。 式中 5 ( k) = 观测方程为
T - 1
现代雷达
23 卷
1 ( 6) ~ T Z= 〔 Dr 2 Dv 2 Dv 1 〕 B = 0 0 ( 7) 因此可以求得
0 1 1 T -
0 0 1 T ( 13)
3 新型自适应卡尔曼滤波器初始值的确定
新型自适应卡尔曼滤波器初始值确定滤波过程 的启动主要是设法提供估值的初始状态向量和初始 协方差矩阵。 本文研究了三种取得初始值的方法。 取 得初始值的算法要求运算简单, 精度尽可能高, 利于 启动卡尔曼滤波算法。
T 2
( 23)
R2 A =
-û2 / 9 A m ax - û A
0
d( k + 1/ k ) 、 ( 2) 由时间传播方程的计算可得 X P ( k + 1/ k ) 。 ( 3) 输入新的测量值 Z ( k + 1) 。 ( 4) 残差计算, 采用公式 ~ Z ( k + 1) = Z ( k + 1) d ~ H ( k ) X ( k + 1/ k ) , 得到 Z ( k ) 。 ( 5) R ( k ) 值计算。根据参考文献〔 2〕 的自适应计 算算法
( 18)
第 6 期 高 勤等 : 一种新型自适应卡尔曼滤波算法 31
1 1 E+ 2 Rr P ( 1) = 0
ቤተ መጻሕፍቲ ባይዱ
0 1 1 E+ 2 Rv 0
0 其中 0 1 E ( 20)
Q ( k) = E 〔 W ( k) W ( k) 〕 = 2A RAQ0
1〕 ( 1) 第一种计算方法 〔 1 设定 P ( 0) = I , 式中 I 为单位矩阵 n× n, E 为 E 很小的一个正数。于是
~X ~T 〕 ~~ ~~ P = e〔 X = E〔 BZ ZTBT 〕 = BE 〔 Z ZT 〕 B ( 14) 可以假定逐次测量是不相关的 , 则 R2 r
k d( k ) = 1 2 d( j / k ) - r 〕 R 〔 Z( j ) - H ( j ) X k j= 1 d T 〔 Z ( j ) - H ( j ) X ( j / k) - r 〕 ( 24)
r E R2 r + 1 X = v E R2 v + 1 0 又因为 E 取一个很小的正数 , 经过简化可得 R2 r P ( 1) ≈ 0 0 0 R
2 v
( 21)
0
0
r 0 X ≈ v 1 0 E
( 22)
d( k / k ) 、 d( k / ( 6) 由测量修正方程的计算得到 X P d k ) 。这样就得到相应与测量值 ~ Z ( k ) 的状态估计 X ( k / k) , 输出对应的状态估计得到滤波值。 ( 7) Q( k ) 值的计算可根据式 ( 23) 的自适应计算 方法。 ( 8) 时间 k 值加 1, 重复 ( 2) 至( 7) , 输入观测值 , 得到相应的滤波输出直至滤波完成。
2 新型自适应卡尔曼滤波算法
离散状态方程为 -+ W ( k ) ( 1) X ( k + 1) = 5 ( k ) X ( k) + U ( k ) A 其中 b( k ) 〕 X ( k) = 〔 x ( k ) xa ( k) x
X 收稿日期 : 2001 — 01 — 31
30 K ( k ) = P ( k / k - 1) H T ( k ) 〔 H ( k ) P ( k / k - 1) H ( k ) + R ( k ) 〕 P ( k/ k) = 〔 I - K ( k) H ( k) 〕 P ( k/ k - 1) d d X ( k / k ) = X ( k / k - 1) + K ( k) d( k / k - 1) 〕 〔 Z ( k) - H ( k) X
现代雷达
第6期
一种新型自适应卡尔曼滤波算法
高 勤 李志强 都学新 ( 军械工程学院光学与电子工程系 石家庄 050003)
X
【 摘要】 主要完成 卡尔曼滤 波在准连续 波雷达数据 处理 ( 距离和速 度 ) 中 算法应用研 究 , 实 现一种新型 自适应 卡尔曼滤波算法。通 过计算机仿真 , 该算法能够适应 不同的机动目标 , 并对测量数 据中的有色噪声有较 强的抑制作 用 , 满足实时要求 , 提高雷达测量精度。 【 关键词】 卡尔曼滤波 , 自适应滤波 , 机动目标
可以看出, 状态矢量中加速度为 0, 加速度估计 的均方误差 1 非常大, 对滤波后的数据处理的精度 E 影响非常大, 甚至卡尔曼滤波数值发散, 导致不能工 作 , 后面的数据仿真分析也将证明这一点。 第二种方法: 它通过两个观测数据来启动滤波过程, 与第一 1 种方法比较 , 不同的是: 状态矢量中加速度为 ( v 2 T - v 1 ) , 加速度估计的均方误差为 12 R2 v , 它比第一种 T 方法有明显的改善。 第三种方法: 它实际上是第二种方法的简化形式, 状态矢量 的计算方法不变 , 对误差协方差矩阵只取为对角阵 形式 , 与第二种方法相比 , 只是令 P ( 2, 1) = P ( 1, 2) = 0。因为按照第一种计算方法 , P 的形式可为对角 阵 , 对角上的值根据实际测量情况来获得, 但在滤波 启动一段时间后影响就不大了, 所以做此简化, 数据 仿真分析也证明该方法运算最为简单 , 精度较高。
5 仿真模型的确定
根据准连续波雷达需要处理的距离和速度, 一 般采用如下的数据进行仿真: ( 1) 匀速直线运动 x 0 = 1000m , y 0= 1000m , 第一种速度较低 v 0= 10m/ s( 称为 M ode 1) 第二种速度较高 v 0= 400m/ s( 称为 M ode 2) ( 2) 匀加速直线运动 x 0 = 1000m , y 0= 1000m , v 0= 10m / s 第一种加速度较小 a0= 1m/ s ( 称为 M ode 3) 第一种加速度较高 a0= 20m/ s2( 称为 Mode 4) ( 3) 加速度阶跃运动 ( 称为 Mode 5) 初始时 x 0 = 1000m, y 0 = 1000m, v 0 = 10m / s, a0 = 1m / s2, 开始后 100 秒, 加速度跳变为 a0= 30m / s2。 ( 4) 转圈运动 ( 称为 M ode 6) x 0 = 6000m , y 0 = 8000m , R = 1500m, 角速 度为 2P / 50rad / s, 法向加速度为 23. 7m / s2 。 我们应该清楚 , 在 M ode 4 和 Mode 5 中 , 当进 行比较长时间后 , 速度非常大 , 超出目标本身的最高 速度 , 我们进行数据仿真主要是检测滤波器的跟踪 性能 , 没有过分约束数据的真实条件。
T ~ E〔 Z~ Z 〕 =
0 R
2 v
0 0 R
2 v
0 0
( 15)
0
式中 : Rr , Rv 分别为距离和速度测量误差的方差, 代
2 2
入上面的公式可得 R2 r 0 0 P= 0 Rv 0
2
P - 1 ( 1) = E I + H T R - 1( 1) H
( 8)
R ( 1) 为观测噪声的方差矩阵 , 须由经验假定获 得。H 为观测矩阵 , 则 P ( 1) = ( E I + H T R - 1 ( 1) H ) - 1 X = (E I + H R ( 1) H )
A N ew A daptive K alman Filtering Algorithm
GAO Qin LI Zhi-qiang DU Xue-xin ( Ordnance Engineering College Shijiazhuang 050003)
【 Abstract 】 T he applications of the K alman filtering algor ithm in data processing ( range and speed) of quasicontinuouswav e radar ar e studied. A new adaptive K alm an algor ithm is r ealized. It is sho wn by comput er simulation that this algor ithm is applicable for all kinds of mo ving targ et s, able to suppr ess coloured measurement noise larg ely , meets the requir ement for real-time pr ocessing , and impro ves the accuracy o f radar measurement. 【 Key words 】 K alman filter ing, adapt ive filt er ing, moving targ et
距离和速度测量噪声不相关, 则 R ( 1) 为 2 × 2 的对 角阵 , 设定 R ( 1) = 则 E+ P - 1 ( 1) = 0 0 1 R2 r E+ 0 0 1 R2 v 0 0 E ( 19) R2 r 0 0 Rv
2
~ 2 = D 1 ( v 2 - v 1 ) = 1 ( Rv 2 - Rv 1) ( 12) X T T 式中: Dr 2, Dv 2, Dv 1 是两次目标坐标测量时距离与速 度的测量误差。 ~ = B~ 设X Z , 式中定 义 B 为测量误差的变换矩 阵, 则
T T
( 11)
速度 , 观测向量 Z = 〔 r v 〕, 观测矩阵 H = 1 0 0 1 0 0 ( 17)
T
~X ~T 〕 这时初始估值的协方差矩阵为 : P = E 〔 X , ~ 式中 X 是误差矢量。
T ~ = 〔 ~ 0 X ~ 1 X ~ 2〕 X X ~ 0 = Dr 2, X ~ 1 = Dv 2 X
1 0 0
T 1 0
(A T - 1+ e (1 - e e
- A T
- A T
)/A
2
- A T
/A
Z ( k ) = H ( k ) X ( k) + V ( k )
( 2)
H ( k) =
1 0 0 1
0 0
为标准的卡尔曼滤波方程。 时间传播方程 d( k / k - 1) = 5 ( k + 1, k ) X d( k - 1/ k - 1) X -( k ) + U ( k) A ( 3) P ( k / k - 1) = 5 ( k + 1, k ) P ( k - 1/ k - 1) 5 T ( k + 1, k ) + Q( k - 1) 根据均值自适应算法 , 状态值预测计算时采用 d( k / k - 1) = 5 ( k + 1, k ) X d( k - 1/ k - 1) ( 5) X 测量修正方程 ( 4)
T - 1 - 1
( 9) ( 10)
2
1 2 1 2 0 Rv - 2 Rv T T
2
1 T 2 R2 v
( 16)
H R
T
- 1
( 1) Z
由此可以看出 , 在实际的卡尔曼滤波算法启动 初始值计算时, Rr , Rv 的值需要由经验获得。 ( 3) 第三种计算方法 初始向量的计算与第二种方法相同 , 初始估值 的协方差矩阵对滤波的影响不大 , 可以给出一个对 角阵作为它的初始值。 几种方法的分析比较 : 第一种方法, 通过一次观测后 , 就可启动滤波。 在准连续波雷达数据处理时, 观测量同时有距离和
式中 : Z 为观测矢量。这样给出的 X , P ( 1) 可以作为 一般卡尔曼滤波的初始条件来用。 ( 2) 第二种计算方法 在平面内跟踪运动目标, 可以通过初始的两次 观测来启动这个滤波过程。 做两次测量, 得到两组观 测数据 ( r 1 , v 1) , ( r 2 , v 2) , 则初始的状态向量 X 为 X = 〔 x 0 x 1 x 2 〕 = r2 v 2 1 ( v 2 - v 1 ) T
1 引 言
标准卡尔曼滤波在理论上是非常完美的 , 也就 是说卡尔曼滤波只有在其数学模型确定的情况下才 是最佳的。 但要将它应用于实际系统中, 有许多问题 需要解决, 其中最主要的是 : 滤波数学模型和数值发 散问题。 我们知道在实际应用中 , 要精确地建立滤波 数学模型是相当困难的 , 甚至是不可能的。 在滤波数 学模型与实际过程的数学规律不匹配时, 会使滤波 精度下降, 严重时会导致滤波发散, 这是实际的状态 估计误差趋向无穷大。还有容易出现的问题是数值 发散。 产生发散现象主要原因有 : 系统的数学模型和 噪声的统计模型不准确; 另外, 计算机字长有限 , 因 而存在着计算误差。本文提出了一种自适应卡尔曼 滤波的方法对滤波发散和计算发散都有较好的抑制 作用, 从而提高了滤波的处理精度。 式中 5 ( k) = 观测方程为
T - 1
现代雷达
23 卷
1 ( 6) ~ T Z= 〔 Dr 2 Dv 2 Dv 1 〕 B = 0 0 ( 7) 因此可以求得
0 1 1 T -
0 0 1 T ( 13)
3 新型自适应卡尔曼滤波器初始值的确定
新型自适应卡尔曼滤波器初始值确定滤波过程 的启动主要是设法提供估值的初始状态向量和初始 协方差矩阵。 本文研究了三种取得初始值的方法。 取 得初始值的算法要求运算简单, 精度尽可能高, 利于 启动卡尔曼滤波算法。
T 2
( 23)
R2 A =
-û2 / 9 A m ax - û A
0
d( k + 1/ k ) 、 ( 2) 由时间传播方程的计算可得 X P ( k + 1/ k ) 。 ( 3) 输入新的测量值 Z ( k + 1) 。 ( 4) 残差计算, 采用公式 ~ Z ( k + 1) = Z ( k + 1) d ~ H ( k ) X ( k + 1/ k ) , 得到 Z ( k ) 。 ( 5) R ( k ) 值计算。根据参考文献〔 2〕 的自适应计 算算法
( 18)
第 6 期 高 勤等 : 一种新型自适应卡尔曼滤波算法 31
1 1 E+ 2 Rr P ( 1) = 0
ቤተ መጻሕፍቲ ባይዱ
0 1 1 E+ 2 Rv 0
0 其中 0 1 E ( 20)
Q ( k) = E 〔 W ( k) W ( k) 〕 = 2A RAQ0
1〕 ( 1) 第一种计算方法 〔 1 设定 P ( 0) = I , 式中 I 为单位矩阵 n× n, E 为 E 很小的一个正数。于是
~X ~T 〕 ~~ ~~ P = e〔 X = E〔 BZ ZTBT 〕 = BE 〔 Z ZT 〕 B ( 14) 可以假定逐次测量是不相关的 , 则 R2 r
k d( k ) = 1 2 d( j / k ) - r 〕 R 〔 Z( j ) - H ( j ) X k j= 1 d T 〔 Z ( j ) - H ( j ) X ( j / k) - r 〕 ( 24)
r E R2 r + 1 X = v E R2 v + 1 0 又因为 E 取一个很小的正数 , 经过简化可得 R2 r P ( 1) ≈ 0 0 0 R
2 v
( 21)
0
0
r 0 X ≈ v 1 0 E
( 22)
d( k / k ) 、 d( k / ( 6) 由测量修正方程的计算得到 X P d k ) 。这样就得到相应与测量值 ~ Z ( k ) 的状态估计 X ( k / k) , 输出对应的状态估计得到滤波值。 ( 7) Q( k ) 值的计算可根据式 ( 23) 的自适应计算 方法。 ( 8) 时间 k 值加 1, 重复 ( 2) 至( 7) , 输入观测值 , 得到相应的滤波输出直至滤波完成。
2 新型自适应卡尔曼滤波算法
离散状态方程为 -+ W ( k ) ( 1) X ( k + 1) = 5 ( k ) X ( k) + U ( k ) A 其中 b( k ) 〕 X ( k) = 〔 x ( k ) xa ( k) x
X 收稿日期 : 2001 — 01 — 31
30 K ( k ) = P ( k / k - 1) H T ( k ) 〔 H ( k ) P ( k / k - 1) H ( k ) + R ( k ) 〕 P ( k/ k) = 〔 I - K ( k) H ( k) 〕 P ( k/ k - 1) d d X ( k / k ) = X ( k / k - 1) + K ( k) d( k / k - 1) 〕 〔 Z ( k) - H ( k) X