MyVqf.m

Go to the documentation of this file.

% =========================================================================== %
%> @brief MyVqf is an interface to the VQF python package.
%>
%> This class is an interface to the VQF python package. It is used to create
%> a VQF object that can be used to perform VQF operations. The VQF package
%> is a python package that provides a VQF algorithm implementation. This
%> algorithm is used to estimate the orientation of a sensor in 3D space. The
%> VQF algorithm is based on the work of the following paper:
%> https://doi.org/10.1016/j.inffus.2022.10.014
%>
%> @code
%> % Example usage:
%> % create example data with the JumpSensor class
%> js = JumpSensor();
%> data = js.generate(zeros(1000,3), zeros(1000,3), quaternion.ones(1000,1));
%>
%> % create VQF object
%> vqf = MyVqf();
%>
%> % filter the data
%> result = vqf.filter(data.tt.acc, data.tt.gyr, data.tt.mag);
%> @endcode
%>
%> @note The implementation currently always uses the root python environment.
%>
%> @copyright see the file @ref LICENSE in the root directory of the repository
% =========================================================================== %
 
classdef MyVqf
    
    properties (Access=public)
        %> python environment
        pe
 
        %> VQF python package
        vqf
 
        %> VQF object with forward filter configuration
        vqfForward
 
        %> numpy package
        np
        
        %> @name Sampling times
        %> @{
        %> sampling times for the sensors in [s]
        gyrTs = 1/200;
        accTs = 1/200;
        magTs = 1/200;
        %> @}
        
        %> @name VQF parameters
        %> @{
        %> default values according to the VQF documentation
        tauAcc                       = 3; % [s]
        tauMag                       = 9; % [s]
        motionBiasEstEnabled         = true;
        restBiasEstEnabled           = true;
        magDistRejectionEnabled      = true; % (default: true)
        biasSigmaInit                = 0.5; % [°/s]
        biasForgettingTime           = 100; % [s]
        biasClip                     = 15; % [°/s] (default is 2.0 °/s, but the used MPU has higher ratings)
        biasSigmaMotion              = 0.1; % [°/s] (dafault is 0.1 °/s)
        biasVerticalForgettingFactor = 0.0001;
        biasSigmaRest                = 0.03; % [°/s] (default is 0.03 °/s)
        restMinT                     = 1.5; % [s] (default is 1.5 s)
        restFilterTau                = 0.5; % [s]
        restThGyr                    = 2; % [°/s] (default is 2 °/s)
        restThAcc                    = 0.5; % [m/s^2] (default is 0.5 m/s^2)
        magCurrentTau                = 0.05; % [s]
        magRefTau                    = 20; % [s]
        magNormTh                    = 0.2; % (default is 0.1 = 10%)
        magDipTh                     = 20; % [°] (dfeault is 10 °)
        magNewTime                   = 20; % [s]
        magNewFirstTime              = 5; % [s]
        magNewMinGyr                 = 10; % [°/s] (default is 20 °/s)
        magMinUndisturbedTime        = 0.5; % [s]
        magMaxRejectionTime          = 60; % [s]
        magRejectionFactor           = 2;
        %> @}
        
    end
    
    methods (Access=public)
 
        % ====================================================================== %
        %> @brief Constructor of the MyVqf class.
        %>
        %> @param None
        %>
        %> @retval obj -Instance of the MyVqf class.
        %>
        %> @note The root python environment is used and the packages 'vqf' and
        %> 'numpy' must be installed in the python environment.
        % ====================================================================== %
        function obj = MyVqf()
            % create python environment
            obj.pe = pyenv("ExecutionMode","OutOfProcess");
        end
        
        % ====================================================================== %
        %> @brief Destructor of the MyVqf class.
        %>
        %> @param obj -Instance of the MyVqf class.
        %>
        %> @retval None
        % ====================================================================== %
        function delete(obj)
            % delete python environment
            delete(obj.pe);
        end
        
        % ====================================================================== %
        %> @brief Filter the data using the VQF algorithm (only forward).
        %>
        %> @param obj - Instance of the MyVqf class.
        %> @param acc - Accelerometer data [Nx3] (m/s^2).
        %> @param gyr - Gyroscope data [Nx3] (rad/s).
        %> @param mag - Magnetometer data [Nx3] (uT).
        %>
        %> @retval data - Struct with the filtered data.
        % ====================================================================== %
        function data = filter(obj, acc, gyr, mag)
 
            % convert gyr from deg/s to rad/s
            gyr = gyr * pi/180;
 
            % create numpy arrays
            acc = py.numpy.ascontiguousarray(acc);
            gyr = py.numpy.ascontiguousarray(gyr);
            mag = py.numpy.ascontiguousarray(mag);
 
            % create new VQF
            obj = obj.createVQF();
            
            % filter data
            data = obj.vqfForward.updateBatch(gyr, acc, mag);
 
            % output as struct with matlab arrays
            data                 = struct(data);
            data.quat6D          = double(data.quat6D);
            data.quat9D          = double(data.quat9D);
            data.delta           = double(data.delta);
            data.bias            = double(data.bias);
            data.biasSigma       = double(data.biasSigma);
            data.restDetected    = boolean(data.restDetected);
            data.magDistDetected = boolean(data.magDistDetected);
        end
 
        % ====================================================================== %
        %> @brief Filter the data using the VQF algorithm (offline, forward and
        %> backward).
        %>
        %> @param obj - Instance of the MyVqf class.
        %> @param acc - Accelerometer data [Nx3] (m/s^2).
        %> @param gyr - Gyroscope data [Nx3] (rad/s).
        %> @param mag - Magnetometer data [Nx3] (uT).
        %>
        %> @retval data - Struct with the filtered data.
        % ====================================================================== %
        function data = filterOffline(obj, acc, gyr, mag)
 
            % convert gyr from deg/s to rad/s
            gyr = gyr * pi/180;
            
            % create numpy arrays
            acc = py.numpy.ascontiguousarray(acc);
            gyr = py.numpy.ascontiguousarray(gyr);
            mag = py.numpy.ascontiguousarray(mag);
            
            % filter data
            data = obj.offlineVQF(acc, gyr, mag);
 
            % output as struct with matlab arrays
            data                 = struct(data);
            data.quat6D          = double(data.quat6D);
            data.quat9D          = double(data.quat9D);
            data.delta           = double(data.delta);
            data.bias            = double(data.bias);
            data.biasSigma       = double(data.biasSigma);
            data.restDetected    = boolean(data.restDetected);
            data.magDistDetected = boolean(data.magDistDetected);
        end
 
        % ====================================================================== %
        %> @brief method to set properties of the VQF filter
        %>
        %> @param obj - Instance of the MyVqf class.
        %> @param params - Struct with the properties to set.
        %>  - params.tauAcc (optional)
        %>  - params.tauMag (optional)
        %>  - params.motionBiasEstEnabled (optional)
        %>  - params.restBiasEstEnabled (optional)
        %>  - params.magDistRejectionEnabled (optional)
        %>  - params.biasSigmaInit (optional)
        %>  - params.biasForgettingTime (optional)
        %>  - params.biasClip (optional)
        %>  - params.biasSigmaMotion (optional)
        %>  - params.biasVerticalForgettingFactor (optional)
        %>  - params.biasSigmaRest (optional)
        %>  - params.restMinT (optional)
        %>  - params.restFilterTau (optional)
        %>  - params.restThGyr (optional)
        %>  - params.restThAcc (optional)
        %>  - params.magCurrentTau (optional)
        %>  - params.magRefTau (optional)
        %>  - params.magNormTh (optional)
        %>  - params.magDipTh (optional)
        %>  - params.magNewTime (optional)
        %>  - params.magNewFirstTime (optional)
        %>  - params.magNewMinGyr (optional)
        %>  - params.magMinUndisturbedTime (optional)
        %>  - params.magMaxRejectionTime (optional)
        %>  - params.magRejectionFactor (optional)
        %>
        %> @retval obj - Instance of the MyVqf class.
        %>
        %> @note The properties are set to the default values if not provided.
        %> @note see the VQF documentation for the meaning of the properties.
        % ====================================================================== %
        function obj = setProperties(obj, params)
 
            % check and init with default
            arguments
                obj {mustBeA(obj, 'my_VQF')}
                params.tauAcc                       (1,1) double {mustBePositive} = obj.tauAcc;
                params.tauMag                       (1,1) double {mustBePositive} = obj.tauMag;
                params.motionBiasEstEnabled         (1,1) logical                 = obj.motionBiasEstEnabled;
                params.restBiasEstEnabled           (1,1) logical                 = obj.restBiasEstEnabled;
                params.magDistRejectionEnabled      (1,1) logical                 = obj.magDistRejectionEnabled;
                params.biasSigmaInit                (1,1) double {mustBePositive} = obj.biasSigmaInit;
                params.biasForgettingTime           (1,1) double {mustBePositive} = obj.biasForgettingTime;
                params.biasClip                     (1,1) double {mustBePositive} = obj.biasClip;
                params.biasSigmaMotion              (1,1) double {mustBePositive} = obj.biasSigmaMotion;
                params.biasVerticalForgettingFactor (1,1) double {mustBePositive} = obj.biasVerticalForgettingFactor;
                params.biasSigmaRest                (1,1) double {mustBePositive} = obj.biasSigmaRest;
                params.restMinT                     (1,1) double {mustBePositive} = obj.restMinT;
                params.restFilterTau                (1,1) double {mustBePositive} = obj.restFilterTau;
                params.restThGyr                    (1,1) double {mustBePositive} = obj.restThGyr;
                params.restThAcc                    (1,1) double {mustBePositive} = obj.restThAcc;
                params.magCurrentTau                (1,1) double {mustBePositive} = obj.magCurrentTau;
                params.magRefTau                    (1,1) double {mustBePositive} = obj.magRefTau;
                params.magNormTh                    (1,1) double {mustBePositive} = obj.magNormTh;
                params.magDipTh                     (1,1) double {mustBePositive} = obj.magDipTh;
                params.magNewTime                   (1,1) double {mustBePositive} = obj.magNewTime;
                params.magNewFirstTime              (1,1) double {mustBePositive} = obj.magNewFirstTime;
                params.magNewMinGyr                 (1,1) double {mustBePositive} = obj.magNewMinGyr;
                params.magMinUndisturbedTime        (1,1) double {mustBePositive} = obj.magMinUndisturbedTime;
                params.magMaxRejectionTime          (1,1) double {mustBePositive} = obj.magMaxRejectionTime;
                params.magRejectionFactor           (1,1) double {mustBePositive} = obj.magRejectionFactor;
            end
 
            % set properties
            obj.tauAcc                       = params.tauAcc;
            obj.tauMag                       = params.tauMag;
            obj.motionBiasEstEnabled         = params.motionBiasEstEnabled;
            obj.restBiasEstEnabled           = params.restBiasEstEnabled;
            obj.magDistRejectionEnabled      = params.magDistRejectionEnabled;
            obj.biasSigmaInit                = params.biasSigmaInit;
            obj.biasForgettingTime           = params.biasForgettingTime;
            obj.biasClip                     = params.biasClip;
            obj.biasSigmaMotion              = params.biasSigmaMotion;
            obj.biasVerticalForgettingFactor = params.biasVerticalForgettingFactor;
            obj.biasSigmaRest                = params.biasSigmaRest;
            obj.restMinT                     = params.restMinT;
            obj.restFilterTau                = params.restFilterTau;
            obj.restThGyr                    = params.restThGyr;
            obj.restThAcc                    = params.restThAcc;
            obj.magCurrentTau                = params.magCurrentTau;
            obj.magRefTau                    = params.magRefTau;
            obj.magNormTh                    = params.magNormTh;
            obj.magDipTh                     = params.magDipTh;
            obj.magNewTime                   = params.magNewTime;
            obj.magNewFirstTime              = params.magNewFirstTime;
            obj.magNewMinGyr                 = params.magNewMinGyr;
            obj.magMinUndisturbedTime        = params.magMinUndisturbedTime;
            obj.magMaxRejectionTime          = params.magMaxRejectionTime;
            obj.magRejectionFactor           = params.magRejectionFactor;
        end
 
        % properties getter
        function params = getProperties(obj)
            params.tauAcc                       = obj.tauAcc;
            params.tauMag                       = obj.tauMag;
            params.motionBiasEstEnabled         = obj.motionBiasEstEnabled;
            params.restBiasEstEnabled           = obj.restBiasEstEnabled;
            params.magDistRejectionEnabled      = obj.magDistRejectionEnabled;
            params.biasSigmaInit                = obj.biasSigmaInit;
            params.biasForgettingTime           = obj.biasForgettingTime;
            params.biasClip                     = obj.biasClip;
            params.biasSigmaMotion              = obj.biasSigmaMotion;
            params.biasVerticalForgettingFactor = obj.biasVerticalForgettingFactor;
            params.biasSigmaRest                = obj.biasSigmaRest;
            params.restMinT                     = obj.restMinT;
            params.restFilterTau                = obj.restFilterTau;
            params.restThGyr                    = obj.restThGyr;
            params.restThAcc                    = obj.restThAcc;
            params.magCurrentTau                = obj.magCurrentTau;
            params.magRefTau                    = obj.magRefTau;
            params.magNormTh                    = obj.magNormTh;
            params.magDipTh                     = obj.magDipTh;
            params.magNewTime                   = obj.magNewTime;
            params.magNewFirstTime              = obj.magNewFirstTime;
            params.magNewMinGyr                 = obj.magNewMinGyr;
            params.magMinUndisturbedTime        = obj.magMinUndisturbedTime;
            params.magMaxRejectionTime          = obj.magMaxRejectionTime;
            params.magRejectionFactor           = obj.magRejectionFactor;
        end
 
    end
    
    methods (Access=protected)
 
        % ====================================================================== %
        %> @brief Create and configure VQF forward filter.
        %>
        %> @param obj - Instance of the MyVqf class.
        %>
        %> @retval obj - Instance of the MyVqf class.
        % ====================================================================== %
        function obj = createVQF(obj)
            % create VQF object
            vqf = py.vqf.VQF(...
                obj.gyrTs, ...
                accTs=obj.accTs, ...
                magTs=obj.magTs, ...
                tauAcc=obj.tauAcc, ...
                tauMag                       = obj.tauMag,                       ...
                motionBiasEstEnabled         = obj.motionBiasEstEnabled,         ...
                restBiasEstEnabled           = obj.restBiasEstEnabled,           ...
                magDistRejectionEnabled      = obj.magDistRejectionEnabled,      ...
                biasSigmaInit                = obj.biasSigmaInit,                ...
                biasForgettingTime           = obj.biasForgettingTime,           ...
                biasClip                     = obj.biasClip,                     ...
                biasSigmaMotion              = obj.biasSigmaMotion,              ...
                biasVerticalForgettingFactor = obj.biasVerticalForgettingFactor, ...
                biasSigmaRest                = obj.biasSigmaRest,                ...
                restMinT                     = obj.restMinT,                     ...
                restFilterTau                = obj.restFilterTau,                ...
                restThGyr                    = obj.restThGyr,                    ...
                restThAcc                    = obj.restThAcc,                    ...
                magCurrentTau                = obj.magCurrentTau,                ...
                magRefTau                    = obj.magRefTau,                    ...
                magNormTh                    = obj.magNormTh,                    ...
                magDipTh                     = obj.magDipTh,                     ...
                magNewTime                   = obj.magNewTime,                   ...
                magNewFirstTime              = obj.magNewFirstTime,              ...
                magNewMinGyr                 = obj.magNewMinGyr,                 ...
                magMinUndisturbedTime        = obj.magMinUndisturbedTime,        ...
                magMaxRejectionTime          = obj.magMaxRejectionTime,          ...
                magRejectionFactor           = obj.magRejectionFactor);
 
            obj.vqfForward = vqf;
            
        end
 
        % ====================================================================== %
        %> @brief Call the offline VQF algorithm.
        %>
        %> @param obj - Instance of the MyVqf class.
        %> @param acc - Accelerometer data [Nx3] (m/s^2).
        %> @param gyr - Gyroscope data [Nx3] (rad/s).
        %> @param mag - Magnetometer data [Nx3] (uT).
        %>
        %> @retval data - Struct with the filtered data.
        %>
        %> @note See the VQF documentation for the structure of the data.
        % ====================================================================== %
        function data = offlineVQF(obj, acc, gyr, mag)
 
            % create python dict with params
            params = py.dict(...
                tauAcc                       = obj.tauAcc,                       ...
                tauMag                       = obj.tauMag,                       ...
                motionBiasEstEnabled         = obj.motionBiasEstEnabled,         ...
                restBiasEstEnabled           = obj.restBiasEstEnabled,           ...
                magDistRejectionEnabled      = obj.magDistRejectionEnabled,      ...
                biasSigmaInit                = obj.biasSigmaInit,                ...
                biasForgettingTime           = obj.biasForgettingTime,           ...
                biasClip                     = obj.biasClip,                     ...
                biasSigmaMotion              = obj.biasSigmaMotion,              ...
                biasVerticalForgettingFactor = obj.biasVerticalForgettingFactor, ...
                biasSigmaRest                = obj.biasSigmaRest,                ...
                restMinT                     = obj.restMinT,                     ...
                restFilterTau                = obj.restFilterTau,                ...
                restThGyr                    = obj.restThGyr,                    ...
                restThAcc                    = obj.restThAcc,                    ...
                magCurrentTau                = obj.magCurrentTau,                ...
                magRefTau                    = obj.magRefTau,                    ...
                magNormTh                    = obj.magNormTh,                    ...
                magDipTh                     = obj.magDipTh,                     ...
                magNewTime                   = obj.magNewTime,                   ...
                magNewFirstTime              = obj.magNewFirstTime,              ...
                magNewMinGyr                 = obj.magNewMinGyr,                 ...
                magMinUndisturbedTime        = obj.magMinUndisturbedTime,        ...
                magMaxRejectionTime          = obj.magMaxRejectionTime,          ...
                magRejectionFactor           = obj.magRejectionFactor);
 
            % call the offline VQF
            data = py.vqf.offlineVQF(gyr, acc, mag, obj.gyrTs, params);
        end
        
    end
end