惯性导航实验

2D 地面车辆导航

zhu惯性导航和GPSzhuya

1. 简介

本项目的目的是为您提供实践惯性导航的机会。您将针对地面车辆在局部切平面（LTP）中实现一个简化的 IRS 机理。

需要完成的工作分为 11 个问题。需提交的书面报告应围绕您对这些问题的回答来组织。

本项目的内容结构如下：

第 2 部分（第 2 页） 介绍了您可使用的数据。
第 3 部分（第 3 页） 介绍了如何开发 2D IRS 机制以及对所获得结果的分析。
第 4 部分（第 8 页） 介绍了如何开发 GPS/IRS 融合方案并评估该融合方案的性能。

2. 数据描述

GPS 和 IMU 测量数据都是真实的。

在一辆汽车中安装了一个 GPS 接收机和一个 IMU：IMU 安装在车内；GPS 接收机天线安装在车顶。为简化起见，我们忽略了 IMU 与 GPS 接收机天线位置之间的杠杆臂，以及车辆重心与平台之间的杠杆臂。

该汽车绕 ENACZiegler 大楼行驶了 3 圈。行驶路径如图 1 所示，持续时间约为 4.5 分钟。

图 1 – 参考轨迹（使用轨迹测量设备记录）

在本次实验中，我们关注汽车在局部切平面（t-坐标系）中的二维运动。

2.1. 坐标系定义

2.1.1. 车辆坐标系定义

车辆坐标系，即 b-坐标系，在课程中已有定义。

2.1.2. 航向坐标系定义

航向坐标系是一个局部切平面（ned 坐标系），其中心（0 点）是轨迹的起始点。x 轴沿北向延伸，y 轴沿东向延伸，z 轴沿向下的竖直方向延伸，如图 2 所示。

2.1.3. 平台坐标系的定义

IMU 平台坐标系（即 p-坐标系）如图 3 所示。

(x^p,\, y^p,\, z^p)

定义了平台坐标系 p，这是一个 NWU 坐标系（北-西-上）。

2.2. GPS 测量

我们使用商用的 u-blox GPS 接收机来采集 GPS 测量数据，该接收机能够提供用户位置和速度估计。采集的数据速率为 1 Hz。

这些数据保存在文件 *GPS.mat 中，表 1 展示了该文件的数据结构：

Data	Size	Description
t\_GPS	nPts\_GPS \times 1	时间 [s]
nPts\_GPS	1 \times 1	采集到的 GPS 样本数量
ned\_GPS	nPts\_GPS \times 3	在 LTP（局部切平面）中的 GPS 位置 [m]，即 p_{b/t}
v\_ned\_GPS	nPts\_GPS \times 3	在 LTP 中的 GPS 速度 [m/s]，即 v_{b/e}^t

2.3. 惯性测量

使用 MTi Xsens IMU 采集了加速度计和陀螺仪数据，采集速率为 100 Hz。该 IMU 包含三轴加速度计和三轴陀螺仪（另有三轴磁力计，但在本项目中不使用），它们采用“绑载”（strapped-down）方式安装，并基于 MEMS 技术。

由于传感器精度所限，我们假设测量误差主要由噪声和偏置引起。因此，测量模型可表示为：

\tilde{f}^p(t) = f^p(t) - b_f(t) - n_f(t)

其中

\tilde{f}^p(t)：加速度计的测量值
f^p(t)：真实加速度
b_f(t)：加速度计偏置
n_f(t)：噪声项

其中，

\tilde{\omega}^p(t) = \omega^p(t) - b^\omega(t) - n^\omega(t)

\tilde{\omega}^p 和 \tilde{f}^p 分别是以 p-坐标系为参数的 IMU 输出陀螺仪和加速度计测量值；根据定义，f^p(t) = f_{b/i}^p(t) 且 \omega^p(t) = \omega_{b/i}^p(t)。

\omega^p 和 f^p 是感测到的比力（specific force）和角速率；b^\omega 和 b^f 是测量中的偏置，它们被假设为一阶 Markov 过程：

b^\omega(t) = -\frac{1}{\tau_\omega}\, b^\omega(t) + n^{b^\omega}(t) \quad\text{且}\quad n^{b^\omega}(t)\sim N\bigl(0,\sigma_{b_\omega}\bigr)

b^f(t) = -\frac{1}{\tau_f}\, b^f(t) + n^{b^f}(t) \quad\text{且}\quad n^{b^f}(t)\sim N\bigl(0,\sigma_{b_f}\bigr)

n^\omega 和 n^f 表示测量噪声，建模为 WCGN（白噪声）：

n^\omega(t)\sim N\bigl(0,\sigma_\omega\bigr), \quad n^f(t)\sim N\bigl(0,\sigma_f\bigr)

我们对 IMU 的误差特性进行了初步分析，假设的数值总结在表 2 中。

误差 / 特性	数值
加速度计噪声标准差 (\sigma_f)	0.1 m/s^2
加速度计偏置时间常数 (\tau_\omega)	5 s
加速度计偏置标准差 (\sigma_{b_f})	0.05 m/s^2
陀螺仪噪声标准差 (\sigma_\omega)	0.01 rad/s
陀螺仪偏置时间常数 (\tau_\omega)	4 s
陀螺仪偏置标准差 (\sigma_{b_\omega})	0.001 rad/s

表 2 – 假设的 IMU 误差特性

IMU 的安装方式使得 x^p 轴与车辆沿轨迹前进方向（即 x^b 与 x^p 完全对齐）一致，且 z^p 朝上（即 z^b 与 z^p 的方向相反）。图 4 展示了 IMU 在车内的位置示意。

IMU 数据

IMU 数据保存在文件 IMU.mat 中，表 3 展示了该文件的数据结构：

Data	Size	Description
t_{IMU}	nPts\_IMU \times 1	时间 [s]
nPts\_IMU	1 \times 1	采集到的 IMU 样本数量
Fs	1 \times 1	IMU 数据采集频率 [Hz]
Ts	1 \times 1	IMU 数据采样周期 [s]
f\_{bi}^p	nPts\_IMU \times 3	三轴加速度计所测量到的比力 [m/s^2]，即 \mathbf{f}_{b_i/t}^p
w\_{bi}^p	nPts\_IMU \times 3	三轴陀螺仪所测量到的角速度 [rad/s]，即 \boldsymbol{\omega}_{b_i/t}^p

