Accelerometer and Magnetometer Calibration: Part 3 - Implementation in Matlab

Matlab Implementation

In this part I will be providing the source code for an implementation of the Gauss-Newton solver in Matlab. As you will see, it is a quite simple algorithm that easily can be turned into C(++)/C#/Java without much hassle. However I will not (for starters) provide this algorithm in any other language because I don't have the time, but in the future I plan on having a C, C++ and C# version that uses matrix simplifications to minimize memory consumption for simple integration into any project.

Structure
The structure is composed of three main scripts (the solver) and two scripts as an example. The code for all scripts are at the end of this post, I only explain their function here:

• nonlin_solve.m - The main nonlinear solver that implements the Gauss-Newton solver.
• yHAT.m - Creates $\hat{y}(\boldsymbol{\theta})$ for nonlin_solve.m.
• gradH.m - Creates  $\mathbf{H}(\boldsymbol{\theta})$ for nonlin_solve.m.
• test_solver.m - An example of an accelerometer calibration.
• create_meas.m - Creates a 6-point measurement set for test_ solver.m with gain plus bias errors added.

The script is extremely easy to use, the solver takes nine inputs as: nonlin_solve(x, y, z, bx, by, bz, gx, gy, gz), where x, y and z are the measurement vectors and bx, by, bz, gx, gy and gz are start guesses. To find a good starting guess it is easiest to look at the datasheet, for example an accelerometer might have the value 1024 = 1g, then the starting guess for the gain is 1024. For the bias I always start at zero, you can see my starting guess in the test_solver.m script. If you are unsure, it won't be a problem, the solver will still converge even with a substantial error in the starting guess. For example, try changing the starting guess from 1024 to 1 in test_solver.m, that is three orders of magnitude (!!!) error and it will still converge however it will need more iterations to do so.

Example run of the test_solver.m:

Loop aborted at n = 6 of 100, requested precision reached!

-: Sensor Bias :-
X-axis: 125.6636
  Error: 0.66356 (0.53%) 

Y-axis: -249.7382
  Error: 0.26185 (0.1%) 

Z-axis: 99.9163
  Error: -0.083738 (0.084%) 

MSE: 0.17196 


-: Sensor Gain :-
X-axis: 1079.4757
  Error: -0.52428 (0.049%) 

Y-axis: 1149.8909
  Error: -0.10913 (0.0095%) 

Z-axis: 920.6441
  Error: 0.6441 (0.07%) 

MSE: 0.23388

From this it is quite evident that the solver works well. But to get a feel for it you should play with it yourself! Please give it a try and come with some comments, I am always looking for improvements to this script.
   This is it for this series! I hope you have learned something about estimating parameters and how to calibrate sensors. It has been an interesting problem to try and solve. And thank you for reading!

Matlab Code

test_solver.m

clear
clc

% Set bias and gains for the solver to find
theta = [125; % mx
         -250; % my
         100; % mz
         1080; % gx
         1150; % gy
         920]; % gz

% Create measurements based on the theta
% Std. dev. of 5 and 50 measurements of each axis
[x, y, z] = create_meas(theta(1), theta(2), theta(3), ...
                           theta(4), theta(5), theta(6), 5, 50);
                       
% For nice printing
format shortG

% Run the measurements through the solver to find theta_hat                      
theta_p = nonlin_solve(x, y, z, 0, 0, 0, 1024, 1024, 1024);

% Calculate the errors
err = (theta_p - theta);
MSE_m = sum(err(1:3).^2)/3;
MSE_g = sum(err(4:6).^2)/3;

% Some nice write-out
fprintf(['-: Sensor Bias :-\n'])
fprintf(['X-axis: ', num2str(theta_p(1)), '\n\t Error: ', ...
    num2str(err(1)),' (', num2str(abs(err(1)*100/theta(1)), 2),'%%) \n'])
fprintf(['\nY-axis: ', num2str(theta_p(2)), '\n\t Error: ', ...
    num2str(err(2)),' (', num2str(abs(err(2)*100/theta(2)), 2),'%%) \n'])
fprintf(['\nZ-axis: ', num2str(theta_p(3)), '\n\t Error: ', ...
    num2str(err(3)),' (', num2str(abs(err(3)*100/theta(3)), 2),'%%) \n'])
fprintf(['\nMSE: ', num2str((MSE_m)),' \n'])



