基于无迹卡尔曼滤波（UKF）的刚体姿态估计问题咨询

我正在尝试融合加速度计、陀螺仪和磁力计数据，确定R³空间中刚体的姿态。采用《最优状态估计：卡尔曼、H∞和非线性方法》中的UKF算法并适配四元数，目标是估计将惯性系（如N.E.D.）测量值转换到传感器系的四元数分量。选择陀螺仪测量值作为输入，加速度计与磁力计测量值组成的向量作为输出——通过sigma点对应的四元数将N.E.D.系向量旋转至传感器系，得到旋转后的测量值来计算输出估计与新息向量。

相关代码如下：

x_hat_k_i = x_k_priori + x_tilde_priori; %% Previous state + sigma points
               
q_out = quaternion(x_hat_k_i');   % q_k|k

acc_body = rotatepoint(q_out, [0 0 1]*g); %accel

mag_body = rotatepoint(q_out, href); %magnetometer

y_hat_k_i = [acc_body, ...
             mag_body ]' ; % "sigma-outputs"
             
%% Estimated output:
             
y_hat_k = sum(y_hat_k_i, 2)/(2*nx);
             
%% Output - Input/Output cov:
             
for i = 1:2*nx
    
    x_hat_k_i(:, i) = x_hat_k_i(:, i)/norm(x_hat_k_i(:, i));
    
    Cov_uscita(:, :, i) = (y_hat_k_i(:,i) - y_hat_k)*(y_hat_k_i(:,i) - y_hat_k)';
    
    Cov_xy(:, :, i) = (x_hat_k_i(:,i) - x_k_priori)*(y_hat_k_i(:,i) - y_hat_k)';

end

P_y = sum(Cov_uscita, 3)/(2*nx) + V_out;

P_xy = sum(Cov_xy, 3)/(2*nx);

目前该方法效果不佳，虽模拟了传感器动态但未贴合实际。补充信息：调整测量不确定性矩阵的权重后算法可正常运行；实现Eric A. Wan与Rudolph van der Menve所著《The Unscented Kalman Filter for Nonlinear Estimation》中的通用公式后，效果略有提升。

具体改进建议 #

• 修正四元数sigma点生成逻辑：四元数是单位向量，不能直接对分量做加法生成sigma点后再归一化，这会引入非线性误差。正确做法是基于先验四元数做流形扰动——比如将小扰动向量转为轴角四元数，再与先验四元数相乘得到sigma点，生成后立即归一化，而非先分量相加再处理。
• 严格实现标准UKF权重公式：放弃当前均分权重1/(2*nx)，严格按照Wan论文中的公式计算均值权重与协方差权重，尤其是λ参数的选择，它直接决定sigma点的分布范围，对估计精度影响极大。
• 完善状态转移模型：陀螺仪输入对应的状态转移应遵循四元数微分方程：dq/dt = 0.5 * q ⊗ ω（ω为陀螺仪角速度测量值，⊗为四元数乘法），不能仅用先验状态加扰动的简单方式，否则状态预测会脱离刚体运动规律。
• 预处理传感器原始数据：加速度计需区分重力与线性加速度（静态时才等于重力向量），磁力计必须完成硬铁/软铁校准，否则测量值的固有误差会直接干扰UKF的估计收敛性。
• 自适应调整噪声协矩阵：测量噪声矩阵V_out不应固定，可根据传感器状态动态调整——比如加速度计检测到动态运动时，增大其噪声权重；磁力计受环境干扰时，提高对应的噪声系数。状态协方差矩阵P的初始值也要匹配初始姿态的不确定性。
• 全程保持四元数单位约束：除了sigma点归一化，每次状态更新后都要对估计的四元数做归一化处理，避免协方差矩阵出现奇异性，保证估计的合理性。

内容的提问来源于stack exchange，提问作者Antonio Vscm