jumpEstimateCalibrationParameters.m

Go to the documentation of this file.

%> @brief this function estimates accelerometer calibration parameters
%>
%> @details this function estimates accelerometer calibration parameters
%> (sensitivity and bias) using zero velocity ranges. The function uses data
%> coming from a 6 axis calibration sequence (rotating a cube on a flat surface)
%> or from a 12 axis calibration sequence (rotating a dodecahedron on a flat
%> surface). The function uses a least squares optimization to estimate the
%> calibration parameters. The function also estimates the gyroscope bias.
%>
%> @param data - struct as defined in @ref jumpCheckData, the data must contain
%> zero movement sequences where only one axis is aligned with gravity and the
%> other two axes are perpendicular to gravity. The data must contain at least
%> six different sequences, i.e. every side must lay towards the ground at least
%> once. The order of the sequences is not important.
%> @param zeroVelocity - logical vector indicating zero velocity ranges
%> @param options - the available options are:
%> - plot - logical, if true, plots the results to the console (default false)
%> - minZvDuration - minimum duration of zero velocity range in seconds (default 0.1 s)
%> - maxZvDuration - maximum duration of zero velocity range in seconds, before
%> splitting it into smaller ranges (default 60 s)
%> - dodecahedron - logical, if true, the function expects data from a dodecahedron
%> movement (default false). If enabled, the function expects all zero velocity
%> ranges to be one of the 12 faces, i.e. it will not check if any wrong orientations
%> are present.
%>
%> @retval params - struct containing the estimated calibration parameters
%> for the accelerometer and high g sensor. The struct has the following fields:
%> - acc - struct containing the accelerometer calibration parameters:
%>   - bias - 1x3 vector containing the estimated accelerometer biases in g
%>   - sensitivity - 1x3 vector (six faces case) or 3x3 matrix (twelve faces case)
%> - hig - struct containing the high g sensor calibration parameters:
%>   - bias - 1x3 vector containing the estimated high g sensor biases in g
%>   - sensitivity - 1x3 vector (six faces case) or 3x3 matrix (twelve faces case)
%> containing the estimated accelerometer sensitivities (and cross-axis sensitivities).
%> - gyro - struct containing the gyroscope calibration parameters:
%>   - bias - 1x3 vector containing the estimated gyroscope biases in rad/s
%>
%> @note see @ref derivation7IntialParamEst for more detailes information.
%>
%> @remark @parblock
%> The calibration parameters are estimated using the following error models.
%>
%> <b>For the six faces case</b>:
%>
%> The accelerometer measurements are modeled as:
%> @f[
%> \begin{align*}
%> a_x^\text{(meas)} &= s_{x}^\text{(a)} \cdot a_x^\text{(true)} + b_{x}^\text{(a)} + \varepsilon_{ax} \\
%> a_y^\text{(meas)} &= s_{y}^\text{(a)} \cdot a_y^\text{(true)} + b_{y}^\text{(a)} + \varepsilon_{ay} \\
%> a_z^\text{(meas)} &= s_{z}^\text{(a)} \cdot a_z^\text{(true)} + b_{z}^\text{(a)} + \varepsilon_{az} \\
%> \end{align*}
%> @f]
%> where @f$ \varepsilon @f$ is the residual error.
%>
%> The return value for the sensitivity is a @f$ 1 \times 3 @f$ vector containing the
%> sensitivities in the following order: @f$ s_{x}^\text{(a)} @f$,
%> @f$ s_{y}^\text{(a)} @f$, @f$ s_{z}^\text{(a)} @f$. The return value for the bias
%> is a @f$ 1 \times 3 @f$ vector containing the biases in the following order:
%> @f$b_{x}^\text{(a)} @f$, @f$b_{y}^\text{(a)} @f$, @f$b_{z}^\text{(a)} @f$.
%>
%> The gyroscope measurements are modeled as:
%> @f[
%> \begin{align*}
%> \omega_x^\text{(meas)} &= \omega_x^\text{(true)} + b_{x}^{(\omega)} + \varepsilon_{\omega x} \\
%> \omega_y^\text{(meas)} &= \omega_y^\text{(true)} + b_{y}^{(\omega)} + \varepsilon_{\omega y} \\
%> \omega_z^\text{(meas)} &= \omega_z^\text{(true)} + b_{z}^{(\omega)} + \varepsilon_{\omega z} \\
%> \end{align*}
%> @f]
%>
%> The return value for the bias is a @f$ 1 \times 3 @f$ vector containing the biases
%> in the following order: @f$b_{x}^{(\omega)} @f$, @f$b_{y}^{(\omega)} @f$,
%> @f$b_{z}^{(\omega)} @f$.
%>
%> <b>For the twelve faces case</b>:
%>
%> The accelerometer measurements are modeled as:
%> @f[
%> \begin{align*}
%> a_x^\text{(meas)} &= s_{x}^{(a)} a_{x} + s_{xy}^{(a)} a_{y} + s_{xz}^{(a)} a_{z} + b_x^{(a)} + \varepsilon_{ax} \\[3mm]
%> a_y^\text{(meas)} &= s_{yx}^{(a)} a_{x} + s_{y}^{(a)} a_{y} + s_{yz}^{(a)} a_{z} + b_y^{(a)} + \varepsilon_{ay} \\[3mm]
%> a_z^\text{(meas)} &= s_{zx}^{(a)} a_{x} + s_{zy}^{(a)} a_{y} + s_{z}^{(a)} a_{z} + b_z^{(a)} + \varepsilon_{az},
%> \end{align*},
%> @f]
%>
%> where the returned sensitivity matrix is
%> @f[
%> \mathbf{s}_a = \begin{bmatrix}
%> s_{xx}^{(a)} & s_{xy}^{(a)} & s_{xz}^{(a)} \\
%> s_{yx}^{(a)} & s_{yy}^{(a)} & s_{yz}^{(a)} \\
%> s_{zx}^{(a)} & s_{zy}^{(a)} & s_{zz}^{(a)}
%> \end{bmatrix}
%> @f]
%> and the returned bias vector is identical to the six faces case.
%>
%> The gyroscope measurements are modeled identically to the six faces case.
%>
%> @endparblock
%>
%> @remark The dodecahedron math is from Battaglia, F., & Prato, E. (2022). Generalized Laurent monomials in nonrational toric geometry. https://doi.org/10.48550/ARXIV.2212.11104
%>
%>
%> @file jumpEstimateCalibrationParameters.m
%>
%> @copyright see the file @ref LICENSE in the root directory of the repository
 
