卡尔曼滤波器GPS + IMU

我知道这个问题可能已经问了上千次，但是我正在尝试将GPS + Imu（具有陀螺仪，acc和磁力计）与扩展kalman滤波器集成在一起，以在下一步进行更好的定位。我正在使用一个本地化的全局框架，主要是纬度和经度。我有点“了解”了卡尔曼方程，但我正在努力应对自己的实际状态和传感器的预测情况。

我一方面掌握了GPS的经纬度，欧拉度以欧拉度表示我的IMU（已经由我认为芯片上的某种算法融合了）的滚动，俯仰和偏航。我想我可以甩开并滚开。

而且我知道我必须为我的状态展示一个函数$$ x_t = f（x_ {t-1}，\ mu_ {t-1 }）$$和一个可预测我的传感器在该步骤中处于什么状态的函数$ t $
$$ \ mu_ {t} = h（x_t）$$

不知道这些功能应该依靠什么，我的状态应该关于gps吗？在那种情况下，由于我认为我无法从gps位置的差异中获得旋转，因此我该如何预测下一次偏航。

如果我的状态是偏航，则我需要GPS给我的某种速度，在这种情况下，kalman可以工作吗？由于我正在使用GPS的速度来预测下一个GPS位置。

长话短说，我不知道我的状态和传感器的预测应该是什么案例。

提前致谢。

编辑：我有一个没有编码器的ackerman转向移动机器人，它装有GPS和IMU（gyr acc和mag）。 imu将这些值融合到欧拉度中，GPS给出了经度和纬度。

如果您确定自己的车辆始终处于水平状态，则只能扔掉滚动并俯仰。

#1 楼

这是我最初提供的答案的完整重做。如果您感到好奇，可以查看编辑历史记录并查看之前发布的内容。

在对这个问题的评论中，OP表示他们可能能够获得机器人的油门和转向角，但可能并不准确。没关系;

OP还指出，IMU输出融合方向，其中融合方向来自加速度计，陀螺仪和磁力计。

IMU的加速度计输出可能会或可能不会通过方向进行校正。如果是这样，那就太好了。如果没有，这是一个简单的转换，只要您选择正确的表格即可。我认为通常您想要的是XYZ Tait-Bryan旋转矩阵，但绝对要与制造商核对，并绝对要通过大量的测试数据来自己检查结果。

此处的注释，我一直为自己构建快速的可视化工具，强烈建议您也自己做。假设您有一个包含所有时间索引的变量time，这是一个快速使用的Matlab代码段。如果只有采样间隔，则可以将时间作为采样间隔的累积总和。 time = cumsum(sampleIntervals);。

 width = 1;
length = 2;
height = 0.5;
shapeTop = [...
    -width/2, -length/2; ...
    -width/2, length/2; ...
    width/2, length/2; ...
    width/2, -length/2];
shapeTop(:,3) = ones(4,1)*height/2;
shapeBottom = shapeTop;
shapeBottom(:,3) = shapeBottom(:,3) - ones(4,1)*height;
shapeVertices = [shapeTop;shapeBottom];
shapeFaces = [...
    1,2,3,4,1; ...
    5,6,7,8,5; ...
    8,5,1,4,8; ...
    7,6,2,3,7; ...
    5,6,2,1,5; ...
    8,7,3,4,8];
shapeColor = [0.6 0.6 1]; % Light blue

figure(1)
shapePatch = patch(...
    'Faces', shapeFaces, ...
    'Vertices', shapeVertices, ...
    'FaceColor', shapeColor);
axis equal;
xlabel('X-Axis')
ylabel('Y-Axis')
zlabel('Z-Axis')
view([60,20]);

