使用无迹卡尔曼滤波器的姿态估计

Question

我正在尝试通过融合来自加速度计、陀螺仪和磁力计的数据来确定 R³ 中刚体的姿态。

我采用了“最佳状态估计：卡尔曼、H∞ 和非线性方法”一书中所写的 UKF 算法，并使用了一些操作来处理四元数。 目标是估计四元数的分量，旋转在传感器坐标系中的惯性坐标系（例如：N.E.D.）中进行的测量。 我选择使用陀螺仪测量作为输入，由加速度和磁力计测量组成的矢量作为输出。 这个想法是使用旋转测量来计算输出的估计，然后计算新向量。 使用 sigma 点偏离四元数获得旋转测量值，然后每个四元数（对应于 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);

看起来这不是一个好的方法。它模仿传感器的动态，但不跟随它。有什么建议吗？

提前致谢

P.S. 这种方法似乎没问题，我试过改变不确定矩阵中测量的权重并且算法有效。我还尝试实现在“The Unscented Kalman Filter for Nonlinear Estimation, Eric A. Wan and Rudolph van der Menve”中定义的（更多）一般公式，它的效果稍微好一些。

Answer 1

看来您正在使用对称 Sigma 点进行 UKF 估计，总 Sigma 点为 2n。

然而，我没有看到任何权重应用于先验估计计算和协方差计算中的外推西格玛点。每个 sigma 点的权重必须为 W(i) = 1/2n。

查看书籍：Grewal 和 Andrews 的 Kalman Filtering Fourth Edition。 UKF 算法在那里有更好的解释。