实验题目：基于卡尔曼滤波的目标定位设计

1. 课程简要信息

课程名称：数字信号处理
实验案例名称：基于卡尔曼滤波的目标定位设计
课程学时：理论课程 48学时 $^ +$ 课内实验 16学时
实验案例学时：课内4学时+课外
适用专业：通信工程、自动化、电气工程及其自动化、电子信息工程、电子信息科学与技术
学生年级：第三学年第一学期

2. 实验内容与任务（限 500字，可与“实验过程及要求”合并）

实验内容：

使用两个或多个无线模块对平面上的目标进行定位测量，并应用卡尔曼滤波算法来估算目标的最优位置，提升定位精度。

实验任务：

（1）基础任务：

1）针对系统建立数学模型；
2）通过无线方式得到无线基站到目标的距离，并转换为实际坐标；
3）运用卡尔曼滤波算法，减少测量噪声，估算最佳目标位置；
4）会调整系统参数，改变输出响应。

（2）拓展任务：

1）会对整个系统进行优化，包括主控模块的编程、实验的参数等等
2）可实现对运动目标的跟踪测量

3. 实验过程及要求（限 300 字）

（1）掌握卡尔曼滤波算法原理，了解无线定位原理，学习构建数学模型方法；
（2）掌握无线基站测距与调试方法，了解 TOA等各种通信协议、标签种类、天线种类；根据实验设计要求，完成预习报告；
（3）利用 Matlab或者Excel软件对各单元电路进行功能仿真测试，并记录仿真结果；
（4）根据定位原理合理布置基站和标签的位置，并记录；
（5）进行整体系统调试，观察噪声参数不同，卡尔曼滤波的效果，并分析原因；
（6）结合仿真和硬件测试数据，进行系统分析，最后撰写实验报告。

4. 相关知识及背景（限 150 字）

卡尔曼滤波算法在现代控制理论中应用非常广泛，比如卫星定位导航、目标跟踪、航空航天、自动驾驶等。

本实验综合运用了卡尔曼滤波、TOA 算法、无线定位技术等知识，使用无线射频基站对标志物的距离进行测量，通过卡尔曼滤波算法将多个带有噪声的传感器数据进行融合，使系统能够准确测定标志物的位置。

5. 教学目标与目的（限 100 字）

通过本次实验加深学生对卡尔曼滤波、无线定位、TOA测距等知识的理解；通过实践与仿真相结合培养学生对线性系统的设计、测试的能力；提升学生综合知识的运用能力，锻炼学生的工程实践能力与创新思维。

6. 教学设计与引导

本实验属于系统综合性实验，包含多个基础知识点的学习，教学过程包括：课前预习、课堂讲解、系统设计、软件仿真、综合调试、总结答辩。各阶段的教学目标不同，主要采用仿真和实践相结合的教学方法，让学生在实践中体会理论，在理论学习过程中融合实验，同时会从以下几个方面加强对学生的引导（见图6-1）：

（1）实验课前，要求学生复习相关理论知识（无线定位原理，TOA 算法，卡尔曼滤波等）。留有思考问题，引导学生使用Matlab进行编程练习；

（2）由本实验的工程背景引入，激发学生学习兴趣，和学生一起分析系统的重要知识点，将系统分解成多个模块，进行知识拆解练习；

（3）实验主要解决如何得到电子标签的精确位置，结合《数字信号处理》课程的卡尔曼滤波器设计理论，引导学生在大量数据中找到规律，选择合适的算法，确定滤波器的各个参数；

（4）对系统改变不同的噪声参数，对比测量结果的不同，引导学生从实验中分析其理论依据，并用Matlab进行仿真演示，让学生从实践再回到理论，加深对课堂理论知识的理解，提升自主学习的积极性；

（5）实验结束后，组织学生以项目答辩、讲评等的形式进行交流，再列举多个生活中的例子来引导学生从理论到实践，将学到的知识如何运用到实际的工程中，提升学生处理工程实践问题的能力。

图 6-1 教学设计与构思

7. 实验原理及方案

（1）实验原理

本实验涉及的主要知识是卡尔曼滤波以及无线测距方法。

卡尔曼滤波算法原理：