function params = jumpEstimateCalibrationParameters(data, zeroVelocity, options)
 
%% input checks
arguments
    data (1,1) struct
    zeroVelocity (:,1) logical
    options.plot (1,1) logical = false
    options.minZvDuration (1,1) double = 0.5
    options.maxZvDuration (1,1) double = 30
    options.dodecahedron (1,1) logical = false
end
 
% check data structure
assert(jumpCheckData(data), 'Data structure is not correct')
assert(height(data.tt == height(zeroVelocity)), 'Data and zeroVelocity must have the same length')
 
% define g
g = 9.81;
 
%% data preparation
 
% check the amount of zero velocity ranges (consecutive zero velocity detections)
d_zero = diff([0; zeroVelocity; 0]);
zero_ranges = [find(d_zero == 1), find(d_zero == -1)-1];
 
% remove zero velocity ranges that are too short
zero_ranges = zero_ranges((zero_ranges(:,2) - zero_ranges(:,1))/data.mpu_rate > options.minZvDuration, :);
 
% if any zero velocity range is larger than maxZvDuration, split it into smaller ranges
fs = data.mpu_rate;
for i = size(zero_ranges,1):-1:1
    if (zero_ranges(i,2) - zero_ranges(i,1))/fs > options.maxZvDuration
        % split the range into smaller ranges
        n = ceil((zero_ranges(i,2) - zero_ranges(i,1))/fs/options.maxZvDuration);
        split_ranges = round(linspace(zero_ranges(i,1), zero_ranges(i,2), n+1));
        zero_ranges = [zero_ranges(1:i-1,:); split_ranges(1:end-1)', split_ranges(2:end)'; zero_ranges(i+1:end,:)];
    end
end
 
% check if there are any zero velocity ranges
if isempty(zero_ranges)
    error('No zero velocity ranges found')
end
 
% number of zero velocity ranges
n_ranges = size(zero_ranges,1);
 
% extract mean measurements for each zero velocity range
acc_meas = zeros(n_ranges, 3); % accelerometer measurements
hig_meas = zeros(n_ranges, 3); % high g accelerometer measurements
gyr_meas = zeros(n_ranges, 3); % gyroscope measurements
for i = 1:n_ranges
    acc_meas(i,:) = mean(data.tt.acc(zero_ranges(i,1):zero_ranges(i,2),:),1);
    hig_meas(i,:) = mean(data.tt.hig(zero_ranges(i,1):zero_ranges(i,2),:),1);
    gyr_meas(i,:) = mean(data.tt.gyr(zero_ranges(i,1):zero_ranges(i,2),:),1);