2.4. 参考解

对于每条行驶路径，我们使用 Novatel SPAN（战术级 IRU + 差分 GPS）测量设备计算得到一条参考解。

参考解数据保存在文件 Reference.mat 中，表 4 展示了该文件的数据结构：

Data	Size	Description
t_{Ref}	nPts\_Ref \times 1	时间 [s]
nPts\_Ref	1 \times 1	参考解样本数量
llh\_{Ref}	nPts\_Ref \times 3	参考解在 e-坐标系下的位置 [m]，记为 p_{b/e} 。具体形式： llh\_{Ref}(i,:) = [\lambda \,[rad],\, \phi\,[rad],\, h\,[m]]
ned\_{Ref}	nPts\_Ref \times 3	参考解在 LTP（局部切平面）中的位置 [m]，记为 p_{b/t} 。具体形式： ned\_{Ref}(i,:) = [p_n,\, p_e,\, p_d]
v\_{ned\_Ref}	nPts\_Ref \times 3	参考解在 LTP 中的速度 [m/s]，记为 v_{b/e}^t
heading\_{Ref}	nPts\_Ref \times 1	参考解航向角 [rad]，记为 \psi
theta\_{Ref}	nPts\_Ref \times 1	参考解俯仰角 [rad]，记为 \theta
XYZ0	1 \times 3	初始点位置， p_{b/e}(0) = [x,\, y,\, z]^e

传感器测量模型回顾

根据前文的假设，传感器测量误差主要由噪声与偏置引起，因此加速度计的测量模型可写成：

\tilde{f}^p(t) = f^p(t) - b_f(t) - n_f(t)

其中

\tilde{f}^p(t) ：加速度计输出（测量值）
f^p(t) ：真实比力
b_f(t) ：加速度计偏置
n_f(t) ：加速度测量噪声

同理，陀螺仪的测量模型可写为：

\tilde{\omega}^p(t) = \omega^p(t) - b_\omega(t) - n_\omega(t)

\tilde{\omega}^p(t) ：陀螺仪输出（测量值）
\omega^p(t) ：真实角速度
b_\omega(t) ：陀螺仪偏置
n_\omega(t) ：角速度测量噪声

3. 适用于地面车辆的 2D IRS 解算（mechanisation）

我们实现了一个简化的 2D IRS 解算方法，适用于低成本惯性传感器。它仅使用了数量有限的传感器：1 个加速度计（忽略道路倾斜）和 1 个陀螺仪。

用于实现该简化 2D 解算的模板文件为 IRS_Simplified_Mechanization_2D.m。在完成理论分析之后，你需要补充完善此程序。

3.1. 前提假设

对于 IMU 测量，我们仅考虑：

沿车辆前进方向（即 x^b）的加速度计测量：这是沿 x^b 的比力（specific force），记为 f_{IMU}^b(x)。
沿竖直方向（即 z^b）的陀螺仪测量：记为 \omega_{IMU}^b(z)。

我们不考虑另外 4 个传感器提供的信息。

3.2. 在局部切平面（LTP）中简化解算的理论分析

问题 1

运行 IRS_Simplified_Mechanization_2D.m（确保第 60 行没有被注释掉）。

展示运行后得到的图像（图 1、图 2 和图 3），并对所记录的 IMU 测量结果做简要分析。

特别地，请指出测量中路径（行驶轨迹）的特征。

特征:

沿平台 x 轴的比力测量值 f_{b/i}^p（m/s²），表示 IMU 中加速度计沿 x 轴测量的比力随时间的变化。反映了车辆在行驶过程中由于加速、减速或其他动态变化引起的比力变化。图中有周期性高峰和低谷，对应于车辆在轨迹的加速段和减速段。

沿平台 z 轴的角速度测量值 \omega_{b/i}^p（rad/s），表示 IMU 中陀螺仪沿 z 轴（垂直方向）测量的角速度随时间的变化。

波形中出现多个明显的负值峰值，说明了？

问题 2

计算可以将平台坐标系 p 与车辆坐标系 b对齐的旋转矩阵 R_{p2b}。

给出 \tilde{f}_{IMU}^b 和 \tilde{\omega}_{IMU}^b（即在 b-坐标系下的比力和陀螺仪测量）与 \tilde{f}_{IMU}^p 和 \tilde{\omega}_{IMU}^p（即在 p 坐标系下的测量）之间的关系表达式。

平台坐标系 p 是 NWU（北-西-上）坐标系。车辆坐标系 b 是机体坐标系，其 x^b 轴是车辆的前进方向，z^b 轴朝向车辆底部，y^b 轴则完成右手坐标系。
根据实验内容，IMU 的 x^p 轴与车辆的 x^b 轴完全对齐，且 z^p 轴与 z^b 轴相反。因此，这两个坐标系之间存在明确的方向映射。

坐标映射推导

基于上述信息，给定平台坐标系 p 相对于车辆坐标系 b 的旋转只需绕 p 的 x 轴旋转 180°,因此有

R_{p2b}=R_x(180°)

平台坐标系 p 和车辆坐标系 b 的坐标轴映射关系为：

\begin{aligned} x^b & = x^p \\ y^b & = -y^p \\ z^b & = -z^p \end{aligned}

因此旋转矩阵 R_{p2b} 可以表示为：

R_{p2b} = \begin{bmatrix} 1 & 0 & 0 \\ 0 & -1 & 0 \\ 0 & 0 & -1 \end{bmatrix}

平台测量值与车辆测量值的关系

比力（加速度计）测量值之间的关系为：

\tilde{f}_{IMU}^b(t) = R_{p2b}\,\tilde{f}_{IMU}^p(t)

角速度（陀螺仪）测量值之间的关系为：

\tilde{\omega}_{IMU}^b(t) = R_{p2b}\,\tilde{\omega}_{IMU}^p(t)

文件验证

文件 IRS_Simplified_Mechanization_2D.m 中明确提到需要补全代码以计算 R_{p2b} 以及测量值的转换。
R_{p2b} 应在第 64 行定义，并用于将 IMU 数据从平台坐标系转换到车辆坐标系。

具体实现（Matlab代码）

根据上述推导，可以在文件 IRS_Simplified_Mechanization_2D.m 中完成如下代码：

定义旋转矩阵 R_{p2b}：

R_platform2body = [1  0   0;
                   0  -1   0;
                   0  0  -1];

转换测量值：

