EKF1.m

Go to the documentation of this file.

% ============================================================================ %
%> @brief EKF1 is an implementation of a state estimator for JUMP sensors
%>
%> This class is an implementation of an Extended Kalman Filter (EKF) based
%> on [this derivation](@ref derivation9Ekf6D).
%>
%> @warning this implementation is changed from the original derivation. if you
%> want to use the original implementation, use the version of the commit
%> [4fac0d658d](https://github.zhaw.ch/isc/MT_moss_IMU_sensor_fusion/tree/4fac0d658de33c09c19e74a554307eb90db0d781)
%> the changes are:
%> - the accelerometer bias is not estimated
%> - the accelerometer sensitivity is not estimated
%> - the measurement update of the accelerometer is changed
%>
%> @code
%> % example code
%> ekf = EKF1();
%>
%> % set measurements
%> ekf.z_acc = acc;
%> ekf.z_gyr = gyr;
%>
%> % set zero velocity flag
%> ekf.zv = zv;
%>
%> % run the filter
%> [smoothed, filtered] = ekf.run();
%> @endcode
%>
%> @warning all values regarding angles must be given in radians
%>
%> @copyright see the file @ref LICENSE in the root directory of the repository
% ============================================================================ %
 
classdef EKF1 < handle
    
    properties (Constant)
        n = 13; % number of states
        m = 13; % number of measurements
    end
    
    properties (Access = public)
        
        %> sampling time (default 1/200 s)
        dt (1,1) double = 1/200;
        
        %> zero velocity flag (1 x N)
        zv (1,:) logical;
        
        %> @name Measurement Vector
        %> @{
        %> attributes regarding the measurement vector
        
        %> number of observations
        N (1,1) double;
        %> accelerometer measurements (3 x N) [m/s^2]
        z_acc (3,:) double;
        
        %> velocity measurements (3 x N)
        %> @note these are usually all set to zero as the velocity is applied
        %> as a pseudo measurement during ZUPTs
        z_vel (3,:) double;
        
        %> gyroscope measurements (3 x N) [rad/s]
        z_gyr (3,:) double;
        
        %> position pseudo measurements (3 x N) [m]
        z_pos (3,:) double;
        
        %> yaw pseudo measurements (1 x N) [rad]
        z_yaw (1,:) double;
        
        %> measurement vector (m x N)
        z (:,:) double;
        
        %> @} % Measurement Vector
        
        %> @name State Vector
        %> @{
        %> attributes regarding the estimated state vectors
        
        %> state vector, forward, a priori (n x N)
        x_fm (:,:) double;
        
        %> state vector, forward, a posteriori (n x N)
        x_fp (:,:) double;
        
        %> state vector, backward, a posteriori (n x N)
        x_bp (:,:) double;
        
        %> @} % State Vector
        
        %> @name Initial State Vector
        %> @{
        %> attributes regarding the initial state vector
        
        %> initial irientation quaternion
        q_0 (4,1) double = [1,0,0,0]';
        
        %> initial velocity
        v_0 (3,1) double = [0,0,0]';
        
        %> initial position
        p_0 (3,1) double = [0,0,0]';
        
        %> initial gyroscope bias
        b_w_0 (3,1) double = [0,0,0]';
        
        %> initial state vector (n x 1)
        x_0 (:,1) double;
        
        %> @} % Initial State Vector
        
        %> @name State Covariance
        %> @{
        %> attributes regarding the state covariance matrix
        
        %> state covariance matrix, forward, a priori (n x n x N)
        P_fm (:,:,:);
        
        %> state covariance matrix, forward, a posteriori (n x n x N)
        P_fp (:,:,:);
        
        %> state covariance matrix, backward, a posteriori (n x n x N)
        P_bp (:,:,:);
        
        %> @} % State Covariance
        
        %> @name Initial State Covariance
        %> @{
        %> attributes regarding the initial state covariance matrix
        
        %> initial orientation standard deviation (default 5e-4)
        %> @remark based on experimental tuning
        P_0_q (1,1) double = 5e-4;
        
        %> initial velocity standard deviation (default 1e-7 m/s)
        %> @remark based on experimental tuning
        P_0_v (1,1) double = 1e-7;
        
        %> initial position standard deviation (default 1e-7 m)
        %> @remark based on experimental tuning
        P_0_p (1,1) double = 1e-7;
        
        %> initial gyroscope bias standard deviation (default 5e-3 rad/s)
        %> @remark based on experimental tuning
        P_0_b_w (1,1) double = 5e-3;
        
        %> initial state covariance matrix (n x n)
        %> @remark based on experimental tuning
        P_0 (:,:) double;
        
        %> @} % Initial State Covariance
        
        %> @name Measurement Noise
        %> @{
        %> attributes regarding the measurement noise
        
        %> accelerometer variance, x- and y-axis (default 0.045 m/s^2)
        %> @remark based on allan variance analysis
        var_acc_xy (1,1) double = 0.045;
        
        %> accelerometer variance, z-axis (default 0.07 m/s^2)
        %> @remark based on allan variance analysis
        var_acc_z (1,1) double = 0.07;
        
        %> variance of velocity 'measurements' during ZUPTs (default 1e-6)
        %> @remark based on experimental tuning
        var_vel_ZUPT (1,1) double = 1e-6;
        
        %> gyroscope variance (default 1.22e-3 rad/s)
        %> @remark based on allan variance analysis
        var_gyr (1,1) double = 1.22e-3; % 0.07 * pi / 180;
        
        %> variance of x-position 'measurements' during ZUPTs (default 1e-6 m)
        %> @remark based on experimental tuning
        var_pos_x = 1e-6;
        
        %> variance of y-position 'measurements' during ZUPTs (default 1e-6 m)
        %> @remark based on experimental tuning
        var_pos_y = 1e-6;
        
        %> variance of z-position 'measurements' during ZUPTs (default 1e-6 m)
        %> @remark based on experimental tuning
        var_pos_z = 1e-6;
        
        %> variance of yaw pseudo-measurements (default TODO:)
        %> @remark based on experimental tuning
        var_yaw_pseudo = 0.0001;
        
        %> measurement noise covariance matrix (m x m)
        R (:,:) double;
        
        %> @} % Measurement Noise
        
        %> @name Process Noise
        %> @{
        %> attributes regarding the process noise
        %> @note the process noise covariance matrix is calculated for each
        %> iteration based on the current state vector (see @ref Q_cov)
        
        %> angular velocity standard deviation (default 876e-6 rad/s)
        %> @remark based on allan variance analysis
        %> @note as the prediction is based on the measurement, the measurement
        %> noise applies. but since the prediction is based on the preprocessed
        %> angular velocity which is the mean angular velocity between two
        %> samples, the standard deviation is divided by sqrt(2) to get the
        %> standard deviation of the mean angular velocity.
        sigma_w_p (1,1) double = sqrt(0.071) * pi / 180 / sqrt(2);
        
        %> acceleration variance (default 0.045 m/s^2)
        %> @remark based on experimental tuning
        var_a (1,1) double = 0.045;
        
        %> gyroscope bias variance (default 1e-9 rad/s)
        %> @remark based on experimental tuning
        %> @note the value from the allan variance analysis seems to be too
        %> high. the value is set to a lower value based on experimental tuning.
        var_b_w (1,1) double = 1e-9;
        
        %> @} % Process Noise
        
    end
    
    methods (Access = public)
        
        % ==================================================================== %
        %> @brief Constructor of the EKF1 class
        %>
        %> constructs an instance of the EKF1 class with the default values
        % ==================================================================== %
        function obj = EKF1()
            % nothing to do here
        end
        
        % ==================================================================== %
        %> @brief filters and smooths the given data
        %>
        %> this function creates all needed vectors and matrices and also checks
        %> the input data. it then filters the data and smoothes the data
        %>
        %> @param options options for the filter
        %>
        %> @retval smoothed smoothed state vector
        %> @retval filtered filtered state vector
        %>
        %> this function creates all needed vectors and matrices and also checks
        %> the input data. it then filters the data and smoothes the data
        % ==================================================================== %
        function [smoothed, filtered] = run(obj, options)
            
            arguments
                obj
                options.plot (1,1) logical = false
            end
            
            % =============== %
            % measurements
            % =============== %
            % check measurements length
            obj.N = max(size(obj.z_acc));
            assert(size(obj.z_vel, 2) == obj.N, "The number of velocity measurements must be equal to the number of accelerometer measurements. You provided %d velocity measurements and %d accelerometer measurements.", size(obj.z_vel, 2), obj.N);
            assert(size(obj.z_gyr, 2) == obj.N, "The number of gyroscope measurements must be equal to the number of accelerometer measurements. You provided %d gyroscope measurements and %d accelerometer measurements.", size(obj.z_gyr, 2), obj.N);
            assert(size(obj.z_pos, 2) == obj.N, "The number of position measurements must be equal to the number of accelerometer measurements. You provided %d position measurements and %d accelerometer measurements.", size(obj.z_pos, 2), obj.N);
            assert(size(obj.z_yaw, 2) == obj.N, "The number of yaw measurements must be equal to the number of accelerometer measurements. You provided %d yaw measurements and %d accelerometer measurements.", size(obj.z_yaw, 2), obj.N);
            
            % check zero velocity flag
            assert(numel(obj.zv) == obj.N, "The number of zero velocity flags must be equal to the number of accelerometer measurements. You provided %d zero velocity flags and %d accelerometer measurements.", numel(obj.zv), obj.N);
            
            % create measurement vector
            obj.z = [obj.z_acc; obj.z_vel; obj.z_gyr; obj.z_pos; obj.z_yaw];
            
            % =============== %
            % state vectors
            % =============== %
            % create state vectors
            obj.x_fm = nan(obj.n, obj.N);
            obj.x_fp = nan(obj.n, obj.N);
            obj.x_bp = nan(obj.n, obj.N);
            
            % create initial state vector
            obj.x_0 = [obj.q_0; obj.v_0; obj.p_0; obj.b_w_0];
            
            % =============== %
            % state covariance matrices
            % =============== %
            % create state covariance matrices
            obj.P_fm = nan(obj.n, obj.n, obj.N);
            obj.P_fp = nan(obj.n, obj.n, obj.N);
            obj.P_bp = nan(obj.n, obj.n, obj.N);
            
            % create initial state covariance matrix
            obj.P_0 = diag([obj.P_0_q * ones(1,4), obj.P_0_v * ones(1,3), obj.P_0_p * ones(1,3), obj.P_0_b_w * ones(1,3)]) .^ 2;
            
            % =============== %
            % measurement noise
            % =============== %
            % create measurement noise covariance matrix
            obj.R = diag([obj.var_acc_xy * ones(1,2), obj.var_acc_z, obj.var_vel_ZUPT * ones(1,3), obj.var_gyr * ones(1,3), obj.var_pos_x, obj.var_pos_y, obj.var_pos_z, obj.var_yaw_pseudo]);
            assert(all(size(obj.R) == [obj.m, obj.m]), "The measurement noise covariance matrix must be of size %d x %d. You provided a matrix of size %d x %d.", obj.m, obj.m, size(obj.R, 1), size(obj.R, 2));
            
            % =============== %
            % process noise
            % =============== %
            % the process noise covariance matrix is calculated in the Q_cov function
            
            % =============== %
            % forward pass
            % =============== %
            obj.forward();
            
            % =============== %
            % backward pass
            % =============== %
            obj.backward();
            
            % =============== %
            % output
            % =============== %
            smoothed = obj.x_bp;
            filtered = obj.x_fp;
            
            % =============== %
            % display results
            % =============== %
            if options.plot
                obj.displayResults(filtered, smoothed, obj.zv);
            end
            
        end
        
        % ==================================================================== %
        %> @brief exports the tuning parameters of the filter to a JSON
        %> file
        %>
        %> this function exports the tuning parameters of the filter to a JSON
        %> file
        %>
        %> @param obj instance of the EKF1 class (implicit)
        %> @param filename name of the JSON file (full or relative path, default
        %> 'tuning.json')
        %>
        %> @retval status 1 if the file was written successfully, 0 otherwise
        %>
        %> @note The following parameters are exported:
        %> - initial covariance
        %> - measurement noise
        %> - process noise
        %>
        % ==================================================================== %
        function status = exportTuning(obj, filename)
            
            arguments
                obj
                filename (1,1) string = "tuning.json"
            end
            
            % create struct
            tuning = struct();
            
            % initial covariance
            tuning.P_0_q = obj.P_0_q;
            tuning.P_0_v = obj.P_0_v;
            tuning.P_0_p = obj.P_0_p;
            tuning.P_0_b_w = obj.P_0_b_w;
            
            % measurement noise
            tuning.var_acc_xy = obj.var_acc_xy;
            tuning.var_acc_z = obj.var_acc_z;
            tuning.var_vel_ZUPT = obj.var_vel_ZUPT;
            tuning.var_gyr = obj.var_gyr;
            tuning.var_pos_x = obj.var_pos_x;
            tuning.var_pos_y = obj.var_pos_y;
            tuning.var_pos_z = obj.var_pos_z;
            tuning.var_yaw_pseudo = obj.var_yaw_pseudo;
            
            % process noise
            tuning.sigma_w_p = obj.sigma_w_p;
            tuning.var_a = obj.var_a;
            tuning.var_b_w = obj.var_b_w;
            
            % write to file
            try
                writestruct(tuning, filename, FileType="json", PrettyPrint=true);
                status = 1;
            catch
                status = 0;
            end
            
        end
        
        % ==================================================================== %
        %> @brief imports the tuning parameters of the filter from a JSON
        %> file
        %>
        %> this function imports the tuning parameters of the filter from a JSON
        %> file
        %>
        %> @param obj instance of the EKF1 class (implicit)
        %> @param filename name of the JSON file (full or relative path)
        %>
        %> @retval status 1 if the file was read successfully, 0 otherwise
        %>
        %> @note the function does only import the parameters that are present
        %> in the JSON file. if a parameter is not present, the existing value
        %> is kept. therefore, **no check** is performed if all parameters are
        %> present.
        % ==================================================================== %
        function status = importTuning(obj, filename)
            
            arguments
                obj
                filename (1,1) string {mustBeFile}
            end
            
            % read from file
            try
                tuning = readstruct(filename, FileType="json");
                status = 1;
            catch
                status = 0;
                return;
            end
            
            % get all the fields in the struct and the filter
            fields = fieldnames(tuning);
            field_used = false(1, numel(fields));
            props_filter = string(properties(obj));
            
            % loop through all fields from the struct
            for i = 1:numel(fields)
                field = fields{i};
                
                % check if the field is a property of the filter, if not, skip
                if ismember(field, props_filter)
                    obj.(field) = tuning.(field);
                    field_used(i) = true;
                else
                    warning("The field %s from the imported tuning file is not used.", field);
                end
                
            end
            
            % check if all fields from the imported struct are used
            if ~all(field_used)
                warning("The following fields from the imported tuning file are not used: %s", strjoin(fields(~field_used), ", "));
            end
            
        end
        
    end
    
    methods (Access = private)
        
        % ==================================================================== %
        %> @brief forward pass of the EKF1 filter
        %>
        %> this function performs the forward pass of the EKF1 filter
        %>
        %> @param obj instance of the EKF1 class
        %>
        % ==================================================================== %
        function forward(obj)
            
            % make local copies of often used variables
            loc_x_fm = obj.x_fm;
            loc_x_fp = obj.x_fp;
            loc_P_fm = obj.P_fm;
            loc_P_fp = obj.P_fp;
            loc_z = obj.z;
            loc_zv = obj.zv;
            
            bad_iterations = false(1,obj.N);
            warning("off");
            for k = 1:obj.N
                if k == 1
                    % initial prediction
                    [loc_x_fm(:, k), w] = obj.f_fun(obj.x_0, loc_z(:, k), loc_z(:, k), loc_zv(k));
                    F = obj.F_jac(w);
                    loc_P_fm(:, :, k) = F * obj.P_0 * F' + obj.Q_cov(obj.x_0);
                else
                    % prediction
                    [loc_x_fm(:, k), w]= obj.f_fun(loc_x_fp(:, k-1), loc_z(:, k-1), loc_z(:, k), loc_zv(k));
                    F = obj.F_jac(w);
                    loc_P_fm(:, :, k) = F * loc_P_fp(:, :, k-1) * F' + obj.Q_cov(loc_x_fp(:, k-1));
                end
                
                % normalize quaternion
                loc_x_fm(1:4, k) = loc_x_fm(1:4, k) ./ norm(loc_x_fm(1:4, k), 2);
                
                h = obj.h_fun(loc_x_fm(:, k));
                H = obj.H_jac(loc_x_fm(:, k));
                
                % handle measurements
                if ~loc_zv(k)
                    % there are no updates during movement
                    % TODO: the computing intensive part can be skipped completely instead of setting the H-Matrix to zero
                    H(:) = 0;
                else
                    % handle measurements
                    
                    % check if the x position is unavailable
                    if isnan(loc_z(10, k))
                        % set the corresponding row in the H matrix to zero
                        H(10, :) = 0;
                    end
                    
                    % check if the y position is unavailable
                    if isnan(loc_z(11, k))
                        % set the corresponding row in the H matrix to zero
                        H(11, :) = 0;
                    end
                    
                    % check if the z position is unavailable
                    if isnan(loc_z(12, k))
                        % set the corresponding row in the H matrix to zero
                        H(12, :) = 0;
                    end
                    
                    % check if the yaw is unavailable
                    if isnan(loc_z(13, k))
                        % set the corresponding row in the H matrix to zero
                        H(13, :) = 0;
                    end
                    
                end
                
                % fill missing values with zeros to prevent NaN values in the
                % kalman gain. these zeros are not used in the update step
                % since the corresponding rows in the H matrix are set to zero
                this_z = fillmissing(loc_z(:, k), 'constant', 0);
                
                % kalman gain
                K = loc_P_fm(:, :, k) * H' * pinv(H * loc_P_fm(:, :, k) * H' + obj.R);
                
                % update
                loc_x_fp(:, k) = loc_x_fm(:, k) + K * (this_z - h);
                loc_P_fp(:, :, k) = (eye(obj.n) - K * H) * loc_P_fm(:, :, k) * (eye(obj.n) - K * H)' + K * obj.R * K';
                
                % normalize quaternion
                loc_x_fp(1:4, k) = loc_x_fp(1:4, k) ./ norm(loc_x_fp(1:4, k), 2);
                
                % check if a warning occured
                [~, warnID] = lastwarn;
                if strcmp(warnID, 'MATLAB:nearlySingularMatrix') || strcmp(warnID, 'MATLAB:singularMatrix')
                    bad_iterations(k) = true;
                    lastwarn("",""); % reset warning
                end
                
                % check for NaN values
                if any(isnan(loc_x_fp(:,k)), "all") || any(isnan(loc_P_fp(:,:,k)), "all")
                    error("NaN detected in forward pass at iteration %d.", k);
                end
            end
            warning("on");
            
            if any(bad_iterations)
                warning("A total of %d bad iterations in the forward pass were detected.", sum(bad_iterations));
            end
            
            % update object properties
            obj.x_fm = loc_x_fm;
            obj.x_fp = loc_x_fp;
            obj.P_fm = loc_P_fm;
            obj.P_fp = loc_P_fp;
            
        end
        
        % ==================================================================== %
        %> @brief backward pass of the EKF1 filter
        %>
        %> this function performs the backward pass of the EKF1 filter
        %>
        %> @param obj instance of the EKF1 class
        %>
        % ==================================================================== %
        function backward(obj)
            
            % make local copies of often used variables
            loc_x_fm = obj.x_fm;
            loc_x_fp = obj.x_fp;
            loc_x_bp = obj.x_bp;
            loc_P_fm = obj.P_fm;
            loc_P_fp = obj.P_fp;
            loc_P_bp = obj.P_bp;
            
            for k = obj.N-1:-1:1
                
                if k == obj.N-1
                    % initialization
                    loc_x_bp(:, obj.N) = loc_x_fp(:, obj.N);
                    loc_P_bp(:, :, obj.N) = loc_P_fp(:, :, obj.N);
                end
                
                F = obj.F_jac(loc_x_fp(:, k));
                
                I = pinv(loc_P_fm(:, :, k+1));
                K = loc_P_fp(:, :, k) * F' * I;
                loc_x_bp(:,k) = loc_x_fp(:,k) + K * (loc_x_bp(:,k+1) - loc_x_fm(:,k+1));
                loc_P_bp(:,:,k) = loc_P_fp(:,:,k) - K * (loc_P_fm(:,:,k+1) - loc_P_bp(:,:,k+1)) * K';
                
                if any(isnan(loc_x_bp(:,k)), "all") || any(isnan(loc_P_bp(:,:,k)), "all")
                    error("NaN detected in backward pass at iteration %d.", k);
                end
                
            end
            
            % update object properties
            obj.x_bp = loc_x_bp;
            obj.P_bp = loc_P_bp;
            
        end
        
        % ==================================================================== %
        %> @brief calculates the state transition function for the EKF1 filter
        %>
        %> this function calculates the state transition, also called the state
        %> prediction/extrapolation.
        %>
        %> @param x_this state vector at time k
        %> @param z_this measurement vector at time k
        %> @param z_next measurement vector at time k+1
        %> @param zv zero velocity flag at time k
        %>
        %> @retval x_next state vector at time k+1
        %> @retval w preprocessed angular velocity
        % ==================================================================== %
        function [x_next, w] = f_fun(obj, x_this, z_this, z_next, zv)
            
            arguments
                obj
                x_this (:,1) double {mustBeNumeric}
                z_this (:,1) double {mustBeNumeric}
                z_next (:,1) double {mustBeNumeric}
                zv (1,1) logical
            end
            
            % new state vector
            x_next = nan(size(x_this));
            
            % new quaternion
            epsilon = 1e-12;
            q_this = x_this(1:4);
            gyro_this = z_this(7:9);
            gyro_next = z_next(7:9);
            gyro_bias = x_this(11:13);
            w = obj.getPreprocessedGyro(gyro_this, gyro_next, gyro_bias);
            w_norm = norm(w,2);
            w_matrix = [0, -w(1) -w(2) -w(3); ...
                w(1) 0 w(3) -w(2); ...
                w(2) -w(3) 0 w(1); ...
                w(3) w(2) -w(1) 0];
            
            q_next = (cos(w_norm * obj.dt / 2) * eye(4) + 1/(w_norm + epsilon) * sin(w_norm * obj.dt / 2) * w_matrix) * q_this;
            x_next(1:4) = q_next;
            
            % new acceleration
            acc_this = z_this(1:3);
            acc_next = z_next(1:3);
            acc = obj.getPreprocessedAcc(acc_this, acc_next, q_this, q_next, zv);
            
            % new velocity
            x_next(5:7) = x_this(5:7) + acc * obj.dt;
            
            % new position
            x_next(8:10) = x_this(8:10) + x_this(5:7) * obj.dt + 0.5 * acc * obj.dt^2;
            
            % new gyro bias
            x_next(11:13) = x_this(11:13);
            
        end
        
        
        % ==================================================================== %
        %> @brief calculates the jacobian of the state transition function
        %>
        %> this function calculates the jacobian of the state transition
        %> function.
        %>
        %> @param w preprocessed angular velocity at time k
        %>
        %> @retval F jacobian of the state transition function at time k
        % ==================================================================== %
        function F = F_jac(obj, w)
            
            arguments
                obj
                w (:,1) double {mustBeNumeric}
            end
            
            % F1 ( quaternion and angular velocity )
            epsilon = 1e-12;
            w_norm = norm(w,2);
            alpha = cos(w_norm * obj.dt / 2) * w_norm;
            beta = sin(w_norm * obj.dt / 2);
            
            F_1_1 = 1/(w_norm + epsilon) * [alpha, -w(1) * beta, -w(2) * beta, -w(3) * beta; ...
                w(1) * beta, alpha, w(3) * beta, -w(2) * beta; ...
                w(2) * beta, -w(3) * beta, alpha, w(1) * beta; ...
                w(3) * beta, w(2) * beta, -w(1) * beta, alpha];
            
            F_1 = [F_1_1, zeros(4,9)];
            
            % F2 (position, velocity, acceleration)
            
            F_2_1 = eye(6);
            F_2_1(4:6,1:3) = obj.dt * eye(3);
            
            F_2 = [zeros(6,4), F_2_1, zeros(6,3)];
            
            % F3 (gyro bias)
            F_3 = zeros(3,13);
            F_3(:,11:13) = eye(3);
            
            F = [F_1; F_2; F_3];
            
        end
        
        
        % ==================================================================== %
        %> @brief calculates the process noise covariance matrix
        %>
        %> this function calculates the process noise covariance matrix
        %>
        %> @param x state vector at time k
        %>
        %> @retval Q process noise covariance matrix
        % ==================================================================== %
        function Q = Q_cov(obj, x)
            
            arguments
                obj
                x (:,1) double {mustBeNumeric}
            end
            
            % quaternion
            q0 = x(1); q1 = x(2); q2 = x(3); q3 = x(4);
            Q_q = (obj.dt * obj.sigma_w_p / 2)^2 * [...
                1 - q0^2, -q0 * q1, -q0 * q2, -q0 * q3; ...
                -q0 * q1, 1 - q1^2, -q1 * q2, -q1 * q3; ...
                -q0 * q2, -q1 * q2, 1 - q2^2, -q2 * q3; ...
                -q0 * q3, -q1 * q3, -q2 * q3, 1 - q3^2];
            
            % acceleration, velocity and position
            Q_a = zeros(6);
            Q_a(1:3, 1:3) = obj.dt^2 * eye(3);
            Q_a(4:6, 4:6) = obj.dt^4 / 4 * eye(3);
            Q_a(4:6, 1:3) = obj.dt * eye(3);
            Q_a(4:6, 1:3) = obj.dt^3 / 2 * eye(3);
            Q_a = (triu(Q_a.',1) + tril(Q_a)) * obj.var_a;
            
            % biases
            Q_b_w = eye(3) * obj.var_b_w;
            
            % combine
            Q = blkdiag(Q_q, Q_a, Q_b_w);
            
            % check size
            assert(all(size(Q) == [obj.n, obj.n]), "The process noise covariance matrix must be of size %d x %d. You provided a matrix of size %d x %d.", obj.n, obj.n, size(Q, 1), size(Q, 2));
            
        end
        
    end
    
    methods (Static, Access = private)
        
        % ==================================================================== %
        %> @brief calculates the jacobian of the measurement function from a
        %> given state vector x
        %>
        %> @param x state vector at time k
        %>
        %> @retval H jacobian of the measurement function at time k
        % ==================================================================== %
        function H = H_jac(x)
            
            arguments
                x (:,1) double {mustBeNumeric}
            end
            
            g = 9.81;
            
            % H1 (acceleration)
            q0 = x(1); q1 = x(2); q2 = x(3); q3 = x(4);
            
            H_1_1 = (2 * g * [ q2, -q3,  q0, -q1; ...
                -q1, -q0, -q3, -q2; ...
                -q0,  q1,  q2, -q3]);
            
            H_1 = [H_1_1, zeros(3,9)];
            
            % H2 (ZUPT)
            H_2 = [zeros(3,4), eye(3), zeros(3,6); ...
                zeros(3,10), eye(3)];
            
            % H3 (position pseudo measurement)
            H_3 = [zeros(3,7), eye(3), zeros(3,3)];
            
            % H4 (yaw pseudo measurement)
            H_4_fac = 1/(2*q1*q3 - 2*q0*q2 + 1) + 1/(2*q0*q2 - 2*q1*q3 + 1);
            H_4_1 = H_4_fac * ...
                [2 * q3   * q1^2 - 2 * q0 * q2 * q1 - q3, ...
                -2 * q1^2 * q2   - 2 * q0 * q1 * q3 - 2 * q2^3 - 2 * q2 * q3^2 + q2, ...
                -2 * q1   * q3^2 + 2 * q0 * q2 * q3 + q1, ...
                -2 * q0   * q2^2 + 2 * q1 * q3 * q2 + q0 ];
            H_4 = [H_4_1, zeros(1,9)];
            
            H = [H_1; H_2; H_3; H_4];
            
        end
        
        % ==================================================================== %
        %> @brief calculates the measurement function from a given state vector
        %>
        %> @param x state vector at time k
        %>
        %> @retval h measurement vector at time k
        % ==================================================================== %
        function h=h_fun(x)
            
            arguments
                x (:,1) double {mustBeNumeric}
            end
            
            g = [0;0;-9.81]; % gravity vector in world frame
            
            % acceleration
            RotMat = quaternion(x(1:4)').rotmat("frame");
            
            h1 = RotMat * g; % acceleration in global frame
            
            % ZUPT
            v_g = x(5:7); % velocity in global frame
            b_w = x(11:13); % gyro biases
            h2 = [v_g; b_w];
            
            % position pseudo measurement
            h3 = x(8:10); % position in global frame
            
            % yaw pseudo measurement
            q0 = x(1); q1 = x(2); q2 = x(3); q3 = x(4);
            h4 = atan2(2 * q1 * q2 + 2 * q0 * q3, q1^2 + q0^2 - q3^2 -q2^2);
            
            h = [h1; h2; h3; h4];
            
        end
        
        % ==================================================================== %
        %> @brief calculates the preprocessed acceleration measurement
        %>
        %> this function calculates the preprocessed acceleration measurement as
        %> explained in the derivation. The acceleration measurements are
        %> preprocessed to get the bias free mean acceleration without gravity
        %> between two samples.
        %>
        %> @param acc_this acceleration measurement at time k
        %> @param acc_next acceleration measurement at time k+1
        %> @param q_this orientation quaternion at time k
        %> @param q_next orientation quaternion at time k+1
        %> @param zv zero velocity flag at time k
        %>
        %> @retval acc preprocessed acceleration
        % ==================================================================== %
        function acc = getPreprocessedAcc(acc_this, acc_next, q_this, q_next, zv)
            
            arguments
                acc_this (3,1) double {mustBeNumeric}
                acc_next (3,1) double {mustBeNumeric}
                q_this (4,1) double {mustBeNumeric}
                q_next (4,1) double {mustBeNumeric}
                zv (1,1) logical
            end
            
            % if zero velocity is detected, the acceleration measurements are set to zero
            if zv
                acc = [0;0;0];
            else
                % rotate acceleration measurements to global frame
                acc_this = quaternion(q_this').rotmat("frame")' * (acc_this);
                acc_next = quaternion(q_next').rotmat("frame")' * (acc_next);
                
                % calculate mean acceleration
                acc = (acc_this + acc_next) / 2;
                
                % remove gravity
                acc = acc - [0;0;-9.81];
                
            end
            
        end
        
        % ==================================================================== %
        %> @brief calculates the preprocessed gyro measurement
        %>
        %> this function calculates the preprocessed gyro measurement as
        %> explained in the derivation. The gyro measurements are preprocessed
        %> to get the bias free mean angular velocity between two samples.
        %>
        %> @param gyro_this gyro measurement at time k
        %> @param gyro_next gyro measurement at time k+1
        %> @param bias bias of the gyroscope at time k
        %> @param zv zero velocity flag at time k (not used at the moment)
        %>
        %> @retval w preprocessed angular velocity
        %>
        %> @note since a zero angular velocity leads to zeros in the F matrix
        %> which leads to zero entries in the covariance matrix, the
        %> differentiation cases are disabled. in some later implementations
        %> (e.g. UKF or particle filter approach) this could be enabled again.
        % ==================================================================== %
        function w = getPreprocessedGyro(gyro_this, gyro_next, bias) %, zv)
            
            arguments
                gyro_this (3,1) double {mustBeNumeric}
                gyro_next (3,1) double {mustBeNumeric}
                bias (3,1) double {mustBeNumeric}
                %zv (1,1) logical
            end
            
            % if zero velocity is detected, the gyro measurements are set to zero
            %if zv
            %    w = [0;0;0];
            %else
            w = (gyro_this + gyro_next) / 2 - bias;
            %end
            
        end
        
        % ==================================================================== %
        %> @brief displays the results of the EKF1 filter
        %>
        %> this function displays the results of the EKF1 filter
        %>
        %> @param x_fp filtered state vector
        %> @param x_bp smoothed state vector
        %> @param zv zero velocity flag
        %> @param options
        %> @param options.monitor monitor to display the results
        % ==================================================================== %
        function displayResults(x_fp, x_bp, zv, options)
            
            arguments
                x_fp (13,:)
                x_bp (13,:)
                zv (1,:)
                options.monitor = 1
            end
            
            % filter results
            ori_filt = quaternion(x_fp(1:4,:)').rotvecd;
            pos_filt = x_fp(8:10,:)';
            
            % smoothing results
            ori_smoo = quaternion(x_bp(1:4,:)').rotvecd;
            pos_smoo = x_bp(8:10,:)';
            
            disps = get(0, 'MonitorPositions');
            f = figure;
            f.Position = disps(options.monitor,:);
            f.WindowState = 'maximized';
            
            % x plot
            pl(1) = subplot(3,3,1);
            plot(pos_filt(:,1), 'DisplayName', 'Filter');
            hold on;
            plot(pos_smoo(:,1), 'DisplayName', 'Smoother', 'LineWidth',2);
            hold off;
            grid on;
            xlabel("Sample");
            ylabel("x [m]");
            
            % y plot
            pl(2) = subplot(3,3,2);
            plot(pos_filt(:,2), 'DisplayName', 'Filter');
            hold on;
            plot(pos_smoo(:,2), 'DisplayName', 'Smoother', 'LineWidth',2);
            hold off;
            grid on;
            xlabel("Sample");
            ylabel("y");
            
            % y,z plot
            pl(3) = subplot(3,3,3);
            plot(pos_filt(:,3), 'DisplayName', 'Filter');
            hold on;
            plot(pos_smoo(:,3), 'DisplayName', 'Smoother', 'LineWidth',2);
            hold off;
            grid on;
            xlabel("Sample");
            ylabel("z [m]");
            
            % x,y plot (over two subplots)
            pl(4) = subplot(3,3,[4,5]);
            
            % filter path (thin line)
            x = pos_filt(:,1)';
            y = pos_filt(:,2)';
            z = pos_filt(:,3)';
            col = 1:width(x);
            surface([x;x], [y;y], [z;z], [col;col], "LineWidth",1, "EdgeColor","interp");
            colormap turbo;
            hold on;
            
            % smooth path (fat line)
            x = pos_smoo(:,1)';
            y = pos_smoo(:,2)';
            z = pos_smoo(:,3)';
            col = 1:width(x);
            surface([x;x], [y;y], [z;z], [col;col], "LineWidth",3, "EdgeColor","interp");
            colormap turbo;
            grid on;
            xlabel("x position [m]")
            ylabel("y position [m]")
            axis equal;
            
            % bias gyro plot
            pl(5) = subplot(3,3,6);
            yyaxis left;
            plot(x_fp(11:13,:)', 'DisplayName', 'Filter');
            ylabel("Bias [rad/s]");
            yyaxis right;
            plot(x_bp(11:13,:)', 'DisplayName', 'Smoother', 'LineWidth',2);
            ylabel("Bias [rad/s]");
            grid on;
            xlabel("Sample");
            title("Bias Gyroscope");
            
            % orientation plot
            pl(7) = subplot(3,3,7);
            plot(ori_filt(:,1), 'DisplayName', 'Roll (filtered)');
            hold on;
            plot(ori_filt(:,2), 'DisplayName', 'Pitch (filtered)');
            plot(ori_smoo(:,1), 'LineWidth',2, 'DisplayName', 'Roll (smoothed)');
            plot(ori_smoo(:,2), 'LineWidth',2, 'DisplayName', 'Pitch (smoothed)');
            plot(ori_smoo(:,3), 'LineWidth',2, 'DisplayName', 'Yaw (smoothed)');
            hold off;
            grid on;
            xlabel("Sample");
            ylabel("Angle [°]");
            title("Orientation");
            legend("Location", "best");
            
            % velocity plot
            pl(8) = subplot(3,3,8);
            plot(x_fp(5:7,:)', 'DisplayName', 'Filter');
            hold on;
            plot(x_bp(5:7,:)', 'DisplayName', 'Smoother', 'LineWidth',2);
            hold off;
            grid on;
            xlabel("Sample");
            ylabel("Velocity [m/s]");
            title("Velocity");
            
            % zero velocity detection plot
            pl(9) = subplot(3,3,9);
            plot(zv, '.-', 'DisplayName', 'Zero Velocity Detection', 'MarkerSize', 10);
            ylim([-0.1, 1.1]);
            grid on;
            xlabel("Sample");
            ylabel("Zero Velocity Detection");
            title("Zero Velocity Detection");
            
            % link axes
            linkaxes(pl([1:3,6:9]), 'x');
            
        end
        
    end
    
end