Attitude estimation using the unscented Kalman filter

Question

I'm trying to determine the attitude of a rigid body in R³ fusing the data coming from an accelerometer, a gyroscope and a magnetometer.

I've taken the UKF algorithm as written in the book "Optimal State Estimation: Kalman, H∞, and Nonlinear Approaches" and used some manipulation in order to work with quaternions. The goal is to estimate the components of a quaternion rotating the measurements taken in an inertial frame (eg: N.E.D.) in the sensor frame. I choose to use the gyroscope measurements as input and a vector composed by the acceleration and magnetometer measurements as output. The idea is to use the rotated measurements to compute the estimate of the output and then to compute the innovation vector. The rotated measurements are obtained using the sigma points to deviate the quaternion and then each quaternion (corresponding to a sigma point) is used to rotate the vector in the N.E.D. frame in a vector in the sensor frame.

        
        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);

It looks like that this is not a good approach. It mimics the dynamics of the sensors, but it doesn't follow it. Any suggestion?

Thanks in advance

P.S. This approach seems to be okay, I've tried changing the weight of the measurement in the uncertainty matrix and the algorithm works. I've also tried to implement the (more) general formulation defined in: "The Unscented Kalman Filter for Nonlinear Estimation, Eric A. Wan and Rudolph van der Menve" and it works slightly better.

Answer 1

It seems you are using symmetric Sigma points for UKF estimation with total Sigma points being 2n.

I however don't see any weightages being applied to the extrapolated sigma points in the a priori estimate calculation and the covariance calculations. The weightages for each sigma point must be W(i) = 1/2n.

Check out the book: Kalman Filtering Fourth Edition by Grewal and Andrews. The UKF algorithm is explained better there.

Answer 2

Here is my code for Extrapolation, a priori Covariance calculation and Innovation of an UKF with 2n+1 sigma points:

    % Generate Sigma Points

    Clt = chol(PxxU4k)' ;                                                   % Lower triangular cholesky decomposition

    S4(:, 1) = xUcap4_post ;                                                % S0

    for i = 1:n

        S4(:, 1+i) = xUcap4_post + sqrt(n + ku)*Clt(:, i) ;                 % S(i)

        S4(:, n+1+i) = xUcap4_post - sqrt(n + ku)*Clt(:, i) ;               % S(i+n)

    end

    % Extrapolation

    for i = 1:2*n+1

        S4k1(:, i) = F4k*S4(:, i) + + g4k*u(k) ;

    end

    % Unscented Transform

    xUcap4_prio = sum(W4.*S4k1, 2) ;                                        % Sample mean State

    yUcap4_prio = H4*xUcap4_prio ;                                          % Sample Mean Measurements

    PxxU4k1 = Q4k ;                                                         % gets added to the end result

    PyyU4k1 = zeros(1, 1) ;

    PxyU4k1 = zeros(n, 1) ;

    for i = 1:2*n+1                                                         % a priori covariance estimated from propogated sigma pts

        sdx = (S4k1(:, i) - xUcap4_prio) ;

        PxxU4k1 = PxxU4k1 + W4(1, i)*sdx*sdx' ;

        sdy = (H4*S4k1(:, i) - yUcap4_prio) ;

        PyyU4k1 = PyyU4k1 + W4(1, i)*sdy*sdy' ;

        PxyU4k1 = PxyU4k1 + W4(1, i)*sdx*sdy' ;

    end

    % Innovation

    KU4k = PxyU4k1/(PyyU4k1 + R) ;                                          % Kalman Gain

    xUcap4_post = xUcap4_prio + KU4k*(y(k) - yUcap4_prio) ;                 % Measurement Update

    PxxU4k = PxxU4k1 - KU4k*PxyU4k1' ;                                      % Covariance Update

Refer to this video by Prof. Asada for theory behind my code: MIT Robotics Lab Video

Hope you find this useful