% 将平台坐标系中的IMU测量值转换为车辆坐标系
f_bi_b = R_platform2body * f_bi_p';  % 比力测量值
w_bi_b = R_platform2body * w_bi_p';  % 角速度测量值

问题 3

我们做出如下假设：

解算在 LTP（局部切平面）中进行。
在轨迹持续时间内忽略地球自转（即 \omega_{e/i} \approx 0）。
假设局部重力在整个过程中保持不变，等于初始时刻的数值。

请计算下列运动方程：

在 b-坐标系下表征的车辆沿轨迹方向速度 v_{AT}^b(t)
在 t-坐标系（局部切平面）下的北向位置 p_{n}^t(t)
在 t-坐标系下的东向位置 p_{e}^t(t)
航向角 \psi(t)（基于欧拉角微分而来）

以 f_{b/t}^b(x)(t) 和 \omega_{b/t}^b(z)(t) 为函数，其中，f_{b/t}^b(x)(t) 表示比力 f_{b/t}^b(t) 的 x 分量，\omega_{b/t}^b(z)(t) 表示角速度 \omega_{b/t}^b(t) 的 z 分量。

由此推导相应的机理方程（Mechanization equations）。

车辆只在水平面上运动，忽略垂直方向的运动(即俯仰角\theta \approx 0、横滚角\phi \approx 0)，只保留绕竖直轴z^b的旋转(即航向角\psi)。

因此重力只在-z^b方向，同时忽略垂直运动，使得沿x^b方向无需考虑重力的投影。由此，在车辆坐标系b中，沿前进方向x^b的净加速度可近似认为就是加速度计测量到的“比力”f_{b/t}^b(x)(t)。

车辆沿轨迹方向速度更新方程 v_{AT}^b(t)

因此，沿x^b方向的速度微分方程为

\dot{v}_{AT}^b(t) \;=\; f_{b/t}^b(x)(t)

因此，沿轨迹方向的速度通过时间积分得到：

v_{AT}^b(t) \;=\; v_{AT}^b(0) \;+\; \int_0^t f_{b/t}^b(x)(\tau)\, d\tau

航向角更新方程 \psi(t)

在2D简化中，车辆只绕z^b转动，故欧拉角只保留\psi，因此由陀螺仪测量的角速度 \omega_{b/t}^b(z)(t) 确定：

\dot{\psi}(t) = \omega_{b/t}^b(z)(t)

将其对时间积分得到航向角：

\psi(t) \;=\; \psi(0) \;+\; \int_0^t \omega_{b/t}^b(z)(\tau)\, d\tau

位置更新方程

航向角\psi(t)描述的是车辆x^b轴相对于北向x^t轴(n轴)的偏转。其从车辆坐标系b到局部坐标系t的3D旋转矩阵可表示为:

R_{b2t}(\psi) \;=\; \begin{bmatrix} \cos\psi & -\,\sin\psi & 0 \\ \sin\psi & \cos\psi & 0 \\ 0 & 0 & 1 \end{bmatrix}

由于我们讨论2D形式，因此只取前两行。

于是车辆速度在t坐标系的n-e分量为

\begin{bmatrix} v_n^t(t) \\[6pt] v_e^t(t) \end{bmatrix} \;=\; \begin{bmatrix} \cos\psi(t) & -\sin\psi(t) \\ \sin\psi(t) & \cos\psi(t) \end{bmatrix} \begin{bmatrix} v_{AT}^b(t) \\[4pt] 0 \end{bmatrix} \;=\; \begin{bmatrix} v_{AT}^b(t)\cos\psi(t) \\[4pt] v_{AT}^b(t)\sin\psi(t) \end{bmatrix}

北向速度：

v_{N}^t(t) = v_{AT}^b(t) \cdot \cos(\psi(t))

北向位置 p_n^t(t)

将其对时间积分得到北向位置：

p_n^t(t) \;=\; p_n^t(0) \;+\; \int_0^t v_{AT}^b(\tau)\,\cos\bigl[\psi(\tau)\bigr]\,d\tau

东向速度：

v_{E}^t(t) = v_{AT}^b(t) \cdot \sin(\psi(t))

东向位置 p_e^t(t)

将其对时间积分得到东向位置：

p_e^t(t) \;=\; p_e^t(0) \;+\; \int_0^t v_{AT}^b(\tau)\,\sin\bigl[\psi(\tau)\bigr]\,d\tau

        psi_ins(i) = psi_ins(i-1) + 0.5 * Ts * (HeadingRate_ins(i) + HeadingRate_ins(i-1)); %计算航向角 ψ
        vAT_ins(i) = vAT_ins(i-1) + 0.5 * Ts * (aAT_ins(i) + aAT_ins(i-1)); %计算纵向速度
        vn_ins(i) = vAT_ins(i) * cos(psi_ins(i)); %计算北向速度
        ve_ins(i) = vAT_ins(i) * sin(psi_ins(i)); %计算东向速度
        pn_ins(i) = pn_ins(i-1) + 0.5 * Ts * (vn_ins(i) + vn_ins(i-1)); %计算北向和东向位置
        pe_ins(i) = pe_ins(i-1) + 0.5 * Ts * (ve_ins(i) + ve_ins(i-1));

3.3 在 LTP 下实现简化的 IRS 机理

Question 5

假设两次连续 IMU 测量之间的时间步长为常数，记为 \Delta t。使用梯形积分原理来计算：

在时间 k 的 IRS 航向角，记为 \hat{\psi}_{IRS}(k)
在时间 k、用 b-坐标系参量化的 IRS 纵向速度，记为 \hat{v}_{AT_{IRS}}^b(k)
在时间 k、用 t-坐标系参量化的 IRS 北向位置，记为 \hat{p}_{n_{IRS}}^t(k)
在时间 k、用 t-坐标系参量化的 IRS 东向位置，记为 \hat{p}_{e_{IRS}}^t(k)

这些量由在时刻 k 和 k-1 的 IMU 测量（在 b-坐标系中参数化）

\bigl(\,\tilde{\omega}_{IMU}^b(z)(k),\, \tilde{f}_{IMU}^b(x)(k),\, \tilde{\omega}_{IMU}^b(z)(k-1),\, \tilde{f}_{IMU}^b(x)(k-1)\bigr)

以及时刻 k-1 时的估计值共同得到。

航向角 \hat{\psi}_{IRS}(k)

陀螺仪在 b-坐标系下的测量为

