具有激光扫描+已知图的扩展卡尔曼滤波器

我目前正在为一个学校项目工作，该项目需要为带有激光扫描仪的点机器人实现扩展的卡尔曼滤波器。机器人可以以0度转弯半径旋转并向前行驶。所有运动都是分段线性的（驱动，旋转，驱动）。

我们使用的模拟器不支持加速，所有运动都是瞬时的。

我们还需要定位一个已知的地图（png图像）。我们可以在图像中进行光线跟踪以模拟激光扫描。

我和我的搭档对我们需要使用的运动和传感器模型几乎不感到困惑。

到目前为止，我们将状态建模为向量。$(x,y,\theta)$

我们使用更新公式如下

void kalman::predict(const nav_msgs::Odometry msg){
    this->X[0] += linear * dt * cos( X[2] ); //x
    this->X[1] += linear * dt * sin( X[2] ); //y
    this->X[2] += angular * dt; //theta

    this->F(0,2) = -linear * dt * sin( X[2] ); //t+1 ?
    this->F(1,2) =  linear * dt * cos( X[2] ); //t+1 ?

    P = F * P * F.t() + Q;

    this->linear = msg.twist.twist.linear.x;
    this->angular = msg.twist.twist.angular.z;
    return;
}

我们以为一切正常，直到我们注意到我们忘记初始化P并且它为零，这意味着没有更正发生。显然，由于我们尚未将噪声引入系统，因此传播非常准确。

对于运动模型，我们对F使用以下矩阵：

$F = \begin{bmatrix}1 & 0 & -v*\Delta t*sin(\theta) \\ 0 & 1 & v*\Delta t*cos(\theta) \\ 0 & 0 & 1 \end{bmatrix}$

作为我们的更新公式的雅可比行列式。它是否正确？

对于传感器模型，我们通过对机器人，和位置以及地图中的光线跟踪进行有限差分来逼近Jacobian（H）。我们与电讯局长交谈，电讯局长说这会奏效，但我仍不确定会否。我们的教授不在了，所以我们不能遗憾地问他。我们每个校正步骤使用3次激光测量，因此H为3x3。ÿ $x$$y$$\theta$

关于如何初始化P的另一个问题。我们尝试了1,10,100，并且当地图仅为50x50时，它们都将机器人放置在地图的外部（-90，-70）。

我们项目的代码可以在这里找到：https : //github.com/en4bz/kalman/blob/master/src/kalman.cpp

任何意见是极大的赞赏。

编辑：

在这一点上，我已经使滤波器在基本运动噪声的作用下稳定下来，但没有实际运动。一旦机器人开始移动，过滤器就会迅速发散并离开地图。

• 使用EKF进行基于激光扫描和已知地图的定位是一个坏主意，因为EKF的主要假设之一是无效的，因此无法使用：测量模型不可区分。

我建议研究一下蒙特卡洛本地化（粒子过滤器）。这不仅可以解决您的测量模型问题，还可以进行全局定位（地图上的初始姿态完全未知）。

编辑：对不起，我忘了提到另一个要点：

• 如果要应用EKF，则测量模型以及运动模型都可能仅包含高斯噪声。这意味着您需要以记下测量模型。这是一个严重的限制，因为它不允许像Probabilistic Robotics中的beam_range_finder_model那样稍微复杂的模型，该模型还考虑了机器人前面的动态对象，无效（最大）测量值和完全随机的读数。您会被困在零件的投射光线上并添加高斯噪声。h （x t$z_t = h(x_t) + N(0, Q_t)$$h(x_t)$

首先，您没有提到所使用的本地化类型。它具有已知或未知的对应关系吗？

现在要在Matlab中获取Jacobian，可以执行以下操作

>> syms x y a Vtran Vrotat t
>> f1 = x + Vtran*t*cos(a);
>> f2 = y + Vtran*t*sin(a);
>> f3 = a + Vrotat*t;
>> input  = [x y a];
>> output = [f1 f2 f3];
>> F = jacobian(output, input)

结果

F =
[ 1, 0, -Vtran*t*sin(a)]
[ 0, 1,  Vtran*t*cos(a)]
[ 0, 0,               1]

扩展卡尔曼滤波器要求系统是线性的，噪声是高斯的。这里的系统意味着运动和观察模型必须是线性的。激光传感器提供了从机器人框架朝向界标的范围和角度，因此测量模型不是线性的。关于P，如果您假设它很大，则您的EKF估计器在开始时就很差，并且它依赖于测量值，因为先前的状态向量不可用。但是，一旦您的机器人开始移动和感应，EKF就会变得更好，因为它使用运动和测量模型来估计机器人的姿势。如果您的机器人没有感应到任何地标，则不确定性将急剧增加。另外，您需要注意角度。在C ++中，cos and sin，角度应为弧度。您的代码中没有关于此问题的任何内容。如果在以弧度计算时假设角度的度数为度，则可能会得到奇怪的结果，因为在以弧度计算时将度数作为噪声是巨大的。