end
 
 
% perform various checks depending on the calibration sequence (cube or dodecahedron)
 
if ~options.dodecahedron
    % check that all detected zero velocity ranges are valid (i.e. only one axis is
    % aligned with gravity). since we know from the datasheet that we can expect
    % cross-axis sensitivity of +-2% and biases of +- 0.08 g, we can use these values
    % to check if the zero velocity range is valid.
    
    fac = 2; % factor to extend typical datasheet range
    max_value_perpendicular = fac * (0.1 * g);
    min_value_aligned = (1 - (fac * 0.1)) * g;
    for i = n_ranges:-1:1
        perp = 0;
        aligned = 0;
        for j = 1:3
            if abs(acc_meas(i,j)) < max_value_perpendicular
                perp = perp + 1;
            elseif abs(acc_meas(i,j)) > min_value_aligned
                aligned = aligned + 1;
            end
        end
        
        if perp ~= 2 || aligned ~= 1
            warning('Found invalid range and removed it. Was the sensor laying flat on a surface?')
            zero_ranges(i,:) = [];
            acc_meas(i,:) = [];
            hig_meas(i,:) = [];
            
            n_ranges = n_ranges - 1;
        end
    end
    
    % extract true gravity vector for each zero velocity range
    true_acc = zeros(n_ranges, 3);
    for i = 1:n_ranges
        [~, idx] = max(abs(acc_meas(i,:)));
        true_acc(i,idx) = sign(acc_meas(i,idx)) * g;
    end
    
    % check if all six sides of the cube are present at least once
    try
        assert(any(true_acc(:,1) ==  g & true_acc(:,2) ==  0 & true_acc(:,3) ==  0)); % x axis positive
        assert(any(true_acc(:,1) == -g & true_acc(:,2) ==  0 & true_acc(:,3) ==  0)); % x axis negative
        assert(any(true_acc(:,1) ==  0 & true_acc(:,2) ==  g & true_acc(:,3) ==  0)); % y axis positive
        assert(any(true_acc(:,1) ==  0 & true_acc(:,2) == -g & true_acc(:,3) ==  0)); % y axis negative
        assert(any(true_acc(:,1) ==  0 & true_acc(:,2) ==  0 & true_acc(:,3) ==  g)); % z axis positive
        assert(any(true_acc(:,1) ==  0 & true_acc(:,2) ==  0 & true_acc(:,3) == -g)); % z axis negative
    catch
        error('Not all six sides of the cube are present in the data')
    end
    
else % dodecahedron case
    
    % define the dodecahedron normal vectors
    
    phi = (1 + sqrt(5)) / 2;
    
    dod_norm = [...
        1/phi,  1,      0    ; ...
        0,      1/phi,  1    ; ...
        1,      0,      1/phi; ...
        -1/phi, 1,      0    ; ...
        0,     -1/phi,  1    ; ...
        1,      0,     -1/phi];
    
    dod_norm = [dod_norm; -dod_norm]'; % add the negative vectors
    
    % rotate around x to align with gravity vector
    alpha = asin( (1/phi) / norm([1/phi, 1]) );
    R = rotx(rad2deg(alpha));
    dod_norm = R * dod_norm;
    
    % norm the vectors
    dod_norm = dod_norm ./ vecnorm(dod_norm);
    
    % multiply with g
    dod_norm = dod_norm' * g;
    
    % swap the first two columns to math the frame
    dod_norm = dod_norm(:,[2,1,3]);
    
    % extract true gravity vector for each zero velocity range
    true_acc = zeros(n_ranges, 3);
    for i = 1:n_ranges
        
        % find the closest normal vector
        [this_min, idx] = min(vecnorm(dod_norm - acc_meas(i,:), 2, 2));
        
        % assign the true gravity vector
        true_acc(i,:) = dod_norm(idx,:);
        
        if norm(this_min) > 0.65
            warning("Found possible wrong orientation. Please double check the data")
        end
        
    end
    
    % check if all twelve faces of the dodecahedron are present at least once
    if ~isempty(setdiff(dod_norm, unique(true_acc, 'rows'), 'rows'))
        error('Not all twelve faces of the dodecahedron are present in the data')
    end
    
end
 
%% extracting gyro bias
params.gyr.bias = mean(gyr_meas);
 
 
%% optimization
 
% problem
prob = optimproblem('ObjectiveSense', 'minimize');
 