\omega_{IMU}^b(z)(k)\quad\text{与}\quad \omega_{IMU}^b(z)(k-1)

在连续时间中，航向角的微分满足

\dot{\psi}(t) \;=\; \omega_{b/i}^b(z)(t)

令离散时刻 t_k = k \Delta t，采用梯形积分法近似得到：

\hat{\psi}_{IRS}(k) = \hat{\psi}_{IRS}(k-1) + \frac{\Delta t}{2} \bigl[ \tilde{\omega}_{IMU}^b(z)(k-1) + \tilde{\omega}_{IMU}^b(z)(k) \bigr]

\hat{\psi}_{IRS}(k-1) 为上一时刻 k-1 的航向角估计值

纵向速度 \hat{v}_{AT_{IRS}}^b(k)

加速度计在 b-坐标系下的测量为

f_{IMU}^b(x)(k)\quad\text{与}\quad f_{IMU}^b(x)(k-1)

假设道路倾斜可忽略，车辆俯仰角较小，沿车辆前向（x^b 轴）的线加速度可直接由加速度计输出得到。令上一时刻 k-1 的纵向速度估计为 \hat{v}_{AT_{IRS}}^b(k-1)，则同样使用梯形积分法：

\hat{v}_{AT_{IRS}}^b(k) = \hat{v}_{AT_{IRS}}^b(k-1) + \frac{\Delta t}{2} \bigl[ \tilde{f}_{IMU}^b(x)(k-1) + \tilde{f}_{IMU}^b(x)(k) \bigr]

北向位置 p_{n_{IRS}}^t(k)

设上一时刻北向位置估计为 \hat{p}_{n,IRS}^t(k-1)，仍使用梯形积分对速度积分可得：

\hat{p}_{n,IRS}^t(k) \;=\; \hat{p}_{n,IRS}^t(k-1) \;+\; \frac{\Delta t}{2}\Bigl[v_{n,IRS}^t(k) \;+\; v_{n,IRS}^t(k-1)\Bigr]

我们将北向速度带入：

v_{n,IRS}^t(k) \;=\; \hat{v}_{AT,IRS}^b(k-1)\,\cos\bigl(\psi_{IRS}(k)\bigr)

因此得到

\hat{p}_{n,IRS}^t(k) \;=\; \hat{p}_{n,IRS}^t(k-1) \;+\; \frac{\Delta t}{2} \Bigl[ \hat{v}_{AT,IRS}^b(k-1)\,\cos\bigl(\hat{\psi}_{IRS}(k-1)\bigr) \;+\; \hat{v}_{AT,IRS}^b(k)\,\cos\bigl(\hat{\psi}_{IRS}(k)\bigr) \Bigr]

东向位置 p_{e_{IRS}}^t(k)

同理可得上一时刻的东向位置为 p_{e_{IRS}}^t(k-1)，则

\hat{p}_{e,IRS}^t(k) \;=\; \hat{p}_{e,IRS}^t(k-1) \;+\; \frac{\Delta t}{2}\Bigl[v_{e_{IRS}}^t(k) \;+\; v_{e_{IRS}}^t(k-1)\Bigr]

我们将东向速度公式带入:

v_{e_{IRS}}^t(k) \;=\; \hat{v}_{AT_{IRS}}^b(k)\,\sin\bigl(\psi_{IRS}(k)\bigr)

因此得到

\hat{p}_{e,IRS}^t(k) \;=\; \hat{p}_{e,IRS}^t(k-1) \;+\; \frac{\Delta t}{2} \Bigl[ \hat{v}_{AT,IRS}^b(k-1)\,\sin\bigl(\hat{\psi}_{IRS}(k-1)\bigr) \;+\; \hat{v}_{AT,IRS}^b(k)\,\sin\bigl(\hat{\psi}_{IRS}(k)\bigr) \Bigr]

请完成 Matlab 程序 IRS_Simplified_Mechanization_2D.m，其中包括：

平台坐标系到机体坐标系的旋转矩阵（第 64 行）
在机体坐标系中参数化的 IMU 测量矩阵（第 65 和 66 行）
会影响纵向加速度和航向角速率测量的初始偏置（第 71 和 76 行）
IRS 2D 机理，用于在每个时刻 i 估计：
1. 车辆航向角（第 116 行）
2. 在 LTP 中的车辆纵向速度（第 117 行）
3. 在 LTP 北轴和东轴方向上的车辆速度（第 118 和 119 行）
4. 在 LTP 北轴和东轴方向上的车辆位置（第 120 和 121 行）

注意：所有相关变量已经在第 106~112 行被初始化，假设车辆初始时刻处于静止。车辆的初始航向角由参考解（Reference solution）给出。

将您完成的代码行取消注释后即可使用。

Question 6

将第 60 行（其中含有 return 命令）注释掉，然后运行 IRS_Simplified_Mechanization_2D.m。

解释程序输出的图像，并针对该 2D IRS 简化机理所获得的结果进行讨论与评价。

IRS position

这个图代表了局部切平面中的二维位置，蓝色线是基于简化IRS计算后的轨迹，我们可以看到随着时间推移，IRS轨迹产生漂移，尤其在北向和东向上，这种漂移是由于惯性传感器误差（偏置、噪声）累积造成的。但与此同时我们可以看出IRS轨迹和参考轨迹形状基本一致，说明简化后的IRS捕捉到了运动的动态特性

这个图代表了位置误差分析，绿色线是北向位置误差，蓝色线是东向位置误差，我们可见随着时间增加，两个方向上的误差逐渐积累，和轨迹图所呈现出的漂移相符合。

这张图代表车辆在局部切平面中的速度，上图是北向速度，下图是东向速度，我们可以看出随着时间增加，两个方向速度越来越偏离参考速度（随着时间增加，速度误差增大）。

综上所述，简化 IRS 机理捕捉到了车辆的大致运动轨迹，但估计不够精确，并且误差随着时间增大

这个图代表了车辆航向角，我们可以看出简化IRS几乎和参考航向角重合，我们可以在误差图中更精确的分析，可见其误差位置虽然随着时间增大，但是数值较小，这代表了简化IRS航向角捕捉到了车辆的转向状态

4. 面向地面车辆的 GPS/IRS 松耦合

通过线性化卡尔曼滤波来融合 GPS 和 IRS 数据，旨在估计 IRS 的误差。利用这些估计值来修正 IRS 导航解，从而得到 GPS/IRS 混合的导航解。