卡尔曼滤波是在有随机干扰和噪声的情况下，以线性最小方差估计方法给出状态的最优估计值。它可以应用在任何含有不确定信息的动态系统中，可有效的抵抗噪声的干扰并对状态变量作为最优估计，这使得其非适合应用在姿态解算、轨迹规划和目标定位等方面。根据运动学公式，其离散模型如下，状态方程：

$$ x _ { k + 1 } = A _ { k } x _ { k } + B _ { k } u _ { k } + w _ { k } $$

观测方程：

$$ y _ { k } = H x _ { k } + v _ { k } $$

其中式中 $x _ { k }$ 为状态矩阵， $u _ { k }$ 为控制矩阵， $y _ { k }$ 为观测矩阵， $A _ { k }$ 为状态转移矩阵， $B _ { k }$ 为状态控制矩阵，H为观测矩阵， $w _ { k }$ 为 $\mathsf { k }$ 时刻的动态噪声， $v _ { k }$ 为 $\mathsf { k }$ 时刻的观测噪声。

时间更新方程：

$$ \left{ \begin{array} { l l } { { \widehat { x } } ^ { - } = A { \widehat { x } } _ { k - 1 } ^ { - } + B _ { k } u _ { k - 1 } } \ { P _ { k } ^ { - } = A P _ { k - 1 } A ^ { T } + Q } \end{array} \right. $$

状态更新方程：

$$ \left{ \begin{array} { c } { K _ { k } = P _ { k } ^ { - } H ^ { T } ( H P _ { k } ^ { - } H ^ { T } + R ) ^ { - 1 } } \ { \hat { x } _ { k } = \hat { x } ^ { - } + K _ { k } ( y _ { k } - H \hat { x } ^ { - } ) } \ { P _ { k } = ( I - K _ { k } H ) P _ { k } ^ { - } } \end{array} \right. $$

$P _ { k }$ 为协方差矩阵， $K _ { k }$ 表示卡尔曼增益，表明根据上一时刻最优估计并加上已知的外部控制量预测得到当前时刻新的最优估计。卡尔曼滤波分为两步对模型中的参数进行更新，分别是时间更新和状态更新，以保证跟踪的实时准确。可以看出 $\widehat { x } _ { k }$ 是新的最优估计，可以将它和新的 $P _ { k }$ 继续在下一时刻的时间更新和状态更新中不断迭代。

无线测距的方法主要有四种， TOA、TDOA、AOA和RSSI四种，节点定位的流程一般是先测量距离，再使用一些经典的定位算法，比如三边测量法、三角测量法和极大似然估计法等，来获取待测节点的坐标位置。

单程双边测距 SS-TWR 属于 TOA 方法中的一种，当基站发送的信号到达接收端后，接收端返回一个确认信号，它是一个双向交互的过程。具体过程如图 7-1所示。

图 7-1 SS-TWR 算法原理

在T1时刻，设备 A向B发送一个消息包，在T2时刻设备B 收到该消息包，经过响应时间 Treply 之后，在 T3 时刻向设备 A 发送确认消息。Tround=2TOF+Treply，其中Tround是指设备A 发送测距消息到B返回用的总时间，由此可知：

进一步可以测得距离为：

V 近似为光速,取 $3 * 1 0 ^ { 8 }$ 。

（2）实现方案

为有效锻炼学生的工程实践与创新能力，本实验采用工程化引导的模式进行，主要包括建立系统模型、器件选型与调试、算法研究、系统调试、成果验收五个部分，见图7-2 所示。

图 7-2 工程化引导模式

具体方案如下：

1）首先按照实验任务，选择需要使用的硬件和算法，建立数学模型，并用尺量出标签到两个基站的距离 d1,d2，如图7-3所示。

图7-3 构建数学模型

2）每个组用不同的无线测距获取基站到标签的距离值，并记录数据 20 次以上。

3）参照卡尔曼滤波的 5个公式，建立本实验系统的卡尔曼估算模型。

4）运用上述计算方法对采集到的数据进行计算，绘制时间距离曲线，并调整系统噪声Q和测量噪声R 的参数，观察结果的变化，体会卡尔曼滤波的实际应用。

5）用 Matlab 程序自动实现卡尔曼滤波算法，给出最优标签位置，基础任务完成的同学可进行拓展练习，实现对移动目标进行跟踪测量。

6）最后进行总结讲评，由学生演示与讲解测量的过程。

8. 教学实施进程

本实验的教学实施进程共分为四部进行，综合学生学习成果可进行相应调整。

（1）预习安排阶段

为使学生巩固理论知识，任课教师提前 1~2 周布置预习任务以及实验题目和要求。学生需完成相关理论知识的预习，并完成预习报告。重点内容是卡尔曼滤波算法与无线定位原理，熟悉MATLAB 环境编程。

（2）现场教学阶段

本环节共 4 学时，分 2 次课进行。第一次课，任课教师进行重点知识的讲授，学生进行实际操作，教师进行指导，主要目的是熟悉测试流程，熟系掌握无线测距模块并调试好；第二次课，学生完成测试任务并绘制收敛曲线，重点内容是掌握卡尔曼滤波算法的应用。

（3）验收总结阶段

教师针对每一组学生的数据，分别进行分析测试，对于测量值与真实值差距较大的，分析问题产生原因，及时进行改正；符合要求的学生可以进行拓展功能的测试；最后由老师进行总结讲评。

（4）课后任务阶段

完成实验总结报告的撰写，将实验结果（包括计算过程）附在报告中，由教师进行评分。

9. 实验报告要求

预习报告：

（1）总结预习阶段的实验过程，学习到的知识点；
（2）针对仿真题目要求，运用 Matlab 初步对加噪声的数据进行处理；
（3）画出仿真图形，并说明总体设计思路。

实验报告：

（1）需要记录如何从获得的距离数据进行卡尔曼滤波的详细步骤；（2）记录仿真测试以及系统调试数据，并介绍系统仿真及调试方法，在仿真测试以及电路调试过程中遇到的问题以及排查经过，包括记录问题现象，分析存在原因，以及排除方法和效果等；（3）简单叙述对实验项目的结论性意见，对比改变系统参数对结果有哪些影响，并通过资料查询，给出系统进一步完善或改进的意向性说明；（4）对实验进行总结，写出实验的收获、体会。

10.考核要求与方法（限 300 字）

成绩评定采用分阶段多元化考核方式，总成绩由各部分成绩总和构成，同时根据实际情况适当调整评定项目及其构成。

（1）预习报告： $2 0 %$

考察学生对相关知识的复习结果、初步的设计方案是否合理、功能是否完整、对Matlab 程序掌握的程度；

（2）实验过程和结果（功能完成情况、原理分析、演示）： $5 0 %$

1）要求会读取电子标签的距离数据，并理解该通讯的原理；

2）理解卡尔曼滤波的 5个公式的含义，对获得的数据会套用公式计算；

3）会调节系统参数，并会应用matlab进行仿真；

4）实验的结果正确。

（3）拓展练习： $1 0 %$

1）会对整个系统进行优化，包括无线测量模块的程序优化等；

2）可实现对运动目标的跟踪测量。

（4）设计总结报告： $2 0 %$

实验课结束后一周内以班级为单位上交，主要考察报告的规范性和完整性。

11.项目特色或创新（可空缺，限 150 字）

（1）本实验是物联网开发中的典型应用，具有很强的实用性，能激发学生设计的兴趣和热情；

（2）实验采用全流程工程化，培养学生系统思维和工程实践能力；

（3）多元知识点的融合，培养学生创新能力。

一、实验目的

1.掌握卡尔曼滤波的工程应用方法；
2.掌握无线测距原理，工作方式，测距算法；
3.熟系数学建模的思路与方法。

二、实验内容

1.详细对比分析超声波、超宽带、RFID等无线通信模块的测距原理，并选择适合自己的测距方法并调试；

2.按照实验五的任务要求，采用的无线方式对车辆进行定位，用数学模型方法表示出车辆的坐标信息；

3.假定有一辆固定位置的小车，距离起点 50米，雷达测量的数据误差为 3米，请用 Matlab 仿真卡尔曼滤波算法，推导出估计距离，并画图表示。

三、预习要求

1. 简述卡尔曼滤波原理与推导过程；
2. 观看给定的视频课程与资料并记录心得体会、遇到的问题等；
3. 完成 Matlab 仿真习题。

四、预习成果

五、系统设计

1.简述无线定位的原理及卡尔曼滤波原理

2.Matlab 仿真题分析与结果

基于卡尔曼滤波的目标定位设计

实验目标

1、了解卡尔曼滤波的准则和信号模型，以及卡尔曼滤波的应用。
2、熟练掌握卡尔曼滤波的递推过程，提高对信号进行处理的能力。
3、分析讨论实际状态值和估计值的误差。

实验原理

卡尔曼滤波简介卡尔曼滤波是解决以均方误差最小为准则的最佳线性滤波问题，它根据前一个估计值和最近一个观察数据来估计信号的当前值。它是用状态方程和递推方法进行估计的，而它的解是以估计值(常常是状态变量的估计值)的形式给出其信号模型是从状态方程和量测方程得到的。卡尔曼过滤中信号和噪声是用状态方程和测量方程来表示的。因此设计卡尔曼滤波器要求已知状态方程和测量方程。它不需要知道全部过去的数据，采用递推的。

卡尔曼滤波的递推公式

$$ \begin{array} { c } { { \hat { x } _ { k } = A _ { k } \hat { x } _ { k } + H _ { k } ( y _ { k } - C _ { k } A _ { k } \hat { x } _ { k - 1 } ) } } \ { { { \ } } } \ { { H _ { k } = P _ { k } ^ { \prime } C _ { k } ^ { \tau } ( C _ { k } P _ { k } ^ { \prime } C _ { k } ^ { \tau } + R _ { k } ) ^ { - 1 } } } \ { { P _ { k } ^ { \prime } = A _ { k } P _ { k - 1 } A _ { k } ^ { \tau } + Q _ { k - 1 } } } \ { { P _ { k } = ( I - H _ { k } C _ { k } ) P _ { k } ^ { \prime } } } \end{array} $$

3. 递推过程的实现

如果初始状态 $\langle x _ { 0 }$ 的统计特性 $E [ x _ { 0 } ]$ 及?????? $[ x _ { 0 } ]$ 已知，并令

$$ \hat { x } _ { 0 } = E [ x _ { 0 } ] = \mu _ { 0 } $$

$$ P _ { 0 } = E [ ( x _ { 0 } - { \hat { x } } _ { 0 } ) ( x _ { 0 } - { \hat { x } } _ { 0 } ) ^ { \tau } ] = v a r [ x _ { 0 } ] $$

将 $P _ { 0 }$ 代入式（3）可求得 $P _ { 1 } ^ { \prime }$ ，将 $P _ { 1 } ^ { \prime }$ 代入式（2）可求得 $\cdot H _ { 1 }$ ，将此 $H _ { 1 }$ 代入式（1）可求得在最小均方误差条件下的 $\widehat { x } _ { 1 }$ ，同时将 $P _ { 1 } ^ { \prime }$ 代入式（4）又可求得 $P _ { 1 }$ ；由 $P _ { 1 }$ 又可求 $P _ { 2 } ^ { \prime }$ ，由 $P _ { 2 } ^ { \prime }$ 又可求得 $H _ { 2 }$ ，由 $H _ { 2 }$ 又可求得 ${ \widehat { x } } _ { 2 }$ ，同时由 $H _ { 2 }$ 与 $P _ { 2 } ^ { \prime }$ 又可求得 $P _ { 2 }$ ……；以此类推，这种递推计算方法用计算机计算十分方便。

实验器材

电脑 1 台无线基站 2 个标签 1 个

实验内容

卡尔曼滤波算法是目前应用最为广泛的滤波算法，在导航与定位、制导、通信

等领域得到了较好的应用。卡尔曼滤波的核心思想就是：根据当前仪器的测量值以及上一时刻的预测值和误差，计算得到当前最优量，再预测下一时刻的数值。

利用实验装置采集定位数据，运用卡尔曼滤波的方法对采集到的数据进行处理，分别求取与实际位置的均方差，绘制曲线图对比两组实验结果，调整Q 和R 的参数，观察结果的变化，体会卡尔曼滤波的实际应用。

实验步骤

1、将硬件开发板上的串口端连接串口（USB TO TTL）并连接任意电脑主机的USB 口；

2、打开串口调试助手，选择正确的COM口（波特率调制为 9600，停止位为1，数据位为 8，无奇偶校检），分别确认 AB 基站对应的串口号，在 matlab 程序中输入正确的串口号；

串口助手测试

3、打开基站与标签，采集定位数据，用米尺测量出实际定位坐标，求取均方差

4、利用卡尔曼滤波对采集到的定位信息进行处理，再次求取均方差，观察并对比两组数据结果

5、调整参数，再次对比实验结果

实验结果及分析

现场实验照片