% variables accelerometer
% biases (2x typical datasheet values)
b_acc_x = optimvar('b_acc_x', 1, 'Type', 'continuous', 'LowerBound', -0.12 * g, 'UpperBound', 0.12 * g);
b_acc_y = optimvar('b_acc_y', 1, 'Type', 'continuous', 'LowerBound', -0.12 * g, 'UpperBound', 0.12 * g);
b_acc_z = optimvar('b_acc_z', 1, 'Type', 'continuous', 'LowerBound', -0.16 * g, 'UpperBound', 0.16 * g);
 
% sensitivities (2x typical datasheet values)
s_acc_x = optimvar('s_acc_x', 1, 'Type', 'continuous', 'LowerBound', 0.94, 'UpperBound', 1.06);
s_acc_y = optimvar('s_acc_y', 1, 'Type', 'continuous', 'LowerBound', 0.94, 'UpperBound', 1.06);
s_acc_z = optimvar('s_acc_z', 1, 'Type', 'continuous', 'LowerBound', 0.94, 'UpperBound', 1.06);
 
% cross axis sensitivities (2x typical datasheet values)
if options.dodecahedron
    s_acc_xy = optimvar('s_acc_xy', 1, 'Type', 'continuous', 'LowerBound', -0.06, 'UpperBound', 0.06);
    s_acc_xz = optimvar('s_acc_xz', 1, 'Type', 'continuous', 'LowerBound', -0.06, 'UpperBound', 0.06);
    s_acc_yx = optimvar('s_acc_yx', 1, 'Type', 'continuous', 'LowerBound', -0.06, 'UpperBound', 0.06);
    s_acc_yz = optimvar('s_acc_yz', 1, 'Type', 'continuous', 'LowerBound', -0.06, 'UpperBound', 0.06);
    s_acc_zx = optimvar('s_acc_zx', 1, 'Type', 'continuous', 'LowerBound', -0.06, 'UpperBound', 0.06);
    s_acc_zy = optimvar('s_acc_zy', 1, 'Type', 'continuous', 'LowerBound', -0.06, 'UpperBound', 0.06);
end
 
% residuals
eps_acc_x = optimvar('eps_acc_x', n_ranges, 'Type', 'continuous');
eps_acc_y = optimvar('eps_acc_y', n_ranges, 'Type', 'continuous');
eps_acc_z = optimvar('eps_acc_z', n_ranges, 'Type', 'continuous');
 
% variables high g sensor
% biases (max datasheet values)
b_hig_x = optimvar('b_hig_x', 1, 'Type', 'continuous', 'LowerBound', -6 * g, 'UpperBound', 6 * g);
b_hig_y = optimvar('b_hig_y', 1, 'Type', 'continuous', 'LowerBound', -6 * g, 'UpperBound', 6 * g);
b_hig_z = optimvar('b_hig_z', 1, 'Type', 'continuous', 'LowerBound', -6 * g, 'UpperBound', 6 * g);
 
% sensitivities (max datasheet values)
s_hig_x = optimvar('s_hig_x', 1, 'Type', 'continuous', 'LowerBound', 0.87, 'UpperBound', 1.13);
s_hig_y = optimvar('s_hig_y', 1, 'Type', 'continuous', 'LowerBound', 0.87, 'UpperBound', 1.13);
s_hig_z = optimvar('s_hig_z', 1, 'Type', 'continuous', 'LowerBound', 0.87, 'UpperBound', 1.13);
 
% cross axis sensitivities (2x typical datasheet values)
if options.dodecahedron
    s_hig_xy = optimvar('s_hig_xy', 1, 'Type', 'continuous', 'LowerBound', -0.075, 'UpperBound', 0.075);
    s_hig_xz = optimvar('s_hig_xz', 1, 'Type', 'continuous', 'LowerBound', -0.075, 'UpperBound', 0.075);
    s_hig_yx = optimvar('s_hig_yx', 1, 'Type', 'continuous', 'LowerBound', -0.075, 'UpperBound', 0.075);
    s_hig_yz = optimvar('s_hig_yz', 1, 'Type', 'continuous', 'LowerBound', -0.075, 'UpperBound', 0.075);
    s_hig_zx = optimvar('s_hig_zx', 1, 'Type', 'continuous', 'LowerBound', -0.075, 'UpperBound', 0.075);
    s_hig_zy = optimvar('s_hig_zy', 1, 'Type', 'continuous', 'LowerBound', -0.075, 'UpperBound', 0.075);
end
 
% residuals
eps_hig_x = optimvar('eps_hig_x', n_ranges, 'Type', 'continuous');
eps_hig_y = optimvar('eps_hig_y', n_ranges, 'Type', 'continuous');
eps_hig_z = optimvar('eps_hig_z', n_ranges, 'Type', 'continuous');
 
