【滤波跟踪】基于迭代最小二乘法和卡尔曼滤波根据GNSS数据确定割草机GNSS

✅作者简介：热爱科研的Matlab仿真开发者，擅长数据处理、建模仿真、程序设计、完整代码获取、论文复现及科研仿真。

🍎 往期回顾关注个人主页：Matlab科研工作室

🍊个人信条：格物致知,完整Matlab代码获取及仿真咨询内容私信。

🔥 内容介绍

1. 问题背景与融合目标

割草机的定位挑战：

GNSS（全球导航卫星系统）
：提供绝对位置信息（经纬度），但易受遮挡（树木、建筑物）、多径效应影响，导致信号丢失或定位精度下降（米级甚至更差）。
DR（航位推算，Dead Reckoning）
：通常基于轮速编码器和 IMU（惯性测量单元）。它能提供连续的相对位置更新，但存在累积误差（漂移），时间越长误差越大。

融合目标：

取长补短
：利用 GNSS 的绝对位置信息来校准 DR 的累积误差。
提高鲁棒性
：当 GNSS 信号短暂丢失时，DR 可以维持较高精度的定位。
输出平滑、连续、高精度的定位结果
（理想情况下可达分米级或厘米级，取决于传感器精度）。

2. 数据融合核心思想

我们将 DR 作为系统的预测模型，将 GNSS 作为带有噪声的观测值。

预测步（DR）
：根据上一时刻的位置和 IMU / 轮速计的运动信息（速度、角速度），预测当前时刻的位置。
更新步（GNSS + 滤波 / 优化）
：当有 GNSS 观测值时，将预测位置与 GNSS 观测位置进行比较，利用卡尔曼滤波或最小二乘法来估计误差，并对预测位置进行修正。

3. 迭代最小二乘法 (ILS) 在融合中的应用

最小二乘法 (LS) 是一种数学优化技术，它通过最小化误差的平方和来寻找数据的最佳函数匹配。

在 GNSS/DR 融合中：

假设我们有一段时间窗口内的 DR 预测位置序列 {x_dr(t), y_dr(t)} 和 GNSS 观测位置序列 {x_gnss(t), y_gnss(t)}。

DR 的位置可以看作是真实位置 {x_true(t), y_true(t)} 加上一个随时间变化的误差 {dx(t), dy(t)}：x_dr(t) = x_true(t) + dx(t)y_dr(t) = y_true(t) + dy(t)

我们希望找到一组误差 {dx(t), dy(t)} 使得 DR 位置经过修正后，与 GNSS 观测位置尽可能接近，即使得以下目标函数最小化：J = Σ [ (x_dr(t) - dx(t) - x_gnss(t))² + (y_dr(t) - dy(t) - y_gnss(t))² ]

迭代最小二乘法 (ILS) 是当模型参数与观测值之间存在非线性关系时，通过迭代的方式求解最小二乘问题的方法。在我们这个场景中，如果 DR 的误差模型比较复杂（例如包含旋转和缩放），ILS 会非常有用。

应用场景：

批量处理
：对一段历史数据（例如 1 秒内的数据）进行一次性优化，得到这段时间内各时刻的最优估计位置。
作为初始化
：可以在卡尔曼滤波启动时，用 ILS 对初始的几帧数据进行处理，为滤波器提供一个更准确的初始状态估计。
GNSS 信号恢复后
：当 GNSS 信号从丢失状态恢复时，可以用 ILS 对信号丢失期间的 DR 轨迹进行重校正。

4. 卡尔曼滤波 (KF) 在融合中的应用

卡尔曼滤波是一种高效的递归滤波器，它能从一系列不完全包含噪声的测量中，估计动态系统的状态。它非常适合实时、在线的数据融合。

标准卡尔曼滤波的五个核心方程：

