ros - 用于雷达实施的 UKF
问题描述
我正在努力实施无味卡尔曼滤波器以使用雷达跟踪物体。我的状态向量包含 [xyz 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_() 的矩阵,在我的理解中,它是过程函数(在评论中的参考示例中,它用于预测 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;
解决方案
长话短说,我还没有找到解决方案,所以我只是在两架飞机上运行两个 UKF 过滤器来覆盖 3D。视觉部分在 rviz 中运行良好,但我仍在实施数据关联算法,以确保我使用两个过滤器跟踪相同的目标。
推荐阅读
- html - Bootstrap nav 交互式 tablist 左右
- java - 如何解决:“出现意外错误(类型=未找到,状态=404)”
- r - 为 Table() 或 Tibble() 创建向量而不是列,为 Vector by Row Word Occurrence Table r
- intellij-idea - Pycharm 工具窗口的“移动到”选项有什么作用?
- php - Seo url 和多语言路由codeigniter
- python - 匹配特定单词之前的最后一个名词
- python - CNN模型比组合的CNN-SVM模型具有更好的准确性
- javascript - 使用 ACF 字段值填充 JS 变量
- ios - 自动登录,AppDelegate 到 SceneDelegate,UIApplication.shared.delegate 一样!AppDelegate 被替换为?
- postgresql - 在表格上生成日期直方图