derivation7IntialParamEst.m

Go to the documentation of this file.

% =========================================================================== %
%> @example derivation7IntialParamEst.m
%>
%> In this example, we demonstrate how to estimate the initial calibration
%> parameters of an IMU using the `jumpEstimateCalibrationParameters` function.
%>
%> @note a more detailed explanation can be found here: @ref derivation7IntialParamEst
%>
%> <details>
% =========================================================================== %
clear; close all; clc;
 
% set seed for reproducibility
rng(1);
 
%% Sensor Data Generation
% load predefined trajectory
load('rotatingCube.mat');
 
% simulate data
js = JumpSensor;
data = js.generateFromTrajectory(traj);
 
% get parameters
biases = js.getParams().mpu.acc.ConstantBias;
sensitivities = diag(js.getParams().mpu.acc.AxesMisalignment) ./ 100;
biases_hig = js.getParams().hig.acc.ConstantBias;
sensitivities_hig = diag(js.getParams().hig.acc.AxesMisalignment) ./100;
 
 
% estimate parameters
zv = jumpZeroVelocityDetection(data);
params = jumpEstimateCalibrationParameters(data, zv);
 
% display results
disp("Sequence 1, (synthetic data)")
disp('MPU Accelerometer Parameters:')
disp('Known Parameters from simulation:')
fprintf('Biases: x=%.3f, y=%.3f, z=%.3f\n', biases(1), biases(2), biases(3));
fprintf('Sensitivities: x=%.3f, y=%.3f, z=%.3f\n', sensitivities(1), sensitivities(2), sensitivities(3));
disp(' ');
disp('Estimated Parameters from data:')
fprintf('Biases: x=%.3f, y=%.3f, z=%.3f\n', params.acc.bias(1), params.acc.bias(2), params.acc.bias(3));
fprintf('Sensitivities: x=%.3f, y=%.3f, z=%.3f\n', params.acc.sensitivity(1), params.acc.sensitivity(2), params.acc.sensitivity(3));
disp(' ');
disp('High G Accelerometer Parameters:')
disp('Known Parameters from simulation:')
fprintf('Biases: x=%.3f, y=%.3f, z=%.3f\n', biases_hig(1), biases_hig(2), biases_hig(3));
fprintf('Sensitivities: x=%.3f, y=%.3f, z=%.3f\n', sensitivities_hig(1), sensitivities_hig(2), sensitivities_hig(3));
disp(' ');
disp("Estimated Parameters from data:")
fprintf('Biases: x=%.3f, y=%.3f, z=%.3f\n', params.hig.bias(1), params.hig.bias(2), params.hig.bias(3));
fprintf('Sensitivities: x=%.3f, y=%.3f, z=%.3f\n', params.hig.sensitivity(1), params.hig.sensitivity(2), params.hig.sensitivity(3));
disp('=======================================')
 
%% estimate on real data
datapath = getDataPath();
datapath = fullfile(datapath, "self_captured/240424_rotating_cube");
data = jumpReadData(fullfile(datapath, "rotating_cube_1.BIN"));
 
% estimate parameters
zv = jumpZeroVelocityDetection(data);
params = jumpEstimateCalibrationParameters(data, zv);
 
% display results
disp(' ');
disp("Sequence 2, (real data)")
disp('Estimated Accel Parameters from real data:')
fprintf('Biases: x=%.3f, y=%.3f, z=%.3f\n', params.acc.bias(1), params.acc.bias(2), params.acc.bias(3));
fprintf('Sensitivities: x=%.3f, y=%.3f, z=%.3f\n', params.acc.sensitivity(1), params.acc.sensitivity(2), params.acc.sensitivity(3));
disp(' ');
disp('Estimated High G Accel Parameters from real data:')
fprintf('Biases: x=%.3f, y=%.3f, z=%.3f\n', params.hig.bias(1), params.hig.bias(2), params.hig.bias(3));
fprintf('Sensitivities: x=%.3f, y=%.3f, z=%.3f\n', params.hig.sensitivity(1), params.hig.sensitivity(2), params.hig.sensitivity(3));
disp(' ');
disp('=======================================')
 
%% estimate on real data with zero velocity sections where the cube is not flat.
data = jumpReadData(fullfile(datapath, "rotating_cube_2.BIN"));
 
% estimate parameters
zv = jumpZeroVelocityDetection(data);
params = jumpEstimateCalibrationParameters(data, zv);
 
% display results
disp(' ');
disp("Sequence 3, (real data, not always aligned)")
disp('Estimated Parameters from real data with non-flat cube:')
fprintf('Biases: x=%.3f, y=%.3f, z=%.3f\n', params.acc.bias(1), params.acc.bias(2), params.acc.bias(3));
fprintf('Sensitivities: x=%.3f, y=%.3f, z=%.3f\n', params.acc.sensitivity(1), params.acc.sensitivity(2), params.acc.sensitivity(3));
disp(' ');
disp('Estimated High G Accel Parameters from real data with non-flat cube:')
fprintf('Biases: x=%.3f, y=%.3f, z=%.3f\n', params.hig.bias(1), params.hig.bias(2), params.hig.bias(3));
fprintf('Sensitivities: x=%.3f, y=%.3f, z=%.3f\n', params.hig.sensitivity(1), params.hig.sensitivity(2), params.hig.sensitivity(3));