基于卡尔曼滤波的GPS弹道测量误差消除方法

考 虑 到射 程 修 正 引 信 弹 载控 制 器 Ｄ Ｐ的 解 算 Ｓ 实 时 陛及弹 道辨 识 精 度 ， 用 质点 弹道 模 型 作 为 状 采 态 模 型ｌ 。状态方 程 如下 ： ５ ］
量测更新过程由滤波算法其余公式完成， 量测
３ ２
探 测 与 控 制 学 报
ｆ 一・ Ｆ ） ｓ 一 ｃ （・（一 ｉ 丌） ｇ ｎ
杨 小会 ， 鹏 飞 ， 霍 王 超
（ 电动态 控制 重点 实验 室 。 机 陕西 西 安 ７０ ６ ） １ ０ ５
摘 要 ： 针对弹道修正引信工作环境恶劣、 引信空间有限及要求成本低， 难以应用 Ｇ Ｓ 惯导组合 ， Ｐ／ 而最小二乘
滤波在系统表现 为强 非线性 时， 估计精度很差 ， 甚至完全不可用 ， 出一种基 于卡尔曼滤 波的 Ｇ Ｓ测量误 差消 提 Ｐ 除方法 。该方法结合质点 弹道模型 ， 采用 扩展 卡尔曼滤 波对 ＧＰ Ｓ测量值进 行最优估 计 ， 为弹道 预测提供 初始 弹道诸 元 。靶场试 验结 果表明 ： 扩展卡尔曼滤波最 优估计能有效 降低 Ｇ Ｓ Ｐ 测量误 差。
以达 到估 计误 差 最小 的运 动状 态 的 目的 。
２ １ 状 态模 型 ．
由图 １ 卡尔 曼 滤 波 式 （ ） 式 （ ） 以看 出 ， 和 １一 ９可 卡 尔曼 滤 波有两 个 明 显 的 信 息更 新 过 程 ： 间更 新 时 过 程 和量 测 更 新 过 程 。 时 间 更 新 过 程 完 成 状 态 预 估， 量测 更 新过 程用 来计 算对 时 间更新 值 的修 正量 。 时间更 新过 程 由式 （）式 （ ） 成 ， （ ） 成 ３ 、 ４完 式 ３完 了利用 弹 道动态 模 型对 弹丸 状态 进行 预估 ， （ ） 式 ４定 量 地描 述 了这 种预估 的质量 优劣 。
Ｔ ｉ ｍｅ ｏ ｂａ ｅ ｐｉ ｌ ｓｉ ｔ ｎ ＧＰ ａｕ ｅ ｎｓ ｙｃｍｂ ｉｇｔａ ｃｏｙ ｄ ｌｎ ｘｅｄ ｄＫ ｌ ｈｓ ｔ ｄｏ ｔｉ ｄｏ ｔ ｔ ｉ ｈ ｎ ｍａ ｅ ｍａ ｏ Ｓｍｅｓｒｍｅ ｔ ｂ ｏ ｉ ｎ ｊ ｔｒ ｅ ａｄｅ ｔｎ ｅ ａ— ｎ ｒｅ ｍｏ
时 间更新 方程 ：
Ｘ／－ Ｘ－ ＋ ｆ（ ｋ１ ・Ａ ｋ－ ｋ － １一 ｋ１ － ｘ－ ） ｔ
Ｐｋ － 一 ／ １ ｋ
，
（） １ （） ２
（） ３
（） ４
２ 卡 尔 曼 滤 波 误 差 消 除方 法
卡尔 曼滤 波误 差消 除方 法首 先利 用 弹 道动 态 模 型对 弹丸 状态进 行 预估 ， 利用 Ｇ Ｓ量 测值 对 状 态 再 Ｐ
Ｓ测 量误 差消 除方法 。 目前 我 国 只 能 利 用 ＧＰ Ｓ中 的粗 测 距 码 （ ／ Ｃ Ａ ＧＰ
码） 进行定位 ， Ｐ 测量值存在较大随机误差 。传统 Ｇ Ｓ 上利 用惯 性 导 航 系 统 （ｎ ｒｉｌ ｖｇ ｔ ｎ Ｓ ｓｅ Ｉ ｅｔ ｉａｉ ｙｔｍ， ａ Ｎａ ｏ 简称 Ｉ ） ＧＰ ＮＳ 与 Ｓ优 势 互 补 的特 点 ， 两者 组 合成 将
关键 词 ： 全球定位系统（ Ｐ ）弹道修正； Ｇ Ｓ； 弹道预测； 卡尔曼滤波
中图分类 号 ：Ｊ３． 文献标 志 码 ： 文章 编 号 ：０８１９ （０２０— ３－４ Ｔ４０３ Ａ １０－１４２１ ） ０ ０ ４０ ０
ＧＰ ｒｊｃｏｙＭｅｓｒｍｅ ｔ ｒｏｓＥ ｉ ｎ ｔ ｎ ＳＴ ａ ｔｒ ａｕ ｅ ｎ ｒ ｒ ｌ ｅ Ｅ ｍｉａｉ ｏ
第 一偏心 率 ： 一
上
。
傣一 『ｇ ｄ ０： 一 …
ｔ一 ｌ ｄ ・ｓｎ ｉ
（ １ ０ ）
一
在 ＷＧＳ８ －４坐标 系 中 ， 地球 椭 圆长轴 半 径 为 Ｒ ６ ３ ８ １ ７ ｍ，地 球 椭 圆 短 轴 半 径 为 Ｒ。一 ７ ３
６ ３５ ２ ｍ 。 ６ ７５
ｓ ａ ｅ ａ ｄ ｌ ｗ ｏ ｔ ａ ｍｅ ｈ ｄ ｏ ｐ ｃ ｎ ｏ ｃ ｓ ， ｔ ｏ ｆＧＰＳ ｍ ｅ ｓ ｒ ｍｅ ｔｅ ｒ ｒ ｌ ｎ ｔ ｎ ｂ ｓ ｄ ｏ ｌ ｎ Ｆ ｌ ｒｎ ｓｐ ｅ ｅ ｔｄ ａ ｕ ｅ ｎ ｒｏ ｓ ｅｉ ｍｉ ａ ｉ ａ ｅ ｎ Ｋａｍａ ｉ ｅ ｉｇ ｗａ ｒ ｓ ｎ ｅ ． ｏ ｔ
＊ 收稿 日期 ：０ ２０ —１ ２ １ —２０
修 回 日期 ：０ ２０ —８ ２ １ —４１
作者简介 ： 杨小会 （ ９ ９ ， ， １７ 一） 女 陕西澄城人 ， 士 ， 硕 工程师 ， 研究方 向： 弹道修正。Ｅ ｍａｌｌ ｉｇ ５ ５ １３ ｃｍ。 － ｉ ｉ ｎ ８４ ＠ ６． ｏ ：ｂ
３ 试 验 验 证
以某型底 凹榴 弹为 试验平 台 ， 采用 弹载 ＧＰ Ｓ进
行 弹道 测量 ， 跟踪 雷达 全弹道 跟 踪弹道 ， 遥测 装置 实
气压 函数 ：
丌 一ｘ一 ｊ ｃ 一 ｅ（ ： ｐ 惫 ）
式 中， Ｒ 为空气常数 ， 表示标准气象条件的地面 Ｐ 大 气压 强 。
ｆ ｃ ｉ ｅｙ ｒ ｄ ｃ ｈ Ｓ ｍｅ ｓ ｒ ｍ ｅ ｔｅ ｒ ｒ ． ｅ ｔ ｌ ｅ ｕ ｅ ｔ ｅＧＰ ａ ｕ ｅ ｎ ｒ ｏ ｓ ｖ
Ｋｅ ｒ ｓ Ｇ Ｓ ｃｕｓ ｏｒｃｉ ；ｒ ｅｔｒ ｒｄｃｉｎ Ｋ ｌ ｎｆｔｒ ｇ ｙｗｏ ｄ ： Ｐ ；ｏ ｒｅｃｒｅｔ ｎ ｔ ｊｃｏｙｐ ｅｉ ｏ ； ａｍａ ｌ ｉ ｏ ａ ｔ ｉｅ ｎ
具体 的量 测值 （ ） 确定 。 所 滤波 增 益 （ ） Ｋｋ 是使 估计 的均方误 差 阵达 到最 小 的最 佳 增 益 阵 ， 通 过 时 间 它 更新 的质 量 优 劣 （ Ｉ ｔ ）与 量 测 信 息 的 质 量 优 劣 ， － （ ）自动确 定对 量测值 和 状态 预估 值 的利 用 比例 ， Ｒ
图 ｌ中 ， 卡尔曼 滤波 算法 见式 （） 式 （）状态 １一 ９， 方 程见 ２ １ 中弹 道 方 程 ， 测 方 程见 ２ ２节 中量 ．节 量 ．
测模型。
更新过程用来计算对 时间更新值的修正量， 该修正 量 由滤 波增 益 （ ） 量 测 与 状 态 的关 系 （ ）以 及 、 Ｈｋ
第 ３ 卷 第 ４期 ４
２１ ０ ２年 ８月
探 测 与 控 制 学 报
Ｊ ｕ ｎ ｌ ｆＤｅｅ ｔ ｎ ＆ Ｃｏ ｔｏ ｏ ｒ ａ ｔｃｉ ｏ ｏ ｎｒｌ
Ｖｏ ＿ ４ ＮＯ ４ ｌ３ ． Ａｕ ． ０１ ｇ２ ２
基 于 卡 尔 曼滤 波 的 ＧＰ Ｓ弹 道 测 量误 差消 除方 法
０ 引 言
测量 随机误 差 。
由于 弹道修 正 引 信 工 作 环 境 恶 劣 ， 信 空 间有 引
在弹道修正引信 中使用 全 球 定 位 系 统 （ ｏ ａ Ｐ ｓ ｉｎｎ ｙｔｍ， Ｇｌｂ ｌ ｏｉｏ ｉｇＳ ｓｅ 简 限以及要求成本低等原因 ， ｔ ＳＩ 而 称 Ｇ Ｓ 具有全天候、 Ｐ） 高精度 、 快速实时定位的优点 ， ＧＰ ／ＮＳ组合 导航 系统 不太 可能 ， 最 小二 乘 滤波
Ｙ／ ｋ ｌ— ｈ（ ｋ ） Ｘ／ １
１ （ Ｐ ／ １ ＋Ｒ） 研 Ｈｋ － ｋ
一
（） ７
（） ８ （） ９
Leabharlann Baidu
（ ～ Ｈ Ｐｋ Ｉ ） ／ ￣１ Ｉ ）
坼 一 坼 Ｉ Ｋｋ 一 ＋ （
ｏ ＝Ｅ ｘ ］ 豫 ＝ Ｅ０ 一 ＝ ｇ — Ｅ （ ｏ ） 一 ） 一 ｃ ｏ ［ 一 。 （。 。 ］
在 国外 被 广泛应 用 于射程 修 正引 信 中＿ 。随着 我 国 １ ］
仅考 虑 了量 测值 ， 没有 考虑 系统 动态 模 型 ， 在系 统表
估 甚 完 北斗 二代 卫星 导航 定位 系统 的建 成 ， 种 基 于 卫 星 现 为强非 线性 时 ， 计精 度很 差 ， 至 滤 波发 散 ， 这 全不 可 用 ＿ 。为 此 提 出 了一 种 基 于 卡 尔 曼 滤 波 的 ＿ ４ ］ 定位的弹道修正引信 的实现将成为可能。
ｍａ ｉ ｅ ｉ ｇ Ｔｈ ｓｍｅ ｈ ｄ ｃ ｕ ｄ ｄ ｃ ｅ ｓ ｎ ｌ ｅ ｃ ｆｍｅ ｓ ｒ ｍ ｅ ｔｅ ｒ ｒ Ａｔｔ ｅ ｓ ｍ ｅ ｔ ｎ ｆ ｔ ｒ ． ｉ ｔ ｏ ｏ ｌ ｅ ｒ ａ ｅ ｉ ｆ ｎ ｅ ｏ ａ ｕ ｅ ｎ ｒ ｏ ． ｈ ａ ｉ ， ｈ ｎ ｔ ｌｐ ｒ ｍｅ ｌ ｎ ｕ ｍｅ ｔ ｅ ｉ ｉ ａ ａ ａ — ｉ
Ｂａ ｅ ｎ Ｋ ａ ｍ ａ ｌｅ ｉ ｇ ｓ ｄ ｏ ｌ ｎ Ｆｉｔ ｒ ｎ
ＹＡＮＧ ａ ｈ ｉ ＨＵ ０ ｅ ｇ ｅ ， ＡＮＧ ａ Ｘｉｏ ｕ ， Ｐ ｎ ｆｉＷ Ｃｈ ｏ
（ ｃｅ ｃ ｎ ｃ ｎ ｌｇ ｎＥｌｃｒ ｍｅｈ ｎｃ ｌ ｎ ｍ ｉ Ｃｏ ｔｏ ａ ｏ ａｏ ｙ Ｘｉｎ７ ０ ６ ， ｉａ Ｓ ｉｎ ｅａ ｄ Ｔｅ ｈ ｏｏ ｙｏ ｅｔｏ ｃ ａ ｉａ Ｄｙ ａ ｃ ｎｒ ｌＬ ｂ ｒ ｔｒ ， ＇ １ ０ ５ Ｃｈｎ ） ａ
图 ｌ为卡 尔曼 滤波 误差 消 除原理 图。
１ ｝１ ， ＋Ｑ ・ ｔ Ｐ － １ Ａ
／ Ｉ 卜 一 ＋
量测 更新 方程 ：
１
最终估 计 出误 差最 小 的运 动 状 态 。 （） 预估 值进 行校 正 ， ５
图 １ 卡尔曼滤波误差 消除原理
Ｆ ｇ １ Ｐｒ ｃｐ ｅ ｆｅ ｉ ｎ ｔｎ ｒ ｏ ｓ ｂ ｓ ｄ Ｏｌｋ ｌ ｎ ｆ ｔ ｒ ｉ． ｉ ｉ ｌｓ ｏ ｌ ｎ ｍｉ ａ ｉ ｇ ｅ ｒ ｒ ａ ｅ ｉ ａｍａ ｉ ｅ ｌ
ｔｒ ｏ ｌ ｅ ｂａｎ ｄ ｆｒ ｒｊｃｏ ｙｐ ｅ ｉ ｉｎ Ｔ ｅ ｅｔ ｅ ｕｔ ｓ ｏ ｄｔ ａ ｔ ｅｅ ｔｎ ｅ ｌ ｎｆ ｔｒ ｏ ｌ ｆ ｅ ｓ ｕｄｂ ｔｉｅ ａｅ ｔｒ ｒｄｃ ｏ ． ｈ ｓ ｓ ｌ ｈ ｗｅ ｔ ｈ ｘｅ ｄ ｄＫａ ｃ ｏ ｏ ｔ ｔ ｔ ｒ ｓ ｈ ｍａ ｌ ｕｄｅ— ｉｅ ｃ
杨 小会 等 ： 于卡 尔曼滤 波的 Ｇ Ｓ弹道 测 量误 差消 除方 法 基 Ｐ
３ １
（） ６
阶及 二 阶以上 的高 阶 项 ， 得 到 的一 阶近 似 项 作 为 将 原状 态 方 程 和 测 量 方 程 的 近 似 表 述 形 式 。滤 波 公
式［ 如 下 。 。 ］ 初 始 化方 程 ： 一
Ｇ ＳＩ Ｐ ／ＮＳ组 合导 航系 统［ ， ２ 以提高 整 个 系统 的导 航 ］
１ 卡尔 曼 滤 波
在非 线性 领域 ， 卡尔 曼 滤 波 将 非 线 性 状 态 方程 精度及性 能； 或者采用最 小二乘估计［ ， ３ 降低 Ｇ Ｓ 和测 量方 程在 状 态估 计 时 刻 进 行 泰 勒 展 开 ， 略二 ］ Ｐ 忽
Ａｂ ｔ ａ ｔ Ｆ ｃ ｓ ｄｏ ａ ｔ ａ ｅ ｕｒｍｅ ｔｏ ｈｅｃ ｕ ｓ ｏ ｒｃｉｎ ｆ ｚ ｏ ｏ ｒｗｏ ｋｎ ｏ ｄｔ ｎ ｆｌ ｔｄ ｓ ｒ ｃ ： ｏ ｕ ｅ ｎｆｃｕ ｌｒｑ ｉｅ ｎ ｆｔ ｏ ｒｅ ｃｒｅ ｔ ｕ ｅ ｆｒｐ ｏ ｒ ｉｇ ｃ ｎ ｉｏ ｓｏ ｉ ｅ ｏ ｉ ｍｉ
记量 测方 程为 ｚ— ｈ ） Ｒ， 设 量 测 噪声 为 （ ＋ 假 相 互独 立 的不 相关 零均 值 白高斯 噪声 Ｒ ～ （ ，） ０口 。
式 中 ， 表示 弹道倾 角 ， 示 弹丸速 度 ， 表示 地面 表 坐标 系 中的射程 ， Ｙ表示 地面 坐标 系 中的射 向 ， 表 Ｃ 示 弹道 系数 ， ｇ表示 重 力加速 度 。