fprintf(['\n\n-: Sensor Gain :-\n'])
fprintf(['X-axis: ', num2str(theta_p(4)), '\n\t Error: ', ...
    num2str(err(4)),' (', num2str(abs(err(4)*100/theta(4)), 2),'%%) \n'])
fprintf(['\nY-axis: ', num2str(theta_p(5)), '\n\t Error: ', ...
    num2str(err(5)),' (', num2str(abs(err(5)*100/theta(5)), 2),'%%) \n'])
fprintf(['\nZ-axis: ', num2str(theta_p(6)), '\n\t Error: ', ...
    num2str(err(6)),' (', num2str(abs(err(6)*100/theta(6)), 2),'%%) \n'])
fprintf(['\nMSE: ', num2str((MSE_g)),' \n'])

create_meas.m

function [ax, ay, az] = create_meas(mx, my, mz, gx, gy, gz, stddev, meas)
    % Create measurements on each side of the sensor:
    % x, y and z axises (six measurement positions).
    A = [ ones(meas,1)   zeros(meas,1)  zeros(meas,1);
         -ones(meas,1)   zeros(meas,1)  zeros(meas,1);
          zeros(meas,1)  ones(meas,1)   zeros(meas,1);
          zeros(meas,1) -ones(meas,1)   zeros(meas,1);
          zeros(meas,1)  zeros(meas,1)  ones(meas,1)
          zeros(meas,1)  zeros(meas,1) -ones(meas,1)];
    
    % Multiply with the gain to get "sensor accurate data"
    A(:,1) = A(:,1)*gx;
    A(:,2) = A(:,2)*gy;
    A(:,3) = A(:,3)*gz;
    
    % Add the biases of each axis
    A(:,1) = A(:,1) + mx;
    A(:,2) = A(:,2) + my;
    A(:,3) = A(:,3) + mz;
    
    s = size(A);
    
    R = normrnd(0, stddev, s(1), s(2));
    A = A + R;
    
    ax = round(A(:,1));
    ay = round(A(:,2));
    az = round(A(:,3));
end

nonlin_solve.m

function theta = nonlin_solve(x, y, z, bx, by, bz, gx, gy, gz)
    % Formula for Gauss-Newton solver:
    % theta(k+1) = theta(k) + inv(H'*H)*H'*[y - yHAT(theta_k)]
    
    theta = [bx;
             by;
             bz;
             gx;
             gy;
             gz];
    
         
    for i = 1:100
        H = gradH(x, y, z, theta(1), theta(2), theta(3), ...
                  theta(4), theta(5), theta(6));

        yH = yHAT(x, y, z, theta(1), theta(2), theta(3), ...
                  theta(4), theta(5), theta(6));
              
        hTh = (H'*H);
        step = hTh\H'*(1 - yH);
        theta = theta + step;
        
        if max(abs(step)) < 1e-7
            fprintf(['Loop aborted at n = ', num2str(i), ...
                     ' of 100, requested precision reached!\n\n'])
            break
        end
    end
end

yHAT.m

function a = yHAT(ax, ay, az, mx, my, mz, gx, gy, gz)
    % Calculates the acceleration
    a = (ax - mx).^2./gx.^2 + (ay - my).^2./gy.^2 + (az - mz).^2./gz.^2;
end

gradH.m

function H = gradH(ax, ay, az, mx, my, mz, gx, gy, gz)
    % Derivative of a = (ax - mx)^2/gx^2 + (ay - my)^2/gy^2 + (az - mz)^2/gz^2
    % = d/dtheta (ax - mx)^2/gx^2 + (ay - my)^2/gy^2 + (az - mz)^2/gz^2 = 
    % = [ -(2*ax - 2*mx)/gx^2, -(2*ay - 2*my)/gy^2, -(2*az - 2*mz)/gz^2,
    % -(2*(ax - mx)^2)/gx^3, -(2*(ay - my)^2)/gy^3, -(2*(az - mz)^2)/gz^3]
    
    % H = [da/dmx da/dmy da/dmz da/dgx da/dgy da/dgz]
    
    H = -2.*[(ax - mx)./gx.^2,     (ay - my)./gy.^2,    ...
             (az - mz)./gz.^2,     ((ax - mx).^2)./gx.^3,  ...
             ((ay - my).^2)./gy.^3,  ((az - mz).^2)./gz.^3];
end