tic
while true
    elapsedTime = toc;
    if elapsedTime > time(end)
        break;
    end
    currentSample = find(time>=elapsedTime,1);
    rotMatrix = EulerToRotationMatrix(...
        EulerX(currentSample) , ...
        EulerY(currentSample) , ...
        EulerZ(currentSample));
    tempVertices = shapeVertices;
    tempVertices = (rotMatrix*tempVertices.').';
    set(shapePatch,'Vertices',tempVertices);
    drawnow;
    pause(0.01);
end

此代码以1的比例贯穿所有示例数据： 1播放速度，并根据您的X / Y / Z角度更新形状的方向。 EulerToRotationMatrix是您自己创建的函数，可以接受您的Euler x / y / z角度并返回适当的旋转矩阵。这是您需要处理的部分，但是我很确定您想要Tait-Bryan XYZ。

记录将IMU从一个起始姿势移动到另一个起始姿势，并采用不同的路径以相同的方向结束时的多个测试数据集。观看回放时，如果正确执行旋转矩阵操作，您会立即看到很多内容。

因此，无论如何，一旦您确定了旋转矩阵，就需要校正加速度计数据，那就只是accNew = rotMatrix*accOld;。

现在，有四种可能的方法来更新偏航信息：

使用输入（油门和转向角）输入模型。
磁强计相对于磁北的角度。
从IMU读取陀螺仪读数
需要从以前的GPS定位点到当前GPS定位点成一直线的航向。

GPS位置是有关您的实际位置的高斯分布，因此我不愿意使用上述选项4。

IMU已经将磁力仪和陀螺仪数据融合到一个偏航读数中，因此，唯一可以提高偏航估计的方法就是使用该模型。如果您无法访问输入（油门/转向角），则您将无法使用IMU提供的偏航角。

如果可以访问输入，则可以按如下所示对x / y / $ \ theta $速率进行建模：

$$
\ dot {x} = v \ cos（\ frac {\ pi} {2}-\ theta）\\
\ dot {y} = v \ sin（\ frac {\ pi} {2}-\ theta） \\
\ dot {\ theta} = \ frac {v} {L} \ tan（\ delta）\\
$$

其中$ \ theta $是航向，从+ y轴测得的正CW； $ \ delta $是转向角，是从前向车辆方向测量的正CW，$ L $是轴距，前后轴之间的距离，$ v $是车辆的速度。

车辆参数（位置/航向）如何更新的模型是非线性的，以至于我不会尝试将其置于状态空间形式。您确实确实需要实际的$ \ cos $和$ \ sin $函数。小角度近似在这里实际上不起作用。没关系！您正在使用扩展的卡尔曼滤波器，因此无需尝试线性化模型。

我想我可能会尝试将油门信号建模为一阶调速器，例如：

$$
\ dot {v} = \ frac {c \ left（\ mbox {throttle} \ right）-v} {\ tau} \\
$$

其中$ \ tau $是时间常数，而$ c $是将油门定标为速度的值。在车辆静止时开始测试，然后给出节气门的步进命令。长期稳定的速度可用于确定$ c $。然后，您可以计算出给定的特定速度“请求”（油门位置）的时间常数。

阶跃输入和系统响应之间的关系是众所周知的；最终值在一个时间常数处为63.2％，在三个时间常数处为95.0％，依此类推。我将在各种油门输入中分别进行多次测试，并对结果进行平均。

系统的完整模型为：

$$
\ dot {v} = \ frac {c \ left（\ mbox {throttle} \ right）-v} {\ tau} \\
v = v + \ dot {v} \ Delta T \\
$$
$$
\ dot {x} = v \ cos（\ frac {\ pi} {2}-\ theta）\\
\ dot {y} = v \ sin（\ frac {\ pi} {2}-\ theta）\\
\ dot {\ theta} = \ frac {v} {L} \ tan（\ delta）\\
$$
$$
x = x + \ dot {x} dT \\
\ theta = \ theta + \ dot {\ theta} dT \\
$$

卡尔曼模型。

您正在使用扩展的卡尔曼滤波器，与常规（“经典”？）卡尔曼滤波器不同，它不需要线性系统。很好，因为系统模型就在上方。您的状态是位置，速度和偏航角。

因此，您可以执行预测步骤：

预测状态估计值：

$$
\ hat {x} _ {t | t-1} = f \ left（\ hat {x} _ {t-1}，u_ {t-1} \ right）\\
$$

这里$ \ hat {x} _ {t | t-1} $是对状态向量$ x $的预测，这就是帽子$ \ hat {} $的含义，对于当前时间步，在给定上一个时间步的状态向量的情况下，这就是$ {t | t-1} $的含义。

预测误差协方差估计值：

$$
P_ {t | t-1} = F_k P_ {t-1} F_k ^ T + Q_k \\
$$

$ F_k $是Jacobian $ \ left。\ frac {\ partial f} {\ partial x} \ right | _ {\ hat {x} _ {t-1}，u_ {t-1}} $，$ P $是误差协方差（您对状态估计值的“信任”程度；较小=更多信念），$ Q_k $是过程噪声矩阵，通常是[数字]的对角矩阵。您如何测量过程噪声？关于这个主题有很多论文，您应该检查一下。尝试将0.1放在对角线上，看看它是如何工作的。通常，过程噪声是您调整滤波器的方式。同样，$ t $和$ t | t-1 $的含义与它们相同（当前和当前为过去）。

然后执行更新步骤：

这对您来说是一件棘手的事情。您有多个传感器-GPS和IMU。该怎么办？如果您有独立的传感器，答案将是更新，然后再次更新。例如，如果您仅将IMU用于速度和偏航角，而将GPS仅用于位置，则将具有以下残差/创新：

$$
\ tilde {y} _ {\ mbox {IMU}} = \ left [\ begin {matrix} {}
\ mbox {速度测量} \\
\ mbox {偏航角测量}
\ end {matrix} \ right]-\ left [\ begin {matrix} {}
0＆1＆0 \\
0＆0＆1
\ end {matrix} \ right] \ left [\ begin {matrix} {}
\ mbox {位置状态预测} \\
\ mbox {速度状态预测} \\
\ mbox {偏航角状态预测}
\ end {matrix} \ right]
$$
$$
\ tilde {y} _ {\ mbox {GPS}} = \ left [\ begin {matrix} {}
\ mbox {位置测量}
\ end {matrix} \ right]-\ left [\ begin {matrix} {}
1＆0＆0
\ end {matrix} \ right] \ left [\ begin {matrix} {}
\ mbox {位置状态预测} \\
\ mbox {速度状态预测} \\
\ mbox {偏航角状态预测}
\ end {矩阵} \ right]
$$

然后您可以继续计算残余协方差和卡尔曼增益，而不必担心，因为这些测量是独立的，并且不会彼此“覆盖”。

你不太幸运；您必须处理以下事实：您有两个冗余测量值-IMU位置和GPS位置。处理冗余的常用方法是在将传感器发送到滤波器之前对它们进行平均。对于两个相同的传感器，这可以正常工作，但是那不是您所拥有的。

更好的方法可能是通过它们的协方差来加权测量，以便更多“可信赖”的传感器获得更大的权重。

话虽如此，两者不必冲突。 IMU在技术上输出加速度。如果您只集成一次以获取速度并保持其速度不变，那么IMU和GPS突然之间就不会发生冲突，您可以将它们的测量结果分开，而不必担心冲突。这就是我要走的路线。如果您觉得不满意，可以尝试再次整合IMU输出并将其与GPS数据混合，但是我认为这不会使情况变得更好。

希望我已经把事情弄清楚了。

最后一点，您可能还会发现GPS和IMU的更新速度不同。那该怎么办？关于这一点有很多论文，但是基本上可以归结为对高频数据进行下采样或伪造低频数据。下采样可以哑（丢弃数据）或智能（过滤数据）完成，就像伪造数据可以哑（重复相同的值）或智能（以某种方式推断预计的传感器读数）一样。

$ \ begingroup $
您还应该仔细检查您是从IMU获得欧拉角，而不是从欧拉角获得。陀螺仪需要进行一次数值积分以获取角度，而加速度计需要进行两次数值积分以获取位置。
$ \ endgroup $
–卡盘
16 Dec 6'在15:36

$ \ begingroup $
@Aram-我重写了答案。希望这会有所帮助。我删除了所有评论，除了关于检查欧拉角或比率的评论之外。您没有具体答复，我想确保您看到了。
$ \ endgroup $
–卡盘
16 Dec 9'在21:06

#2 楼

id建议估算来自imu的偏航读数中的位置，速度和偏差。您将假定偏航偏置是由零均值高斯噪声驱动的一阶系统。如果您希望在操作过程中出现明显的漂移，还可以估计加速度读数中的偏差。

编程黑洞网

卡尔曼滤波器GPS + IMU

评论

#1 楼

评论

#2 楼