混合（hybridization）的示例程序在 Hybridization_Simplified_Mechanization_2D.m 中给出。

我们假设 IRS 的解是由之前的 2D 简化解算所得到的。

在融合过程中，采用的是线性化卡尔曼滤波器。

令 \delta x(t) 表示线性化卡尔曼滤波器的误差状态向量。它由以下分量构成：

IRS 在 t-坐标系下的北向位置误差 \delta p_n(t)
IRS 在 t-坐标系下的东向位置误差 \delta p_e(t)
IRS 在 b-坐标系下表征的沿轨迹方向速度误差 \delta v_{AT}^b(t)
IMU 加速度计偏置（1 个状态）， b^f(t)
IRS 航向角误差 \delta \psi(t)
IMU 陀螺仪偏置（1 个状态）， b^\omega(t)

其中，对任意误差量，都有 \delta s = s - \hat{s}_{IRS} 。

该系统可用以下方程描述：

\delta \dot{x}(t) \;=\; F(t)\,\delta x(t) + u(t)

y(t) \;=\; H(t)\,\delta x(t) + w(t)

其中：

\delta x(t) 是误差状态向量
F(t) 是状态转移矩阵
u(t) 是过程噪声
y(t) 是观测量向量
H(t) 是观测矩阵
w(t) 是观测噪声

4.1 卡尔曼滤波器模型的理论研究

老师给的预备知识，随后删除

状态转移模型：

X_{k-1} = F_k X_{k-1} + U_k

观测模型：

Y_k = H_k X_k + W_R

估计状态：

\hat{X}_k = X_{k|k}

协方差：

\Sigma_{k|k} = \text{COV}(X_{k|k} - X_k)

4.1.1 状态转移模型

问题 7

参照附录 1 中的详细方法以及 IMU 测量数学模型（第 2.3 小节），推导卡尔曼滤波器在连续时间下的状态转移模型（请给出详细计算过程）。

找出状态转移矩阵 F(t) 和过程噪声 u(t) 。

假设过程噪声的各分量相互独立且无时间相关性，给出协方差矩阵 Q(t) 的表达式。

题意中给出的 6 维误差状态向量为

\delta x(t) = \begin{bmatrix} \delta p_n^t(t) \\ \delta p_e^t(t) \\ \delta v_{AT}^b(t) \\ b^f(t) \\ \delta \psi(t) \\ b^\omega(t) \end{bmatrix}

卡尔曼滤波的状态转移方程为

\dot{\delta x}(t) \;=\; F(t)\,\delta x(t)\;+\;u(t)

位置误差：

\dot{\delta p_n}(t) \;=\; \delta v_{AT}^b(t)\,\cos\bigl(\psi(t)\bigr) \;-\; v_{AT}^b(t)\,\sin\bigl(\psi(t)\bigr)\,\delta \psi(t) +n_{p_n}(t)

\dot{\delta p_e}(t) \;=\; \delta v_{AT}^b(t)\,\sin\bigl(\psi(t)\bigr) \;+\; v_{AT}^b(t)\,\cos\bigl(\psi(t)\bigr)\,\delta \psi(t)+n_{p_e}(t)

速度误差：

\dot{\delta v}_{AT}^b(t) = b_f(t) + n_f(t)

加速度计偏置：

\dot{b_f}(t) \;=\; -\frac{1}{\tau_f}\,b_f(t)\;+\;n_{b_f}(t)

航向角误差：

\dot{\delta \psi}(t) \;=\; -b_\omega(t)+ n_\omega(t)

陀螺仪偏置：

\dot{b_\omega}(t) \;=\; -\frac{1}{\tau_\omega}\,b_\omega(t)\;+\;n_{b_\omega}(t)

修订状态转移矩阵 F(t)

F(t) \;=\; \begin{bmatrix} 0 & 0 & \cos\bigl(\psi(t)\bigr) & 0 & -v_{AT}^b(t)\,\sin\bigl(\psi(t)\bigr) & 0 \\[6pt] 0 & 0 & \sin\bigl(\psi(t)\bigr) & 0 & v_{AT}^b(t)\,\cos\bigl(\psi(t)\bigr) & 0 \\[6pt] 0 & 0 & 0 & 1 & 0 & 0 \\[6pt] 0 & 0 & 0 & -\tfrac{1}{\tau_f} & 0 & 0 \\[6pt] 0 & 0 & 0 & 0 & 0 & -1 \\[6pt] 0 & 0 & 0 & 0 & 0 & -\tfrac{1}{\tau_\omega} \end{bmatrix}

过程噪声 u(t) 和协方差矩阵 Q(t)

过程噪声向量 u(t)：

u(t) \;=\; \begin{bmatrix} n_{p_n} \\ n_{p_e} \\ n_{v} \\ n_{b_f} \\ n_{\psi} \\ n_{b_\omega} \end{bmatrix}

因此:

\dot{\delta x}(t) = F(t) \delta x(t) + \underbrace{I_6}_{G} \begin{bmatrix} n_{p_n}(t) \\ n_{p_e}(t) \\ n_{v}(t) \\ n_{b_f}(t) \\ n_{\psi}(t) \\ n_{b_\omega}(t) \end{bmatrix}

过程噪声协方差矩阵 Q(t)：

假设 6 维白噪声 {n_i(t)} 都是相互独立、零均值、方差 \sigma_i^2 的高斯噪声，则

n_{p_n}(t) \sim \mathcal{N}(0, \sigma_{p_n}^2) \\ n_{p_e}(t) \sim \mathcal{N}(0, \sigma_{p_e}^2) \\ n_{v}(t) \sim \mathcal{N}(0, \sigma_{v}^2) \\ n_{b_f}(t) \sim \mathcal{N}(0, \sigma_{b_f}^2) \\ n_{\psi}(t) \sim \mathcal{N}(0, \sigma_{\psi}^2) \\ n_{b_\omega}(t) \sim \mathcal{N}(0, \sigma_{b_\omega}^2)

令:

B = \mathrm{diag}(\sigma_1, \sigma_2, \dots, \sigma_6)

那么

Q = B B^\mathrm{T} = \mathrm{diag}(\sigma_1^2, \dots, \sigma_6^2)

即:

