２０１２年第２ｌ卷第８期 ｈｔｔｐ：／／ｗｗｗ．Ｃ－Ｓ—ａ．ｏｒｇ．ＣＦＩ 计算机系统应用 基于三维扩展卡尔曼滤波的算法① 屠晓东，张浩 ．（中国海洋大学信息科学与工程学院，青岛２６６１００） 摘要：现在的定位算法通常是二维形式的，在基于扩展卡尔曼滤波的ＴＤＯＡ／ＡＯＡ算法的基础上推导出了该算 法的三维形式。在本文中将方位角、俯仰角、ＴＤＯＡ三个量构造成符合扩展卡尔曼滤波器的模型，可求得三个 分量，从而最终得到移动基站的位置估计。我们仿真了该方法的跟踪轨迹，推算出参数克拉美罗下限 关键词：Ｗｅｂ信息提取：知识表示；数据密集型Ｗｅｂ页面；基于本体的关键词库 Ｐａｒａｍｅｔｅｒｓ ３一Ｄ Ｌｏｃａｔｉｏｎ Ｂａｓｅｄ ｏｎ Ｅｘｔｅｎｄｅｄ Ｋａｌｍａｎ Ｆｉｌｔｅｒ ＴＵ Ｘｉａｏ—Ｄｏｎｇ，ＺＨＡＮＧ Ｈａｏ （Ｃｈｉｎａ Ｏｃｅａｎ Ｕｎｉｖｅｒｓｉｔｙ ｏｆＣｈｉｎａ，Ｑｉｎｇｄａｏ ２６６１００，Ｃｈｉｎａ） Ａｂｓｔｒａｃｔ：Ｆｏｒ ｎｏｗ ｔｈｅ ｐｏｓｉｔｉｏｎｉｎｇ ａｌｇｏｒｉｔｈｍ ｉｓ ｕｓｕａｌｌｙ ｔｗｏ・ｄｉｍｅｎｓｉｏｎａｌ ｆｏｒｍ．Ｂａｓｅｄ ｏｎ ｔｈｅ ＴＤＯＡ／ＡＯＡ ａｌｇｏｒｉｔｈｍ ｗｈｉｃｈ ｉｓ ｏｎ ｔｈｅ ｂａｓｉｓ ｏｆ ｅｘｔｅｎｄｅｄ Ｋａｌｍａｎ ｆｉｌｔｅｒ，ｗｅ ｄｅｒｉｖｅ ｔｈｅ ｔｈｒｅｅ—ｄｉｍｅｎｓｉｏｎａｌ ｆｏｒｍ ｏｆ ｔｈｅ ａｌｇｏｒｉｔｈｍ．Ｉｎ ｔｈｉｓ ａｒｔｉｃｌｅ， ｔｈｒｅｅ ｑｕａｎｔｉｔｉｅｓ ｉｎｃｌｕｄｉｎｇ ａｚｉｍｕｔｈ，ｐｉｔｃｈ ａｎｇｌｅ，ＴＤＯＡ ｃｏｎｓｔｒｕｃｔｅｓ ａ ｍｏｄｅｌ ｔｈａｔ ｉｎ ｌｉｎｅ ｗｉｔｈ ｔｈｅ ｅｘｔｅｎｄｅｄ Ｋａｌｍａｎ ｆｉｌｔｅｒ． Ｔｈｕｓ ｔｈｅ ｔｈｒｅｅ ｃｏｏｒｄｉｎａｔｅ ｃｏｍｐｏｎｅｎｔｓ ｃａｎ ｂｅ ｏｂｔａｉｎｅｄ，ａｎｄ ｕｌｔｉｍａｔｅｌｙ ｇｅｔ ｔｈｅ ｌｏｃａｔｉｏｎ ｏｆ ｔｈｅ ｍｏｂｉｌｅ ｂａｓｅ ｓｔａｔｉｏｎ ｅｓｔｉｍａｔｅｓ．Ｗｅ ｓｉｍｕｌａｔｅｄ ｔｈｅ ｔｒａｃｋｉｎｇ ｔｒａｊｅｃｔｏｒｙ ｏｆ ｔｈｅ ａｌｇｏｒｉｔｍ，ａｎｄ ｄｅｄｕｃｅｄ ｔｈｈｅ ｐａｒａｍｅｔｅｒｓ Ｃｒａｍｅｒ－Ｒａｏ Ｌｏｗ Ｂｏｕｎｄ （ＣＲＬＢ）． Ｋｅｙ ｗｏｒｄｓ：ｋａｌｍａｎ；ｃｒｉｂ；ｔｄｏａ／ａｏａ 自从美国ＦＣＣ（联邦通信委员会）于１９９６年发布Ｅ 一法的基础上，将其推广应用到三维空间中。计算克拉 ９ｌｌ定位标准以来，无线定位技术就以其蕴涵的无 美罗下限和仿真跟踪轨迹发现该方法偶较高的精度和 较好的稳健性。 穷商业潜力掀起了研究的热潮。目前，无线定位方法 大部分属于静态定位的方法，即利用某个时刻的到达 时间或到达角度测量值估计出该时刻ＭＳ的位置坐 标。卡尔曼滤波是从有限的观测到的目标位置预测定 ｌ三维ＴＤＯＡ／ＡＯＡ定位系统的向量模型 设Ｎ个观测站的坐标分别为（ ， ，ｚ１）， （Ｘ２，Ｙ２，ｚ２），…，（ ，ＹⅣ，ｚＮ），设被定位对象的坐 标为 ， ，ｚ），用向量的方式表示定位模型如下： Ｙ＝ｈ（Ｘ）＋Ｖ 其中 ＝ ，位坐标的典型例子。卡尔曼滤波被广泛应用在由于ＭＳ 在移动过程中的位置是连续变化的，故ＭＳ在某一时 刻的位置与其在上一刻的位置有关，所以现在有良好 跟踪性能的卡尔曼滤波被广泛应用于对移动台的定位 （１） 中，来提高定位精度。卡尔曼滤波被广泛应用于头脸 识别ｌ、图像分割、目标跟踪２，３等等。 定位方法的研究大多是基于二维平面坐标的，但 Ｙ，ｚ】 Ｙ＝［ 一 ， ，… ， ，…， 】 ３Ⅳ＿ｌ１ 是在现实生活中需要对ＭＳ在三维空问进行定位。本 文在深入研究基于扩展卡尔曼滤波的ＴＤＯＡ／ＡＯＡ算 Ｖ＝［ 。，… ，… ， ，… ］：３ ） 每个观测站均能获得方位角 、俯仰角 的观测 ①收稿时间：２０１１－１１－２３；收到修改稿时［ｈｑ：２０１２一Ｏ１—２０ Ｒｅｓｅａｒｃｈ ａｎｄ Ｄｅｖｅｌｏｐｍｅｎｔ研究开发７３ 计算机系统应用 ｈｔｔｐ：Ｎｗｗｗ．ｃ－Ｓ—ａ．ｏｒｇ．ｃｎ ２０１２年第２ｌ卷第８期 量，除了主站以外的其它站均能获得ＴＤＯＡ的观测量 Ｐ。下面用公式描述这些观测量： 方位角： ａｒｃｔ卸 Ｙ －一其中， ２ 是驱动噪声的方差，由它来弥补 矩 阵描述运动规律的局限性，并适应于任何运动规律。 Ｔ是采样时间间隔，也理解为计算机仿真的步长。 Ｙｔ ］，ｆ＝ｌ，２，…’Ⅳ （２） 目标的观测方程定义如下 ： （ ）十 （９） 俯仰角： ＝一（ 霸 卜２，… 到达时间差： ＝ 丽一 丽 ｆ＝Ｚ・・ Ⅳ （４） ２扩展卡尔曼滤波模型 ２．１模型的跟踪轨迹 扩展卡尔曼滤波是一种线性最小方差估计，算法 具有递推性，适用于对多维随机过程进行估计。 为了利用扩展卡尔曼滤波器，必须用下面的模型进行 描述【 』ｘＧ＋ ）＝ｆ（Ｘ（ｑ）， （ ）） 【】，（ ＋ ）＝ｈ（ｘＧ＋１））＋ （ ＋ ） ｒ 其中，函数ｆ由目标的运动规律确定，函数ｈ为前面 描述的三维ＴＤＯＡ／ＡＯＡ定位模型。 首先由运动规律确定目标的状态方程 ＋ｌ＝ Ｓｋ＋ （６） 其中 ＝［ｘｋ，Ｙ ，ｚ七，Ｖｘ Ｖｙ３ Ｖ砧】 是ｔｋ时刻的状态向 量，（ ，Ｙｋ，ｚ七）是目标的位置坐标，（ 。 ，Ｖｙ， ，Ｖ：， ） 代表目标的瞬时速度。状态转移矩阵为 ，噪声向量 的协方差矩阵为Ｑ，它们的定义如下 ７４研究开发Ｒｅｓｅａｒｃｈ ａｎｄ Ｄｅｖｅｌｏｐｍｅｎｔ 其中 是观测数据向量， 是观测噪声，由于 （Ｓｋ） 是非线性变换，为了应用扩展卡尔曼滤波，进行线性化 后的观测方程如下 ＝ ＋ （１ｏ） 仿真结果显示如下： ５０ ２００ 耋１５０ １∞ ５０ 口 １［哪 图１被定位节点的追踪轨迹 图１假设被定位节点从Ａ（１００，５００，３００）匀速移动 到Ｂ（９００，５００，Ｏ），从图中跟踪轨迹可以看出估计轨迹跟 实际在整个过程中基本吻合，说明该方法在三维空间 中有较好的稳健性。 ２．２模型的克拉美罗下限 对于参数估计问题，克拉美一罗界任何无偏估计 量的方差确定了一个下限，即不可能求得方差小于下 限的无偏估计量，并为比较无偏估计量的性能提供一 个标准，而且当无偏估计量达不到ＣＲＢ也可以渐进达 到这个下界。 设日 为ｋ时刻ｈ（Ｓ ）在级数展开点的雅可比矩 阵，那么 垫 ！ ； ２ １ ２ ｏ ｏ ｏ ａｌ ａｙ ａｚ ２ ２ 垫 ２ ０ ｏ ｏ ｍ ａｖ Ｏｚ 垫 ＝！ ２ 堡 ：ｊ ２垫 ！ ２ ０ ０ ０ 其中， 研Ｙ－翮Ｚ ２０１２年第２ｌ卷第８期 ｈｔｔｐ：／／ｗｗｗ．ｃ—Ｓ－ａ．ｏｒｇ．ｃｎ 计算机系统应用 ２ ３５ （１３） －３０ 爸２５ （ ｚ－ｚ，）（ｘ －ｘＤ ｕ 加 （ 一 ＋ ＋ 一 ＋ ）ｊｌ ｌ ＿＿ Ｉ １５ （１４） １０ 屹＝ （１５） 屹： （１６） ＝—————— —————１ （ 一２峨＋ ＋ ？２ｙｙ，＋ ： ２ｚｚ ＋： 一（ — 』） 』 （ 一２ ＋ ＋ｙ ２ｙｙ，＋ｙｌ＋Ｚ２—２ｚｚ ＋＝ （１７） ＝—————— Ｌ＿———１ 一２ ＋ｔ＋ｙ：一２ ．＋ｙ ＋ｚ：一２ｚ２ ＋ｚ 二 ＝ ！ Ｉ’ 一２ ＋ｔ＋ｙ：一２ｙｙｉ＋ｙｆ＋ｚ２－２ｚｚ￣＋ｚ＇） （１８） ＝—————— Ｌ————１ —２ ＋ｉ＋ 一２ ＋ ＋ 一２ ＋毒 ｚ－ｚ１） Ｊ —２ ＋ ＋ 一２ｙｙ ＋ ＋ 一２ ＋ｔ （１９） 克拉美罗下限的计算公式如下；， ６ 中＝［ Ｑ 日］： 霹 （２０） ＣＲＬＢ＝砰＋霹＋…＋ （２１） １口 图２ 中克拉美罗下限随ＴＤＯＡ和ＡＯＡ的变换 ｘ轴表示ＴＤＯＡ噪声，Ｙ轴表示ＡＯＡ噪声，ｚ 轴表示克拉美罗下限。图中显示克拉美罗下限是随着 ＴＤＯＡ噪声和ＡＯＡ噪声的增长而增长的。虽然曲线 不是完全平衡，但总体是是呈上升趋势的。 ３ 结语 该论文研究了基于扩展卡尔曼滤波的ＴＤＯＡ／ ＡＯＡ定位算法，并把它扩展到三维空间中。由克拉美 罗下限和跟踪轨迹可推算出该方法具有较高的稳定性 和精度。 参考文献 ｌ Ｊａｈｎｓ Ｊ，Ｈｏｌｍｄｅｌ ＪＮ．Ｏｐｔｉｃａｌ Ｃｏｍｐｕｔｉｎｇ Ｈａｒｄｗａｒｅ．［ｓ．ｔ．】： Ａ：ｒ＆ｔ Ａｃａｄｅｍｉｃ Ｐｒｅｓｓ，１ ９９４． ２ Ｍｕｒｄｏｃｃａ Ｍ．Ａｄｖａｎｃｅｓ ｉｎ Ｄｉｇｉｍｌ Ｏｐｔｉｃａｌ Ｃｏｍｐｕｔｉｎｇ．Ｔｈｅ Ｉｎｔｅｍａｔｉｏｎａｌ Ｊｏｕｒｎａｌ ｏｆ Ｏｐｔｏｅｌｅｃｃｒｔｒｏｎｉｃｓ．１ ９９０，３（２）：１ ９ １－ ２０５． ３ Ｈａｉｂｏｃｈ ＦＧ Ｍｙｒｉｃｋ ＭＬ．Ｐｒｅｃｉｓｉｏｎ ｉｎ ｍｕｌｔｉｖａｒｉａｔｅ ｏｐｔｉｃａｌ ｃｏｍｐｕｔｉｎｇ．Ａｐｐｔ Ｏｐｔ，２００４，４３（１Ｏ）：２１２—２ｌ７． ４赵春晖，张朝柱．自适应信号处理．哈尔滨：哈尔滨工程大学 出版社。２００６． ５陈志波，陆雍森．Ｓｕｒｆｅｒ在环境评价和规划中的应用．同济大 学学报（自然科学版），２００５，３３（２）：１９１—１９５． ６韩丽娜，石吴苏．利用Ｓｕｒｆｅｒ ８．０绘制地质等值线图．计算机 与现代化，２００８，１１：８５—８８． ７张二勇，李云峰，王玮．Ｓｕｒｆｅｒ软件绘图接口的开发及应用．地 下水，２００５，２７（３）：２１２—２１４． Ｒｅｓｅａｒｃｈ ａｎｄ Ｄｅｖｅｌｏｐｍｅｎｔ研究开发７５