% constraints
if ~options.dodecahedron
    % acc
    meas_acc_x = s_acc_x * true_acc(:,1) + b_acc_x - acc_meas(:,1) == eps_acc_x;
    meas_acc_y = s_acc_y * true_acc(:,2) + b_acc_y - acc_meas(:,2) == eps_acc_y;
    meas_acc_z = s_acc_z * true_acc(:,3) + b_acc_z - acc_meas(:,3) == eps_acc_z;
    
    % hig
    meas_hig_x = s_hig_x * true_acc(:,1) + b_hig_x - hig_meas(:,1) == eps_hig_x;
    meas_hig_y = s_hig_y * true_acc(:,2) + b_hig_y - hig_meas(:,2) == eps_hig_y;
    meas_hig_z = s_hig_z * true_acc(:,3) + b_hig_z - hig_meas(:,3) == eps_hig_z;
    
else
    % acc
    meas_acc_x = s_acc_x * true_acc(:,1) + s_acc_xy * true_acc(:,2) + s_acc_xz * true_acc(:,3) + b_acc_x - acc_meas(:,1) == eps_acc_x;
    meas_acc_y = s_acc_y * true_acc(:,2) + s_acc_yx * true_acc(:,1) + s_acc_yz * true_acc(:,3) + b_acc_y - acc_meas(:,2) == eps_acc_y;
    meas_acc_z = s_acc_z * true_acc(:,3) + s_acc_zx * true_acc(:,1) + s_acc_zy * true_acc(:,2) + b_acc_z - acc_meas(:,3) == eps_acc_z;
    
    % hig
    meas_hig_x = s_hig_x * true_acc(:,1) + s_hig_xy * true_acc(:,2) + s_hig_xz * true_acc(:,3) + b_hig_x - hig_meas(:,1) == eps_hig_x;
    meas_hig_y = s_hig_y * true_acc(:,2) + s_hig_yx * true_acc(:,1) + s_hig_yz * true_acc(:,3) + b_hig_y - hig_meas(:,2) == eps_hig_y;
    meas_hig_z = s_hig_z * true_acc(:,3) + s_hig_zx * true_acc(:,1) + s_hig_zy * true_acc(:,2) + b_hig_z - hig_meas(:,3) == eps_hig_z;
end
 
 
% objective
obj = sum(eps_acc_x.^2) + sum(eps_acc_y.^2) + sum(eps_acc_z.^2) + ...
    sum(eps_hig_x.^2) + sum(eps_hig_y.^2) + sum(eps_hig_z.^2);
 
% add constraints and objective
prob.Constraints.meas_acc_x = meas_acc_x;
prob.Constraints.meas_acc_y = meas_acc_y;
prob.Constraints.meas_acc_z = meas_acc_z;
prob.Constraints.meas_hig_x = meas_hig_x;
prob.Constraints.meas_hig_y = meas_hig_y;
prob.Constraints.meas_hig_z = meas_hig_z;
prob.Objective = obj;
 
% options
opts = optimoptions("lsqlin", ...
    "Algorithm","interior-point", ...
    "Display","off", ...
    "OptimalityTolerance", 1e-18, ...
    "StepTolerance", 1e-15, ...
    "MaxIterations", 1e6);
 
if options.plot
    opts.Display = "iter-detailed";
end
 
[sol, ~, flag]  = solve(prob, "Options",opts);
 
if flag < 1
    error('Optimization did not converge')
end
 
% extract results
params.acc.bias = [sol.b_acc_x, sol.b_acc_y, sol.b_acc_z];
params.hig.bias = [sol.b_hig_x, sol.b_hig_y, sol.b_hig_z];
 
if ~options.dodecahedron
    params.acc.sensitivity = [sol.s_acc_x, sol.s_acc_y, sol.s_acc_z];
    params.hig.sensitivity = [sol.s_hig_x, sol.s_hig_y, sol.s_hig_z];
else
    params.acc.sensitivity = [sol.s_acc_x, sol.s_acc_xy, sol.s_acc_xz; ...
        sol.s_acc_yx, sol.s_acc_y, sol.s_acc_yz; ...
        sol.s_acc_zx, sol.s_acc_zy, sol.s_acc_z];
    params.hig.sensitivity = [sol.s_hig_x, sol.s_hig_xy, sol.s_hig_xz; ...
        sol.s_hig_yx, sol.s_hig_y, sol.s_hig_yz; ...
        sol.s_hig_zx, sol.s_hig_zy, sol.s_hig_z];
end
 
end