用于雷达实施的 UKF答案

【问题标题】：UKF for radar implementation用于雷达实施的 UKF
【发布时间】：2020-08-02 18:22:33
【问题描述】：

我正在努力实施无味卡尔曼滤波器以使用雷达跟踪物体。我的状态向量包含 [x y z vx vy vz]，我可以测量 [rho phi theta 速度]。所以一切看起来都是微不足道的，因为状态估计很简单

  x = rho * sin(theta) * cos(phi);
  y = rho * sin(theta) * sin(phi);
  z = rho * cos(theta);
  vx = v * sin(theta) * cos(phi);
  vy = v * sin(theta) * sin(phi);
  vz = v * cos(theta);

测量模型也是众所周知的：

rho = sqrt(p_x*p_x + p_y*p_y + p_z*p_z);         
phi = atan(p_y/p_x); 
theta = atan(sqrt(p_x*p_x + p_y*p_y)/p_z); 
velocity = sqrt(v_x*v_x + v_y*v_y + v_z*v_z);

我的预测基于等速模型，如下所示：

    //predicted state values
    px_p = p_x + v_x*delta_t;
    py_p = p_y + v_y*delta_t;
    pz_p = p_z + v_z*delta_t;
    vx_p = v_x + err_x*delta_t;
    vy_p = v_y + err_y*delta_t;
    vz_p = v_z + err_z*delta_t;

而且...它不起作用。它工作的唯一一种情况是沿 x 轴的恒定速度。谁能解释我做错了什么？在这种情况下 Q 矩阵应该是什么？欣赏任何提示和提示。干杯，维姬。

UPD：在 robot_localization 包中，我找到了名为 transferFunction_() 的矩阵，根据我的理解，它是过程函数（在 cmets 的参考示例中，它用于预测 sigma 点），没有噪声。它是 15 维的，实现方式如下：

double roll = state_(StateMemberRoll);
double pitch = state_(StateMemberPitch);
double yaw = state_(StateMemberYaw);

double sp = ::sin(pitch);
double cp = ::cos(pitch);
double cpi = 1.0 / cp;
double tp = sp * cpi;

double sr = ::sin(roll);
double cr = ::cos(roll);

double sy = ::sin(yaw);
double cy = ::cos(yaw);

transferFunction_(StateMemberX, StateMemberVx) = cy * cp * delta;
transferFunction_(StateMemberX, StateMemberVy) = (cy * sp * sr - sy * cr) * delta;
transferFunction_(StateMemberX, StateMemberVz) = (cy * sp * cr + sy * sr) * delta;
transferFunction_(StateMemberX, StateMemberAx) = 0.5 * transferFunction_(StateMemberX, StateMemberVx) * delta;
transferFunction_(StateMemberX, StateMemberAy) = 0.5 * transferFunction_(StateMemberX, StateMemberVy) * delta;
transferFunction_(StateMemberX, StateMemberAz) = 0.5 * transferFunction_(StateMemberX, StateMemberVz) * delta;
transferFunction_(StateMemberY, StateMemberVx) = sy * cp * delta;
transferFunction_(StateMemberY, StateMemberVy) = (sy * sp * sr + cy * cr) * delta;
transferFunction_(StateMemberY, StateMemberVz) = (sy * sp * cr - cy * sr) * delta;
transferFunction_(StateMemberY, StateMemberAx) = 0.5 * transferFunction_(StateMemberY, StateMemberVx) * delta;
transferFunction_(StateMemberY, StateMemberAy) = 0.5 * transferFunction_(StateMemberY, StateMemberVy) * delta;
transferFunction_(StateMemberY, StateMemberAz) = 0.5 * transferFunction_(StateMemberY, StateMemberVz) * delta;
transferFunction_(StateMemberZ, StateMemberVx) = -sp * delta;
transferFunction_(StateMemberZ, StateMemberVy) = cp * sr * delta;
transferFunction_(StateMemberZ, StateMemberVz) = cp * cr * delta;
transferFunction_(StateMemberZ, StateMemberAx) = 0.5 * transferFunction_(StateMemberZ, StateMemberVx) * delta;
transferFunction_(StateMemberZ, StateMemberAy) = 0.5 * transferFunction_(StateMemberZ, StateMemberVy) * delta;
transferFunction_(StateMemberZ, StateMemberAz) = 0.5 * transferFunction_(StateMemberZ, StateMemberVz) * delta;
transferFunction_(StateMemberRoll, StateMemberVroll) = delta;
transferFunction_(StateMemberRoll, StateMemberVpitch) = sr * tp * delta;
transferFunction_(StateMemberRoll, StateMemberVyaw) = cr * tp * delta;
transferFunction_(StateMemberPitch, StateMemberVpitch) = cr * delta;
transferFunction_(StateMemberPitch, StateMemberVyaw) = -sr * delta;
transferFunction_(StateMemberYaw, StateMemberVpitch) = sr * cpi * delta;
transferFunction_(StateMemberYaw, StateMemberVyaw) = cr * cpi * delta;
transferFunction_(StateMemberVx, StateMemberAx) = delta;
transferFunction_(StateMemberVy, StateMemberAy) = delta;
transferFunction_(StateMemberVz, StateMemberAz) = delta;

