derivation7IntialParamEst.m

In this example, we demonstrate how to estimate the initial calibration parameters of an IMU using the jumpEstimateCalibrationParameters function.

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: Part 7: Initial Parameter Estimation

Details

% =========================================================================== %
%> @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));