基于卡尔曼滤波的GPS与IMU融合定位轨迹估计

基于卡尔曼滤波的GPS与IMU融合定位轨迹估计

摘要

本次实验使用卡尔曼滤波技术将GPS和IMU数据进行融合，以实现更为准确的物体位置估计。首先，从IMU设备获取加速度和偏航角数据，结合GPS提供的速度和经纬度信息，通过卡尔曼滤波算法对物体的位置信息进行预测和更新。位置和速度的估计基于加速度和偏航角的输入，同时考虑了系统和测量噪声。通过对每个时间步的数据进行处理，最终绘制出物体的轨迹图，展示了通过数据融合获得的运动轨迹。该方法能够有效减少单一传感器误差的影响，提高位置估计的精度。

根据GPS信号绘制经纬图

1.数据处理

首先，我们观察源数据：

我们可以很清楚的看到，在GPS信号栏处，只有一部分的数据有数值，其他的数值全部都为NaN，所以，我们在最开始需要对数据进行一个筛选：

filename = 'localization_log2.csv';
data = readtable(filename);

gps_columns = {'timestamp', 'gps_lat', 'gps_lon'};
gps_data = data(:, gps_columns);

gps_valid = gps_data(~isnan(gps_data.gps_lat) & ~isnan(gps_data.gps_lon), :);

gps_lat_interp = interp1(gps_valid.timestamp, gps_valid.gps_lat, gps_data.timestamp, 'linear', 'extrap');

gps_lon_interp = interp1(gps_valid.timestamp, gps_valid.gps_lon, gps_data.timestamp, 'linear', 'extrap');

gps_data.gps_lat = gps_lat_interp;
gps_data.gps_lon = gps_lon_interp;

gps_data = gps_data(~isnan(gps_data.gps_lat) & ~isnan(gps_data.gps_lon), :);

disp(gps_data);

writetable(gps_data, 'GPS_Lat_Lon_Processed.xlsx');

首先，我们先对于有数据的部分进行一个筛选，通过条件筛选，将所有包含NaN（缺失值）的行排除，从而得到有效的GPS数据，即那些同时有有效纬度和经度的行。

对于缺失的GPS纬度和经度数据，使用线性插值法（linear）根据已知的时间戳和有效数据进行插值补全，填充缺失的gps_lat和gps_lon。

这样，我们就可以得到连续的GPS信号：

2.绘制轨迹图

通过上面的图像，我们可以很清楚的看出，经纬度型号变成了一个连续变量。接下来，我们就根据经纬度信号，来进行绘制图像：

data = readtable('GPS_Lat_Lon_Processed.xlsx');
gps_lat = data.gps_lat;
gps_lon = data.gps_lon;

figure;h = geoplot(gps_lat, gps_lon, '-o', 'LineWidth', 1.0, 'MarkerSize', 4);
geobasemap topographic;
title('GPS');
     
xlabel('Longitude');
ylabel('Latitude');
title('GPS轨迹图');
grid on;

我们使用MATLAB中的“geobasemap”函数，我们可以得到精确的轨迹图像：

接下来，我们就要对数据中的IMU信号进行轨迹预测，并将它的轨迹与GPS的准确轨迹进行拟合比较。

根据IMU信号预测绘制轨迹图

1.数据处理

首先，我们还是要对于数据进行一个处理：

从这里我们可以看见IMU的所有信号都有一部分数值为NaN，所以，这里就需要我们对于数据进行处理，消除NaN值，并通过部分手段补全这一部分数据：

1.1.GPS_speed

跟上面的GPS信号一样，由于设备的原因，GPS有关的数值只在一部分有数值显示，所以，我们在这里依旧采用的是线性拟合的方式进行数据的补全：

filename = 'localization_log2.csv';
data = readtable(filename);

gps_columns = {'timestamp', 'gps_speed'};
gps_data = data(:, gps_columns);

gps_valid = gps_data(~isnan(gps_data.gps_speed), :);

gps_speed_interp = interp1(gps_valid.timestamp, gps_valid.gps_speed, gps_data.timestamp, 'linear', 'extrap');

gps_data.gps_speed = gps_speed_interp;

disp(gps_data);

writetable(gps_data, 'GPS_Speed_Interpolated.xlsx');

通过去除NaN值以及线性拟合之后，我们可以得到一段连续的速度信号，这对于我们之后进行轨迹的预测很有帮助：

1.2.IMU信号的数据处理

对于IMU型号的处理，我们可以很明显的从源数据中看出，IMU的数值只有一小部分是NaN，所以，在这里我们采用直接去掉NaN的方式来进行数据处理：

filename = 'localization_log2.csv';
data = readtable(filename);

imu_columns = {'timestamp', 'imu_ax', 'imu_ay', 'imu_az', ...
               'imu_roll', 'imu_pitch', 'imu_yaw', ...
               'imu_roll_rate', 'imu_pitch_rate', 'imu_yaw_rate'};

imu_data = data(:, imu_columns);

imu_data_cleaned = rmmissing(imu_data, 'DataVariables', imu_columns(2:end));

disp(imu_data_cleaned);

writetable(imu_data_cleaned, 'IMU_Cleaned.csv');

处理完成之后，我们可以得到完整的IMU数据：

但这里位置，对于这题的数据处理部分就结束了。

２.卡尔曼滤波预测轨迹