Q(t) \;=\; \begin{bmatrix} \sigma_{p_n}^2 & 0 & 0 & 0 & 0 & 0 \\ 0 & \sigma_{p_e}^2 & 0 & 0 & 0 & 0 \\ 0 & 0 & \sigma_{v}^2 & 0 & 0 & 0 \\ 0 & 0 & 0 & \sigma_{b_f}^2 & 0 & 0 \\ 0 & 0 & 0 & 0 & \sigma_{\psi}^2 & 0 \\ 0 & 0 & 0 & 0 & 0 & \sigma_{b_\omega}^2 \end{bmatrix}

在 Hybridization_Simplified_Mechanization_2D.m 中查找状态转移模型的实现

代码中:

Bt = diag([0.75, 0.75, 0.1, 0.05, 1e-2, 1e-3]);
Qt = Bt * Bt';

请注意该模型是如何进行离散化处理的。

状态转移模型的离散化处理

代码中，状态转移模型的离散化通过以下公式完成：

F_k = I + \Delta t \cdot F_t + \frac{(\Delta t \cdot F_t)^2}{2}

Q_k = Q_t \cdot \Delta t + \frac{(F_t \cdot Q_t + Q_t \cdot F_t^T) \cdot \Delta t^2}{2}

对应代码如下：

% Discrete-time state transition model
Fk = eye(6) + dT*Ft + Ft*Ft*dT^2/2;
Qk = Qt*dT + (Ft*Qt+Qt*Ft')*dT^2/2;

离散化公式：
- F_k 是离散时间状态转移矩阵，基于连续时间矩阵 F_t 和时间步长 dT 计算。
- Q_k 是离散时间过程噪声协方差矩阵，考虑了连续时间协方差 Q_t 和离散化步长 dT。
重要特性：
- F_k 和 Q_k 的计算利用了泰勒展开（至二阶项），以保证数值稳定性和精度。

做到这里30/12

4.1.2 测量模型计算

GPS 接收机每 1 秒会提供一次局部切平面（LTP）中的北向和东向位置的 GPS 测量估计位置，并将其输入到融合滤波器（卡尔曼滤波器）中。

在离散时刻 k，假设得到的 GPS 观测量为

p_{n,GPS}^t(k) = p_n^t(k) + n_{p_n,GPS}(k)

p_{e,GPS}^t(k) = p_e^t(k) + n_{p_e,GPS}(k)

p_{n,GPS}^t(k) 与 p_{e,GPS}^t(k) 分别表示在 t-坐标系下的 GPS 北向与东向位置
n_{p_n,GPS} 与 n_{p_e,GPS} 分别表示北向和东向的 GPS 位置解误差。我们假设 n_{p_n,GPS}(k) \sim \mathcal{N}(0,\,\sigma_{p_n,GPS}) 且 n_{p_e,GPS}(k) \sim \mathcal{N}(0,\,\sigma_{p_e,GPS})。
\sigma_{p_n,GPS} = \sigma_{p_e,GPS} = 10\,\text{m}。

Question 8

计算卡尔曼滤波器的测量模型（请给出计算的细节）。

识别测量矩阵 H(k) 和测量噪声 w(k)。

若测量噪声各分量相互独立且在时间上不相关，请给出其协方差矩阵 R(k) 的表达形式。

解答:

卡尔曼滤波器的测量模型一般可写为

y(k) = H(k)\,\delta x(k) + w(k)

其中：

\delta x(k) 是我们在“误差空间”中定义的 6 维状态向量

\delta x = \begin{bmatrix} \delta p_n^t \\ \delta p_e^t \\ \delta v_{AT}^b \\ b^f \\ \delta \psi \\ b^\omega \end{bmatrix}
H(k) 是测量矩阵(或观测矩阵)。
w(k) 是测量噪声向量，满足 w(k)\sim\mathcal{N}(\mathbf{0},\,R(k))。

由于这里我们仅使用 GPS 的位置量测（北向位置 p_n^t 和东向位置 p_e^t），因此量测向量可写成

y(k) = \begin{bmatrix} p_{n,\mathrm{GPS}}^t(k) \\ p_{e,\mathrm{GPS}}^t(k) \end{bmatrix}

而与“误差状态”对应的量测量(在误差空间里)自然是

\delta p_n^t(k)\quad \delta p_e^t(k)

也就是 IRS 对位置的误差(北向、东向)。

由于

p_{n,\mathrm{GPS}}^t(k) = p_{n}^t(k) + n_{p_n,\mathrm{GPS}}(k), \quad p_{e,\mathrm{GPS}}^t(k) = p_{e}^t(k) + n_{p_e,\mathrm{GPS}}(k)

其中 n_{p_n,\mathrm{GPS}}、n_{p_e,\mathrm{GPS}} 为 GPS 在北向、东向位置解的测量噪声。卡尔曼融合是在“误差空间”内进行，也就是说滤波器里要处理的是“IRS 的位置估计误差”。

对 IRS 而言，若真实位置 p_n^t 与 IRS 估计位置 \hat{p}_{n,\mathrm{IRS}}^t 存在误差

\delta p_n^t(k) = p_n^t(k) - \hat{p}_{n,\mathrm{IRS}}^t(k)

则 GPS 的位置量测与 IRS 估计的差分(或“创新”量)可以近似写作

\underbrace{p_{n,\mathrm{GPS}}^t(k) - \hat{p}_{n,\mathrm{IRS}}^t(k)}_{\text{称作 } y_n(k)} \simeq \bigl[p_n^t(k) - \hat{p}_{n,\mathrm{IRS}}^t(k)\bigr] + n_{p_n,\mathrm{GPS}}(k) = \delta p_n^t(k) + n_{p_n,\mathrm{GPS}}(k)

类似地，对于东向位置也有

y_e(k) = p_{e,\mathrm{GPS}}^t(k) - \hat{p}_{e,\mathrm{IRS}}^t(k) \simeq \delta p_e^t(k) + n_{p_e,\mathrm{GPS}}(k)

把二者合并成向量形式，即测量向量

y(k) = \begin{bmatrix} y_n(k)\\ y_e(k) \end{bmatrix} = \begin{bmatrix} \delta p_n^t(k)\\ \delta p_e^t(k) \end{bmatrix} + \begin{bmatrix} n_{p_n,\mathrm{GPS}}(k)\\ n_{p_e,\mathrm{GPS}}(k) \end{bmatrix} = H(k)\,\delta x(k) + w(k)

测量噪声向量 w(k)

GPS 量测噪声(北向、东向位置)可以写成

w(k) = \begin{bmatrix} n_{p_n,\mathrm{GPS}}(k)\\ n_{p_e,\mathrm{GPS}}(k) \end{bmatrix}

其中

n_{p_n,\mathrm{GPS}}(k)\sim\mathcal{N}\bigl(0,\,\sigma_{p_n,\mathrm{GPS}}^2\bigr), \quad n_{p_e,\mathrm{GPS}}(k)\sim\mathcal{N}\bigl(0,\,\sigma_{p_e,\mathrm{GPS}}^2\bigr)

题目给定或默认的典型数值：

\sigma_{p_n,\mathrm{GPS}} = \sigma_{p_e,\mathrm{GPS}} = 10\,\mathrm{m}

测量矩阵 H(k)

由于观测量是 \delta p_n^t(k) 和 \delta p_e^t(k)，而我们的误差状态向量排布顺序是

\delta x = \begin{bmatrix} \delta p_n^t \\ \delta p_e^t \\ \delta v_{AT}^b \\ b^f \\ \delta \psi \\ b^\omega \end{bmatrix}

可以看出，第 1 个分量是 \delta p_n^t，第 2 个分量是 \delta p_e^t，其余分量 (\delta v_{AT}^b, b^f, \delta \psi, b^\omega) 与 GPS 的位置量测没有直接关系。

因此，测量矩阵 H(k) 只需从状态向量里取前两维即可，写为

H(k) = \begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 \end{bmatrix}

测量噪声协方差矩阵 R(k)

若测量噪声各分量相互独立且在时间上不相关，即 n_{p_n,\mathrm{GPS}} 与 n_{p_e,\mathrm{GPS}} 相互独立，且它们在时间上不相关(白噪声模型)。则测量噪声的协方差矩阵为对角阵

R(k) = \begin{bmatrix} \sigma_{p_n,\mathrm{GPS}}^2 & 0 \\ 0 & \sigma_{p_e,\mathrm{GPS}}^2 \end{bmatrix} = \mathrm{diag}\bigl(\sigma_{p_n,\mathrm{GPS}}^2,\;\sigma_{p_e,\mathrm{GPS}}^2\bigr)

在实际实现时，只要根据 GPS 质量或实验调参，把对应的标准差(这里是 10 \,\mathrm{m})写入对角，就可完成测量噪声协方差的设定。在 Matlab 里：

sigma_pnGPS = 10;  % 北向GPS噪声标准差
sigma_peGPS = 10;  % 东向GPS噪声标准差
R = diag([sigma_pnGPS^2, sigma_peGPS^2]);

4.1.3 卡尔曼滤波器的实现

Question 9

在 Hybridization_Simplified_Mechanization_2D.m 文件中，查找卡尔曼滤波器的 3 个主要实现步骤。

由于 IRS 与 GPS 的数据速率不同，请阐述在程序中是如何在卡尔曼滤波器中处理这一情况的？

当 GPS 数据只在两个时刻可用时，IRS 的修正是在这两个时刻之间如何计算的？

(1) 时间预测 (Time Update / Prediction)

卡尔曼滤波的时间预测方程如下：

X_{\text{hat}}(k \mid k-1) \;=\; F_{k}\; X_{\text{hat}}(k-1 \mid k-1)

S(k \mid k-1) \;=\; F_{k}\; S(k-1 \mid k-1)\; F_{k}^{\mathsf{T}} \;+\; Q_{k}

在代码中，对应：

% 1) Time propagation (state prediction)
X_hat = Fk * X_hat;      % X_hat = X_k|k-1
S = Fk*S*Fk' + Qk;       % S = cov(X_k|k-1)