状态预测
：x̂(k|k-1) = A * x̂(k-1|k-1) + B * u(k)
- 根据上一时刻的最优估计 x̂(k-1|k-1) 和控制输入 u(k)（如 IMU 的速度和角速度），预测当前时刻的状态 x̂(k|k-1)。
误差协方差预测
：P(k|k-1) = A * P(k-1|k-1) * A^T + Q
- 预测状态的不确定性 P(k|k-1)。Q 是过程噪声协方差矩阵，反映了 DR 模型的不准确性。
卡尔曼增益计算
：K(k) = P(k|k-1) * H^T * (H * P(k|k-1) * H^T + R)^(-1)
- 计算滤波增益 K(k)，它决定了观测值 z(k)（GNSS 位置）对状态预测 x̂(k|k-1) 的修正程度。R 是观测噪声协方差矩阵，反映了 GNSS 测量的不准确性。H 是观测矩阵。
状态更新
：x̂(k|k) = x̂(k|k-1) + K(k) * (z(k) - H * x̂(k|k-1))
- 利用观测值 z(k) 和卡尔曼增益 K(k) 来修正预测状态，得到当前时刻的最优估计 x̂(k|k)。(z(k) - H * x̂(k|k-1)) 被称为innovation或残差。
误差协方差更新
：P(k|k) = (I - K(k) * H) * P(k|k-1)
- 更新最优估计的不确定性 P(k|k)。

在 GNSS/DR 融合中：

系统状态 x
：通常包含位置 [x, y]、速度 [vx, vy]，有时也包含姿态 θ 和 IMU 偏差等。
状态转移矩阵 A
：描述了状态如何从 k-1 时刻演变到 k 时刻。例如，基于匀速运动模型。
观测矩阵 H
：如果 GNSS 直接观测位置，则 H 是一个选择矩阵，例如 [1, 0, 0, 0; 0, 1, 0, 0] 用于从 [x, y, vx, vy] 中提取位置。
过程噪声 Q
：主要来自 IMU 的 gyro 和 accelerometer 噪声，以及轮速计的误差。
观测噪声 R
：主要来自 GNSS 的定位误差。可以根据 GNSS 模块的规格或实时的 HDOP/VDOP 值来动态调整。

5. 具体实现步骤（以割草机为例）

传感器数据采集与预处理：
- IMU
  ：读取加速度计和陀螺仪数据，进行积分得到速度和姿态变化。可能需要进行零偏校准、温度补偿。
- 轮速计
  ：读取左右轮的脉冲数，计算轮速和车辆的瞬时速度及转向角。
- GNSS
  ：读取经纬度、海拔、速度、时间、HDOP/VDOP 等信息。进行异常值检测和剔除（例如，当 HDOP 值过大时，认为 GNSS 数据不可靠）。
坐标转换：
- 将 GNSS 的经纬度坐标转换为局部平面坐标（如 UTM 或自定义的笛卡尔坐标系），以便与 DR 的位置在同一坐标系下进行融合。
DR 航位推算：
- 基于预处理后的 IMU 和轮速计数据，利用选定的运动学模型（如 Ackermann 模型或 differential drive 模型）计算出 DR 的预测位置 (x_dr, y_dr)。
数据同步：
- 由于不同传感器的数据输出频率和时间戳可能不同，需要将 DR 预测结果和 GNSS 观测结果对齐到同一时间戳。
融合算法选择与执行：
- 设定一个时间窗口（例如，最近的 N 个数据点）。
- 收集窗口内的 DR 预测位置和 GNSS 观测位置。
- 构建最小二乘优化问题，求解最优的位置修正量。
- 用优化结果修正窗口内的 DR 轨迹。
- 初始化 KF 的状态 x̂(0|0) 和协方差 P(0|0)。
- 在每个时间步：a. 使用 DR 数据执行预测步。b. 如果有同步好的 GNSS 数据，执行更新步，用 GNSS 数据修正 DR 预测结果。c. 输出 KF 的最优估计位置 x̂(k|k)。
- 卡尔曼滤波（推荐实时融合）
  ：
- 迭代最小二乘法（推荐批量优化或重校正）
  ：
融合结果输出与应用：
- 将融合后的平滑、高精度位置信息 (x_fused, y_fused) 输出给割草机的路径规划和控制系统，用于导航和作业。