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:
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
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.23388From 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
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