(2) 量测更新 (Measurement Update / Correction)

当有新的 GPS 测量可用时，执行以下更新公式：

创新 (Innovation)

\text{innov} \;=\; y(k) \;-\; H\,X_{\text{hat}}(k \mid k-1)

卡尔曼增益 (Kalman Gain)

K \;=\; S(k\mid k-1)\,H^{\mathsf{T}}\;\Bigl[\,H\,S(k\mid k-1)\,H^{\mathsf{T}} + R\,\Bigr]^{-1}

状态更新 (State Update)

X_{\text{hat}}(k \mid k) \;=\; X_{\text{hat}}(k \mid k-1)\;+\; K \,\text{innov}

协方差更新 (Covariance Update)

S(k \mid k) \;=\; S(k \mid k-1) \;-\; K\,H\,S(k \mid k-1)

对应的代码片段：

innov = meas - H*X_hat;  % Innovation
V = H*S*H' + R;          % Innovation covariance
K = S*H'*inv(V);         % Kalman gain

X_hat = X_hat + K*innov; % State estimate correction
S = S - K*H*S;           % Covariance correction

(3) 输出并修正 (将误差状态用于修正 IRS 解)

更新后的误差状态向量

X_{\text{hat}} \;=\; \begin{bmatrix} \delta p_n \\ \delta p_e \\ \delta v_{AT} \\ b^f \\ \delta \psi \\ b^\omega \end{bmatrix}

会被用来修正 IRS 的导航解。例如：

p_{n,\mathrm{GPS/IRS}}(i) = p_{n,\mathrm{IRS}}(i) \;+\; X_{\text{hat}}(1)

p_{e,\mathrm{GPS/IRS}}(i) = p_{e,\mathrm{IRS}}(i) \;+\; X_{\text{hat}}(2)

\psi_{\mathrm{GPS/IRS}}(i) = \psi_{\mathrm{IRS}}(i) \;+\; X_{\text{hat}}(5)

对应的代码片段：

pn_GPS_IRS(i)  = pn_ins(i)  + X_hat(1);
pe_GPS_IRS(i)  = pe_ins(i)  + X_hat(2);
psi_GPS_IRS(i) = psi_ins(i) + X_hat(5);

由于 IRS 与 GPS 的数据速率不同，请阐述在程序中是如何在卡尔曼滤波器中处理这一情况的？

IMU (IRS) 速率：Fs = 100，意味着每秒 100 次测量。
GPS 速率：1 Hz，即每秒 1 次。

在代码里，每次 i 增加（即每个 IMU 时刻）都会执行时间预测；但只有 i 可以被 Fs 整除（即 mod(i,Fs)==0）时，才说明 “到了 GPS 时间”，才会进行量测更新。

