function [M,B] = calibaccel(data)
%-------------------------------------------------------------------------------
% Computes the Scale factor Matrix and the bias vector of a MEMS accelerometer.
% The procedure exploits the fact that, in static conditions, the
% modulus of the accelerometer output vector matches that of the
% gravity acceleration. The calibration model incorporates the bias
% and scale factor for each axis and the cross-axis symmetrical
% factors. The parameters are computed through Gauss-Newton nonlinear optimization.
% The mathematical model used is A = M(V - B)
% where M and B are scale factor matrix and bias vector respectively.
% M = [ Mxx Mxy Mxz; Myx Myy Myz; Mzx Mzy Mzz ]; where Mxy = Myx; Myz = Mzy; Mxz = Mzx;
% B = [ Bx; By; Bz ];
% The diagonal elements of M represent the scale factors along the
% three axes, whereas the other elements of M are called cross-axis
% factors. These terms allow describing both the axes misalignment
% and the crosstalk effect between different channels caused
% by the sensor electronics. In an ideal world, M = 1; B = 0
%-------------------------------------------------------------------------------
% Form:
% [M,B] = calibaccel( data )
%-------------------------------------------------------------------------------
% ------
% Inputs
% ------
% data (9,3) columns ax, ay and az acceleration values obtained from the accelerometer represented in units of g
% 9 rows represent data from 9 different static positions.
%
% To convert raw measurements to units of g use the following formula:
% Vx_g = (Vx - Zerox)* Sensitivity_x
% Vy_g = (Vy - Zeroy)* Sensitivity_y
% Vz_g = (Vz - Zeroz)* Sensitivity_z
% For LIS3L02AL (Wii Nunchuck) accelerometer the following values are
% valid:
% Zerox = 505; Zeroy = 517; Zeroz = 549
% Sensitivity_x = 0.004673 g/mV
% Sensitivity_y = 0.004762 g/mV
% Sensitivity_z = 0.004425 g/mV
%
% To find the Zeroxyz values of your own accelerometer, note the max and
% minimum of the ADC values for each axis and use the following formula:
% Zerox = (Max_x - Min_x)/2;Zeroy = (Max_y - Min_y)/2;Zeroz = (Max_z - Min_z)/2
% To find the Sensitivity use the following formula:
% Sensitivity_x = n/(Max_x-Min_x); where n is the max g resolution of your accelerometer: typically n = 2
%
% -------
% Outputs
% -------
% M (3,3) Scale Vector Matrix
% B (3,1) Bias Vector
%-------------------------------------------------------------------------------
% Reference: Iuri Frosio, Federico Pedersini, N. Alberto Borghese
% "Autocalibration of MEMS Accelerometers"
% IEEE TRANSACTIONS ON INSTRUMENTATION AND MEASUREMENT, VOL. 58, NO. 6, JUNE 2009
%-------------------------------------------------------------------------------
% Author: Balaji Kumar, Sep 2011, Canada
% Free to distribute
%-------------------------------------------------------------------------------
clc
% Configurable variables
lambda = 1; % Damping Gain - Start with 1
kl = 0.01; % Damping paremeter - has to be less than 1. Changing this will affect rate of convergence ; Recommend to use k1 between 0.01 - 0.05
tol = 1e-9; % Convergence criterion threshold
Rold = 100000; % Better to leave this No. big.
itr = 200; % No. Of iterations. If your solutions don't converge then try increasing this. Typically it should converge within 20 iterations
% Initial Guess values of M and B. Change this only if you need to
Mxx0 = 5;
Mxy0 = 0.5;
Mxz0 = 0.5;
Myy0 = 5;
Myz0 = 0.5;
Mzz0 = 5;
Bx0 = 0.5;
By0 = 0.5;
Bz0 = 0.5;
% Actual Algorithm
load ('data.txt');
V = data;
[r, c] = size(V);
if r < 9
disp('Need atleast 9 Measurements for the calibration procedure!')
return
end
if c ~= 3
disp('Not enough columns in the data')
return
end
% f is the error function given by Ax^2+Ay^2+Az^2 - g, where g = 1
f = inline('(Mxx*(Bx - Vx) + Mxy*(By - Vy) + Mxz*(Bz - Vz))^2 + (Mxy*(Bx - Vx) + Myy*(By - Vy) + Myz*(Bz - Vz))^2 + (Mxz*(Bx - Vx) + Myz*(By - Vy) + Mzz*(Bz - Vz))^2-1','Vx','Vy','Vz','Mxx','Mxy','Mxz','Myy','Myz','Mzz','Bx','By','Bz');
% Functions f1 to f9 are the elements of the Jacobian vector (partial
% derivatives of the error function with respect to the gain and bias
% components)
f1 = inline('2*(Bx - Vx)*(Mxx*(Bx - Vx) + Mxy*(By - Vy) + Mxz*(Bz - Vz))','Vx','Vy','Vz','Mxx','Mxy','Mxz','Myy','Bx','By','Bz');
f2 = inline('2*(By - Vy)*(Mxx*(Bx - Vx) + Mxy*(By - Vy) + Mxz*(Bz - Vz)) + 2*(Bx - Vx)*(Mxy*(Bx - Vx) + Myy*(By - Vy) + Myz*(Bz - Vz))','Vx','Vy','Vz','Mxx','Mxy','Mxz','Myy','Myz','Bx','By','Bz' );
f3 = inline('2*(Bx - Vx)*(Mxz*(Bx - Vx) + Myz*(By - Vy) + Mzz*(Bz - Vz)) + 2*(Bz - Vz)*(Mxx*(Bx - Vx) + Mxy*(By - Vy) + Mxz*(Bz - Vz))','Vx','Vy','Vz','Mxx','Mxy','Mxz','Myz','Mzz','Bx','By','Bz');
f4 = inline('2*(By - Vy)*(Mxy*(Bx - Vx) + Myy*(By - Vy) + Myz*(Bz - Vz))','Vx','Vy','Vz','Mxy','Myy','Myz','Bx','By','Bz');
f5= inline('2*(By - Vy)*(Mxz*(Bx - Vx) + Myz*(By - Vy) + Mzz*(Bz - Vz)) + 2*(Bz - Vz)*(Mxy*(Bx - Vx) + Myy*(By - Vy) + Myz*(Bz - Vz))','Vx','Vy','Vz','Mxy','Mxz','Myy','Myz','Mzz','Bx','By','Bz');
f6= inline('2*(Bz - Vz)*(Mxz*(Bx - Vx) + Myz*(By - Vy) + Mzz*(Bz - Vz))','Vx','Vy','Vz','Mxz','Myz','Mzz','Bx','By','Bz');
f7 = inline('2*Mxx*(Mxx*(Bx - Vx) + Mxy*(By - Vy) + Mxz*(Bz - Vz)) + 2*Mxy*(Mxy*(Bx - Vx) + Myy*(By - Vy) + Myz*(Bz - Vz)) + 2*Mxz*(Mxz*(Bx - Vx) + Myz*(By - Vy) + Mzz*(Bz - Vz))','Vx','Vy','Vz','Mxx','Mxy','Mxz','Myy','Myz','Mzz','Bx','By','Bz');
f8 = inline('2*Mxy*(Mxx*(Bx - Vx) + Mxy*(By - Vy) + Mxz*(Bz - Vz)) + 2*Myy*(Mxy*(Bx - Vx) + Myy*(By - Vy) + Myz*(Bz - Vz)) + 2*Myz*(Mxz*(Bx - Vx) + Myz*(By - Vy) + Mzz*(Bz - Vz))','Vx','Vy','Vz','Mxx','Mxy','Mxz','Myy','Myz','Mzz','Bx','By','Bz');
f9 = inline('2*Mxz*(Mxx*(Bx - Vx) + Mxy*(By - Vy) + Mxz*(Bz - Vz)) + 2*Myz*(Mxy*(Bx - Vx) + Myy*(By - Vy) + Myz*(Bz - Vz)) + 2*Mzz*(Mxz*(Bx - Vx) + Myz*(By - Vy) + Mzz*(Bz - Vz))','Vx','Vy','Vz','Mxx','Mxy','Mxz','Myy','Myz','Mzz','Bx','By','Bz');
Vx = V(:,1);
Vy = V(:,2);
Vz = V(:,3);
m = length(V);
R = zeros(m, 1);
J = zeros(m, 2);
v = [Mxx0, Mxy0, Mxz0, Myy0, Myz0, Mzz0, Bx0, By0, Bz0]';
for n=0:itr % iterate
% Calculate the Jacobian at every iteration
for i=1:length(Vx)
R(i) = f (Vx(i), Vy(i), Vz(i), Mxx0,Mxy0,Mxz0,Myy0,Myz0,Mzz0,Bx0,By0,Bz0);
J(i, 1) = f1 (Vx(i),Vy(i),Vz(i),Mxx0,Mxy0,Mxz0,Myy0,Bx0,By0,Bz0);
J(i, 2) = f2(Vx(i),Vy(i),Vz(i),Mxx0,Mxy0,Mxz0,Myy0,Myz0,Bx0,By0,Bz0);
J(i, 3) = f3(Vx(i),Vy(i),Vz(i),Mxx0,Mxy0,Mxz0,Myz0,Mzz0,Bx0,By0,Bz0);
J(i, 4) = f4(Vx(i),Vy(i),Vz(i),Mxy0,Myy0,Myz0,Bx0,By0,Bz0);
J(i, 5) = f5(Vx(i),Vy(i),Vz(i),Mxy0,Mxz0,Myy0,Myz0,Mzz0,Bx0,By0,Bz0);
J(i, 6) = f6(Vx(i),Vy(i),Vz(i),Mxz0,Myz0,Mzz0,Bx0,By0,Bz0);
J(i, 7) = f7(Vx(i), Vy(i), Vz(i), Mxx0,Mxy0,Mxz0,Myy0,Myz0,Mzz0,Bx0,By0,Bz0);
J(i, 8) = f8(Vx(i), Vy(i), Vz(i), Mxx0,Mxy0,Mxz0,Myy0,Myz0,Mzz0,Bx0,By0,Bz0);
J(i, 9) = f9(Vx(i), Vy(i), Vz(i), Mxx0,Mxy0,Mxz0,Myy0,Myz0,Mzz0,Bx0,By0,Bz0);
end
Rnew = norm(R);
H = inv(J'*J); % Hessian matrix
D = (J'*R)';
v = v - lambda*(D*H)';
disp(sprintf('%d %0.9g %0.9g %0.9g %0.9g %0.9g %0.9g %0.9g %0.9g %0.9g % 0.9g ', n, v(1), v(2), v(3), v(4), v(5), v(6), v(7), v(8), v(9), norm(R)));
% This is to make sure that the error is decereasing with every
% iteration
if (Rnew <= Rold)
lambda = lambda-kl*lambda;
else
lambda = kl*lambda;
end
% Iterations are stopped when the following convergence criteria is
% satisfied
if (n>1)
disp(sprintf('%d', abs(max(2*(v-vold)/(v+vold)))));
if (abs(max(2*(v-vold)/(v+vold))) <= tol)
disp('Convergence achieved');
break;
end
end
Mxx0 = v(1);
Mxy0 = v(2);
Mxz0 = v(3);
Myy0 = v(4);
Myz0 = v(5);
Mzz0 = v(6);
Bx0 = v(7);
By0 = v(8);
Bz0 = v(9);
vold = v;
Rold = Rnew;
end
% Save Outputs
M = [v(1) v(2) v(3); v(2) v(4) v(5); v(3) v(5) v(6)];
B = [v(7);v(8);v(9)];
save M.mat;
save B.mat;
clear all