2.1 卡尔曼滤波介绍

卡尔曼滤波（Kalman Filter）是一种基于线性系统的递归滤波算法，广泛应用于估计系统状态、去除噪声干扰和预测系统行为。

2.1.1卡尔曼滤波的基本原理：

卡尔曼滤波是通过结合两部分信息来估计系统状态：

1. 预测（预测阶段）：基于系统的动态模型和先前的状态估计，预测当前时刻的系统状态。

2. 更新（更新阶段）：通过实际测量数据更新预测值，校正系统状态估计。

   

初始化：
    x = x0        // 初始状态估计
    P = P0        // 初始误差协方差矩阵
    Q = 过程噪声协方差矩阵
    R = 测量噪声协方差矩阵
    H = 测量矩阵
    F = 状态转移矩阵

对于每一个时刻 k：
    1. 预测步骤：
        x_pred = F * x        // 预测当前状态
        P_pred = F * P * F' + Q    // 预测误差协方差矩阵
    
    2. 测量更新步骤：
        y = z_k - H * x_pred    // 测量残差（创新）
        S = H * P_pred * H' + R    // 创新协方差
        K = P_pred * H' / S    // 计算卡尔曼增益
    
    3. 更新步骤：
        x = x_pred + K * y    // 更新状态估计
        P = (I - K * H) * P_pred    // 更新误差协方差矩阵
    
输出最终状态估计 x 和误差协方差矩阵 P

通过上面的步骤，我们就通过“t”时刻的抓状态，预测出“t+1”时刻的状态，从而达到预测的目的。

2.2 IMU轨迹预测

在本次实验中，卡尔曼滤波被用于融合IMU和GPS速度数据，通过预测和更新步骤来估计物体的位置。卡尔曼滤波的核心是递归地估计系统的状态（在这个例子中是物体的位置和速度），同时考虑过程噪声和测量噪声。以下是卡尔曼滤波的具体使用细节：

2.2.1. 卡尔曼滤波的状态空间模型

• 状态向量（x）：在这段代码中，状态向量 x 包含了物体的当前位置 (px, py) 和速度 (vx, vy)，即：
  $$x=\left[\begin{array}{c}px\\ py\\ vx\\ vy\end{array}\right]$$

  其中，px 和 py 是位置，vx 和 vy 是速度。初始状态估计是 [0; 0; 0; 0]，即初始位置为 (0, 0)，速度为 (0, 0)。

• 预测模型： 卡尔曼滤波的状态转移模型描述了物体从一个时刻到另一个时刻的运动。假设物体的运动由加速度、速度和偏航角决定，这里使用了以下的状态转移矩阵 F 来描述系统的动态行为：
  $$F=\left[\begin{array}{cccc}1& 0& dt& 0\\ 0& 1& 0& dt\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$$
  其中，dt 是时间步长（由时间戳计算得到）。这个矩阵表示：

  • 位置在下一时刻等于当前时刻位置加上速度乘以时间步长（线性关系）。
  • 速度保持不变（理想化的假设，实际中应考虑加速度影响，但这里通过加速度对位移的影响来进行估算）。

• 预测步骤： x_pred = F * x; 通过状态转移矩阵 F 对当前状态 x 进行预测，得到下一时刻的状态估计（包括位置和速度）。

  P_pred = F * P * F' + Q; 预测误差协方差矩阵 P_pred，其中 P 是当前状态的误差协方差矩阵，Q 是过程噪声协方差矩阵，表示过程的不确定性。

2.2.2. 测量更新

• 测量矩阵（H）： 卡尔曼滤波的测量更新步骤将状态估计映射到实际测量空间。在这里，H 用来将状态向量 x（位置和速度）映射到测量空间：
  $$H=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\end{array}\right]$$
  这表示我们只关注位置的测量（px 和 py），而速度信息并没有直接参与测量更新。

• 创新（测量残差）： 测量残差是实际测量值与预测值之间的差异。z 是根据预测的位移计算得到的“测量值”，即：
  $$z=\left[\begin{array}{c}px+{\delta }_x\\ py+{\delta }_y\end{array}\right]$$
  其中 delta_x 和 delta_y 是通过IMU加速度和速度计算得到的位移。然后计算创新 y：
  $$y=z-H\ast x_{p}red;$$
  这表示测量值和预测值之间的差异。

• 创新协方差和卡尔曼增益： 创新协方差 S 和卡尔曼增益 K 决定了预测与测量之间的权衡。卡尔曼增益通过以下公式计算：
  $$
  S = H * P_pred * H’ + R

  $$

  $$K=P_{p}red\ast H^{\prime }/S$$

  其中，R 是测量噪声协方差矩阵，表示测量数据中的不确定性。

• 更新步骤： 使用卡尔曼增益 K 更新状态估计：
  $$x=x_{p}red+K\ast y;$$
  这表示通过加权平均，结合预测值和测量值来更新物体的状态估计。

  更新误差协方差矩阵：
  $$P=(I-K\ast H)\ast P_{p}red;$$
  这样，更新后的误差协方差矩阵 P 会反映当前状态估计的不确定性。

2.2.3. 轨迹绘制

每次卡尔曼滤波更新后，新的位置信息（px 和 py）被保存到历史记录 px_history 和 py_history 中。这些历史数据会用于绘制物体的轨迹图。