因此，大多数时刻仅执行卡尔曼的时间预测，而在 i=100,\;200,\;300,\dots 时执行量测更新 来校正误差状态。

当 GPS 数据只在两个时刻可用时，IRS 的修正是在这两个时刻之间如何计算的？

如果 GPS 只在时刻 t_{k} 和 t_{k+1} 各出现一次量测（间隔 1 秒），那么在此 1 秒中，IMU(IRS) 以 0.01 s 的间隔不断输出数据。卡尔曼滤波器会：

(a) 在无 GPS 量测的那些时刻只做**“时间预测”**一步；
(b) 等新 GPS 到来再进行**“量测更新”**。

具体流程：

两个 GPS 时刻之间：卡尔曼滤波器只执行时间预测；误差状态和协方差随 IMU 更新，并会逐渐增大不确定度。
到达下一个 GPS 时刻：利用新的 GPS 量测，执行量测更新，修正误差状态，从而校正 IRS 的位置、航向等解算结果。

这就是松耦合 (loose coupling) 卡尔曼滤波的典型工作方式：IMU 连续解算，GPS 低频校正漂移。

在 GPS 数据的两个更新时刻之间，IRS 使用惯性测量进行推算，基于状态预测方程：

航向角：

\psi_{\text{IRS}}(t_k) = \psi_{\text{IRS}}(t_{k-1}) + \Delta t \cdot \omega_{\text{IRS}}
纵向速度：

v_{AT,\text{IRS}}(t_k) = v_{AT,\text{IRS}}(t_{k-1}) + \Delta t \cdot a_{AT,\text{IRS}}
北向和东向位置：

p_{n,\text{IRS}}(t_k) = p_{n,\text{IRS}}(t_{k-1}) + \Delta t \cdot v_{n,\text{IRS}}

p_{e,\text{IRS}}(t_k) = p_{e,\text{IRS}}(t_{k-1}) + \Delta t \cdot v_{e,\text{IRS}}

这些计算基于惯性导航机制的积分推算，因此误差会随着时间累积。当下一次 GPS 数据可用时，通过更新步骤纠正这些累积误差。

4.2 GPS/IRS 融合性能评估

在程序中，可通过设置参数 MaskingInterval（位于第 149 行）来定义一个以秒为单位的信号遮蔽时长，从而在 100 秒后模拟 GPS 信号被遮蔽的情形。

Question 10

在 MaskingInterval = 0\,\text{s} 的条件下运行对应的 M 文件，观察并解释输出的图像。

对所得的 IRS/GPS 导航解进行分析，并将其与仅使用 IRS 机理的结果进行比较。

解释你在 GPS/IRS 位置解中观察到的锯齿状形态。

问题 11

现在将 M 文件中的 MaskingInterval=60s，并运行程序。

将得到的结果与问题 10 中的结果进行比较。

5. 附录 1 – 北向位置分量方程

在问题 3 中，你需要证明北向位置分量的运动方程为：

\dot{p}_n^t(t) = v_{AT}^b(t) \cdot \cos\bigl(\psi(t)\bigr)

这意味着 p_n^t(t) 是 v_{AT}^b(t) 与 \psi(t) 的函数。让我们将其记作 g，则可写为：

\dot{p}_n^t(t) = g\bigl(v_{AT}^b(t),\, \psi(t)\bigr)

因此，对应的解算方程为：

\dot{p}_{n,IRS}^t(t) \;=\; g\Bigl(\hat{v}_{AT,IRS}^b(t),\, \hat{\psi}_{IRS}(t)\Bigr) \;=\; \hat{v}_{AT,IRS}^b(t)\,\cos\bigl(\hat{\psi}_{IRS}(t)\bigr)

在 t-坐标系下定义 IRS 北向位置误差的状态转移方程为：

\delta p_n(t) \;=\; p_n^t(t) \;-\; \dot{p}_{n,IRS}^t(t)

或者写成：

\delta p_n(t) \;=\; g\bigl(v_{AT}^b(t),\, \psi(t)\bigr) \;-\; g\Bigl(\hat{v}_{AT,IRS}^b(t),\, \hat{\psi}_{IRS}(t)\Bigr)

让我们考虑对 g\bigl(v_{AT}^b(t),\, \psi(t)\bigr) 在 IRS 解附近进行一阶泰勒展开。假设 \delta v_{AT}^b(t) 与 \delta \psi(t) 都足够小，有：

g\bigl(v_{AT}^b(t),\, \psi(t)\bigr) = g\Bigl(\hat{v}_{AT,IRS}^b(t),\, \hat{\psi}_{IRS}(t)\Bigr) \;+\; \frac{\partial g\bigl(v_{AT}^b(t),\, \psi(t)\bigr)}{\partial v_{AT}^b(t)} \Bigg|_{\substack{ v_{AT}^b(t)=\hat{v}_{AT,IRS}^b(t)\\ \psi(t)=\hat{\psi}_{IRS}(t) }} \,\delta v_{AT}^b(t) \;+\; \frac{\partial g\bigl(v_{AT}^b(t),\, \psi(t)\bigr)}{\partial \psi(t)} \Bigg|_{\substack{ v_{AT}^b(t)=\hat{v}_{AT,IRS}^b(t)\\ \psi(t)=\hat{\psi}_{IRS}(t) }} \,\delta \psi(t)

因此，

\delta p_n(t) \;=\; \frac{\partial g\bigl(v_{AT}^b(t),\, \psi(t)\bigr)}{\partial v_{AT}^b(t)} \Bigg|_{\substack{ v_{AT}^b(t)=\hat{v}_{AT,IRS}^b(t)\\ \psi(t)=\hat{\psi}_{IRS}(t) }} \,\delta v_{AT}^b(t) \;+\; \frac{\partial g\bigl(v_{AT}^b(t),\, \psi(t)\bigr)}{\partial \psi(t)} \Bigg|_{\substack{ v_{AT}^b(t)=\hat{v}_{AT,IRS}^b(t)\\ \psi(t)=\hat{\psi}_{IRS}(t) }} \,\delta \psi(t)

最后得到：

\delta p_n(t) \;=\; \cos\bigl(\hat{\psi}_{IRS}(t)\bigr)\,\delta v_{AT}^b(t) \;-\; \sin\bigl(\hat{\psi}_{IRS}(t)\bigr) \;\hat{v}_{AT,IRS}^b(t) \;\delta \psi(t)

分享

惯性导航实验：2D 地面车辆导航