Kalman滤波速度数据与位置数据高度相似，求解问题原因

核心问题：运动学积分逻辑错误 #

你当前的速度和位置计算是单帧独立计算，完全不符合IMU积分的基本逻辑，这是两者形态高度相似的直接原因。

当前错误代码：

af = [Accxf, Accyf, Acczf];

vf = af .* dt;
rf = .5 * af .* (dt^2);

这段代码仅适用于初速度为0、且仅存在单帧匀加速运动的极端场景，而实际IMU的速度和位置需要基于前一时刻的状态累积积分。

修正后的正确积分逻辑 #

速度和位置的计算必须依赖前一时刻的状态值，正确的实现应该是：

% 初始化（第一帧）
vf_prev = [0, 0, 0]; % 若无初始速度，设为零向量
rf_prev = [0, 0, 0]; % 若无初始位置，设为零向量

% 后续帧循环计算
af = [Accxf, Accyf, Acczf];
vf = vf_prev + af .* dt; % 速度 = 前一帧速度 + 当前加速度*时间间隔
rf = rf_prev + vf_prev .* dt + 0.5 * af .* (dt^2); % 位置 = 前一帧位置 + 前一帧速度*dt + 0.5*加速度*dt²

% 更新前一时刻状态，用于下一帧计算
vf_prev = vf;
rf_prev = rf;

如果需要用原始数据初始化，也要按照上述累积逻辑计算初始的速度和位置，而非单帧计算。

额外排查方向 #

如果修正积分逻辑后仍有异常，可从以下方向排查：

• Kalman滤波器状态变量设计：确认状态向量是否包含速度、位置和加速度（或姿态相关量）。如果状态仅包含加速度，滤波过程无法建立速度与位置的关联，积分结果仍会异常。
• IMU数据预处理：手机加速度计输出包含重力加速度分量，必须通过角速度融合姿态（如欧拉角、四元数），将重力从af中剥离，否则积分出的速度和位置会持续漂移，形态也会异常。
• 采样间隔dt的准确性：确认dt=0.1秒是否与实际数据的采样率严格匹配，若采样间隔不稳定，积分误差会被快速放大。
• Kalman噪声参数：检查过程噪声和观测噪声矩阵的设置，若噪声参数不合理，滤波后的加速度可能过度平滑，导致速度和位置的变化趋势趋同。

内容的提问来源于stack exchange，提问作者banjaxed W.