Matlab Fuse Inertial Sensors

% Orientation only (default motion model)
filt = insEKF(insAccelerometer, insGyroscope, insMotionOrientation);

% Full pose: orientation + position + velocity
filt = insEKF(insAccelerometer, insGyroscope, insGPS, insMotionPose);

% Full pose with magnetometer
filt = insEKF(insAccelerometer, insGyroscope, insGPS, insMagnetometer, insMotionPose);

% Custom sensor alongside built-in sensors
customSensor = MyCustomSensorModel;   % subclass positioning.INSSensorModel
filt = insEKF(insAccelerometer, insGyroscope, customSensor, insMotionPose);

gpsSensor = insGPS;
gpsSensor.ReferenceLocation = [42.3, -71.1, 100];   % [lat deg, lon deg, alt m]
filt = insEKF(insAccelerometer, insGyroscope, gpsSensor, insMotionPose);

opts = insOptions(ReferenceFrame="ENU", ...
                  SensorNamesSource="Property", ...
                  SensorNames={'GPS1','GPS2','Accel'});
filt = insEKF(insGPS, insGPS, insAccelerometer, insMotionPose, opts);

stateinfo(filt)                             % print all states and their indices
% To query a specific sensor's state index, pass the SAME instance used at construction:
%   accSensor = insAccelerometer;
%   filt = insEKF(accSensor, insGyroscope, insMotionPose);
%   stateinfo(filt, accSensor, 'Bias')      % works because accSensor is the construction instance

accSensor  = insAccelerometer;
gyroSensor = insGyroscope;
gpsSensor  = insGPS;
filt = insEKF(accSensor, gyroSensor, gpsSensor, insMotionPose);

% Initialize state
stateparts(filt, 'Orientation', compact(initOrient));
statecovparts(filt, 'Orientation', 1e-2);

dt = 0.01;   % seconds between IMU samples
for i = 1:N
    predict(filt, dt);

    filt.fuse(accSensor,  accel(i,:), Raccel);   % [3x3] noise covariance
    filt.fuse(gyroSensor, gyro(i,:),  Rgyro);

    if gpsAvailable(i)
        filt.fuse(gpsSensor, lla(i,:), Rgps);    % lla: [lat_deg, lon_deg, alt_m]
    end
end

orientation = quaternion(stateparts(filt, 'Orientation'));
position    = stateparts(filt, 'Position');
velocity    = stateparts(filt, 'Velocity');

stateparts(filt, 'Orientation')             % returns 1x4 double [w x y z]; wrap with quaternion() for a quaternion object
stateparts(filt, 'Orientation', q)          % set orientation (1x4 compact quaternion)
stateparts(filt, 'Position')                % get position [x y z]
stateparts(filt, 'Velocity')                % get velocity [vx vy vz]
statecovparts(filt, 'Orientation', val)     % set orientation covariance

accSensor  = insAccelerometer;
gyroSensor = insGyroscope;
filt = insEKF(accSensor, gyroSensor, insMotionPose);

% Read sensor bias
accBias  = stateparts(filt, accSensor, 'Bias');    % 1x3 accelerometer bias
gyroBias = stateparts(filt, gyroSensor, 'Bias');   % 1x3 gyroscope bias

% Set sensor bias
stateparts(filt, accSensor, 'Bias', [0.01 -0.02 0.005]);

orientation = quaternion(stateparts(filt, 'Orientation'));
position    = stateparts(filt, 'Position');
velocity    = stateparts(filt, 'Velocity');

% sensorData: timetable with columns named by filt.SensorNames
% measureNoise: struct from tunernoise(filt), or a manually constructed struct
estimates = estimateStates(filt, sensorData, measureNoise);

% Request smoothed estimates (RTS smoother — more memory and compute)
[estimates, smoothEstimates] = estimateStates(filt, sensorData, measureNoise);

measureNoise = tunernoise(filt);
cfg = tunerconfig(filt, MaxIterations=50);
measureNoise = tune(filt, measureNoise, sensorData, groundTruth, cfg);
estimates = estimateStates(filt, sensorData, measureNoise);

measureNoise = tunernoise(filt);              % fields named {SensorName}Noise: AccelerometerNoise, GyroscopeNoise, GPSNoise, etc.
cfg = tunerconfig(filt, MaxIterations=200);   % requires filter INSTANCE, not string

% Reinitialize before tuning
stateparts(filt, 'Orientation', compact(initOrient));
statecovparts(filt, 'Orientation', 1e-2);

tunedNoise = tune(filt, measureNoise, sensorData, groundTruth, cfg);

% Apply tuned noise in batch estimate
tunedStates = estimateStates(filt, sensorData, tunedNoise);

config = tunerconfig(filt);

% Inspect defaults — AdditiveProcessNoise is tuned element-by-element (diagonals)
config.TunableParameters
% {{'AdditiveProcessNoise', [1 15 29 ...]}, 'AccelerometerNoise', 'GyroscopeNoise'}

% Tune only AccelerometerNoise and GyroscopeNoise (drop process noise)
config.TunableParameters = {'AccelerometerNoise', 'GyroscopeNoise'};

tunedNoise = tune(filt, tunernoise(filt), sensorData, groundTruth, config);

createTunerCostTemplate(filt)

config = tunerconfig(filt);
config.Cost = 'Custom';
config.CustomCostFcn = @myCostFcn;

tunedNoise = tune(filt, tunernoise(filt), sensorData, groundTruth, config);

% CustomCostFcn receives 3 inputs: (filter, measNoise, sensorData)
% groundTruth is NOT passed — load/access your own reference inside the function
function cost = myCostFcn(filter, measNoise, sensorData)
    states = estimateStates(filter, sensorData, measNoise);
    velError = states.Velocity - myGroundTruthVelocity;
    cost = rms(vecnorm(velError, 2, 2));
end

config = tunerconfig(filt);
config.OutputFcn = @(params, info) logAndStop(params, info);

function stop = logAndStop(params, info)
    fprintf('Iter %d: cost = %.4f\n', info.Iteration, info.Cost);
    stop = info.Cost < 0.05;   % stop early if good enough
end

classdef MyMotionModel < positioning.INSMotionModel
    methods
        function states = modelstates(obj)
            % Return Nx1 string array of state names
            states = ["X"; "Y"; "Heading"];
        end
        function [state, F] = stateTransition(obj, state, dt, varargin)
            % Propagate state forward; return updated state and Jacobian F
        end
        function jac = stateTransitionJacobian(obj, filter, dt, varargin)
            % Return S-by-N Jacobian matrix for the state transition
        end
    end
end