还有明确的processNoiseCovariance矩阵：

processNoiseCovariance_.setZero();
processNoiseCovariance_(StateMemberX, StateMemberX) = 0.05;
processNoiseCovariance_(StateMemberY, StateMemberY) = 0.05;
processNoiseCovariance_(StateMemberZ, StateMemberZ) = 0.06;
processNoiseCovariance_(StateMemberRoll, StateMemberRoll) = 0.03;
processNoiseCovariance_(StateMemberPitch, StateMemberPitch) = 0.03;
processNoiseCovariance_(StateMemberYaw, StateMemberYaw) = 0.06;
processNoiseCovariance_(StateMemberVx, StateMemberVx) = 0.025;
processNoiseCovariance_(StateMemberVy, StateMemberVy) = 0.025;
processNoiseCovariance_(StateMemberVz, StateMemberVz) = 0.04;
processNoiseCovariance_(StateMemberVroll, StateMemberVroll) = 0.01;
processNoiseCovariance_(StateMemberVpitch, StateMemberVpitch) = 0.01;
processNoiseCovariance_(StateMemberVyaw, StateMemberVyaw) = 0.02;
processNoiseCovariance_(StateMemberAx, StateMemberAx) = 0.01;
processNoiseCovariance_(StateMemberAy, StateMemberAy) = 0.01;
processNoiseCovariance_(StateMemberAz, StateMemberAz) = 0.015;

【问题讨论】：

嗨！你能更具体一点吗？什么是行不通的？你尝试了什么？你能分享更多的代码吗？状态和度量当然很有用，但我们需要更多信息来帮助您和查看更多代码。
好的，我明白你的意思了。我查看了您正在遵循的示例代码，发现它使用了激光雷达和雷达的组合。如果您尝试添加其他变量，则需要考虑检索当前状态和新状态的控制输入。基本上你的列向量会改变，需要评估不同的协方差矩阵。该示例假设每个传感器的概率模型都很好。在您的情况下，缺少的部分是处理新状态和新协方差矩阵。
怎么做？首先，我会查看this，这可能会有所帮助。同样在示例代码中，我将添加一组额外的状态来处理您尝试处理的方面。另一个可以帮助您了解如何做到这一点的有用资源是this one。这是了解 EFK 和 UKF 以及如何正确添加变量的宝贵工具。
嗨，维多利亚，好的，我看到了更新，它们看起来不错。是的，可以使用对象的 theta 和 phi 作为俯仰和偏航。例如：如果您为包含 3 个状态 (x,y,z) 的向量的雷达启动回调，则没有什么禁止我们声明相同的向量但带有 6 个状态 (x,y,z, roll, pitch, yaw) .前三个坐标将被雷达用于某些预定操作，但其余状态可以重新用作横滚-俯仰-偏航。我将在接下来的 5 分钟内向您发送一个示例，说明我是如何实现它的。一秒
好的，我创建了here 一个 Dropbox 文件夹，您可以在其中查看如何执行该特定回调。在我的情况下，我有一个传感器给我 x-y-z 并且通过声明一个 6 状态向量，我能够重新使用额外的参数作为 roll-pitch-yaw。转到回调函数void usblCb(const nav_msgs::Odometry::ConstPtr& msg) 的第 239 行，您将看到声明为 6 的 3 状态向量。

标签： ros kalman-filter

【解决方案1】：

雷达只能测量径向速度，因此您的测量模型是错误的。

【讨论】：

这就是我使用vx = v * sin(theta) * cos(phi); vy = v * sin(theta) * sin(phi); vz = v * cos(theta); 进行预测的原因，其中v 是径向速度，后来用于校正velocity = sqrt(v_x*v_x + v_y*v_y + v_z*v_z);，这也是径向速度。
你的速度是整个速度向量的长度，这不是你测量的

【解决方案2】：

长话短说，我还没有找到解决方案，所以我只是在 2 个平面上运行两个 UKF 过滤器来覆盖 3D。可视化部分在 rviz 中运行良好，但我仍在实现数据关联算法，以确保使用两个过滤器跟踪相同的目标。

【讨论】：