138 lines
4.2 KiB
Matlab
138 lines
4.2 KiB
Matlab
|
% syms b1 b2 b3 real
|
|||
|
|
% syms g1 g2 g3 real
|
|||
|
|
% syms v1 v2 v3 real
|
|||
|
|
% syms lambda real
|
|||
|
|
% syms h
|
|||
|
|
% syms l11 l12 l13 l21 l22 l23 l31 l32 l33 real
|
|||
|
|
% L = [l11 l23 l13; l21 l22 l23; l31 l32 l33];
|
|||
|
|
% B = [b1 b2 b3]';
|
|||
|
|
% V = [v1 v2 v3]';
|
|||
|
|
% G = [g1 g2 g3]';
|
|||
|
|
%
|
|||
|
|
% f = V'*L'*L*V - 2*V'*L'*L*B + B'*L'*L*B - h^2
|
|||
|
|
% j = jacobian(f, [l11 l12 l13 l21 l22 l23 l31 l32 l33 b1 b2 b3 lamda]);
|
|||
|
|
% collect(j(1,1), [l11])
|
|||
|
|
%
|
|||
|
|
% alp = L*(V - B);
|
|||
|
|
% beta = V-B;
|
|||
|
|
% 2*alp(1)*beta(1);
|
|||
|
|
% collect(ans,[l11])
|
|||
|
|
|
|||
|
|
function [mis, bias, lamda, inter, residual] = dip13 (acc, mag, mag_norm, epsilon)
|
|||
|
|
%<25><><EFBFBD><EFBFBD><EFBFBD>Ĺ<EFBFBD><C4B9>ܣ<EFBFBD>using DIP(dual inner product mehold to caluate mag)
|
|||
|
|
%<25><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> Calibration of tri-axial magnetometer using vector observations and inner products or Calibration and Alignment of Tri-Axial Magnetometers for Attitude Determination
|
|||
|
|
%<25><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʹ<EFBFBD>ã<EFBFBD>[mis, bias, lamda, inter, J] = dip (acc, mag, mag_norm, epsilon)
|
|||
|
|
%<25><><EFBFBD>룺
|
|||
|
|
% input1: acc: raw acc
|
|||
|
|
% input2: mag: raw mag
|
|||
|
|
|
|||
|
|
%<25><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
% mis: misalign matrix
|
|||
|
|
% bias: bias
|
|||
|
|
% lamda: |reference_vector| * |mag_vector| * cos(theta)
|
|||
|
|
% inter: interation
|
|||
|
|
% J: cost function array
|
|||
|
|
|
|||
|
|
mis = eye(3);
|
|||
|
|
bias = zeros(1,3);
|
|||
|
|
lamda = cos(deg2rad(60));
|
|||
|
|
|
|||
|
|
mu = 0.001; % damp
|
|||
|
|
inter = 100;
|
|||
|
|
last_e = 100;
|
|||
|
|
last_J = 100;
|
|||
|
|
|
|||
|
|
% [l11 l12 l13 l21 l22 l23 l31 l32 l33 b1 b2 b3 lamda], lamba = cos(inclincation)
|
|||
|
|
x = [1 0 0 0 1 0 0 0 1 0 0 0 1];
|
|||
|
|
|
|||
|
|
% using max-min mehold to get a inital bias
|
|||
|
|
x(10:12) = (max(mag) + min(mag)) / 2;
|
|||
|
|
|
|||
|
|
[last_J, ~, ~] = lm_dip(x, mag, acc, mag_norm);
|
|||
|
|
|
|||
|
|
for i = 1:inter
|
|||
|
|
[residual(i), Jacobi, e] = lm_dip(x, mag, acc, mag_norm);
|
|||
|
|
x = x - (inv((Jacobi' * Jacobi + mu * eye(length(x)))) * Jacobi' * e)';
|
|||
|
|
%x = x - mu*(Jacobi' * e)';
|
|||
|
|
|
|||
|
|
if(residual(i) <= last_J)
|
|||
|
|
mu = 0.1 * mu;
|
|||
|
|
else
|
|||
|
|
mu = 10 * mu;
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
if((abs(norm(e) - norm(last_e))) < epsilon)
|
|||
|
|
mis =[x(1:3); x(4:6); x(7:9)];
|
|||
|
|
bias = x(10:12)';
|
|||
|
|
lamda = x(13);
|
|||
|
|
inter = i;
|
|||
|
|
break;
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
last_e = e;
|
|||
|
|
last_J = residual(i);
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
%% Function
|
|||
|
|
% Jval: error
|
|||
|
|
% gradient : gradient of cost func
|
|||
|
|
% e: Fx
|
|||
|
|
|
|||
|
|
function[residual, gradient, e]=lm_dip(x, mB, gB, h)
|
|||
|
|
n = length(mB);
|
|||
|
|
e = zeros(n*2, 1);
|
|||
|
|
gradient = zeros(n*2, 13);
|
|||
|
|
L =[x(1:3); x(4:6); x(7:9)];
|
|||
|
|
b = x(10:12)';
|
|||
|
|
for i = 1:n
|
|||
|
|
|
|||
|
|
v = mB(i, :)';
|
|||
|
|
g = gB(i , :)';
|
|||
|
|
|
|||
|
|
% caluate e
|
|||
|
|
e(i , :) = v'*L'*L*v -2*v'*L'*L*b + b'*L'*L*b - h^2;
|
|||
|
|
% e(i , :) = (L*(v - b))' * (L*(v - b)) - h^2;
|
|||
|
|
% e(n+ i, :) = g'*L*v - g'*L*b - h*norm(g)*x(13);
|
|||
|
|
e(n+ i, :) = g'*L*(v-b) - h*norm(g)*x(13);
|
|||
|
|
|
|||
|
|
alpa = L * (v - b);
|
|||
|
|
beta = v - b;
|
|||
|
|
|
|||
|
|
% caluate J 1- r
|
|||
|
|
gradient(i, 1) = 2 * alpa(1) * beta(1);
|
|||
|
|
gradient(i, 2) = 2 * alpa(1) * beta(2);
|
|||
|
|
gradient(i, 3) = 2 * alpa(1) * beta(3);
|
|||
|
|
gradient(i, 4) = 2 * alpa(2) * beta(1);
|
|||
|
|
gradient(i, 5) = 2 * alpa(2) * beta(2);
|
|||
|
|
gradient(i, 6) = 2 * alpa(2) * beta(3);
|
|||
|
|
gradient(i, 7) = 2 * alpa(3) * beta(1);
|
|||
|
|
gradient(i, 8) = 2 * alpa(3) * beta(2);
|
|||
|
|
gradient(i, 9) = 2 * alpa(3) * beta(3);
|
|||
|
|
|
|||
|
|
gradient(i, 10) = -2 * (alpa(1) * L(1,1) + alpa(2) * L(2,1) + alpa(3) * L(3,1));
|
|||
|
|
gradient(i, 11) = -2 * (alpa(1) * L(1,2) + alpa(2) * L(2,2) + alpa(3) * L(3,2));
|
|||
|
|
gradient(i, 12) = -2 * (alpa(1) * L(1,3) + alpa(2) * L(2,3) + alpa(3) * L(3,3));
|
|||
|
|
gradient(i, 13) = 0;
|
|||
|
|
|
|||
|
|
% caluate J r- 2r
|
|||
|
|
gradient(n + i, 1) = g(1) * beta(1);
|
|||
|
|
gradient(n + i, 2) = g(1) * beta(2);
|
|||
|
|
gradient(n + i, 3) = g(1) * beta(3);
|
|||
|
|
gradient(n + i, 4) = g(2) * beta(1);
|
|||
|
|
gradient(n + i, 5) = g(2) * beta(2);
|
|||
|
|
gradient(n + i, 6) = g(2) * beta(3);
|
|||
|
|
gradient(n + i, 7) = g(3) * beta(1);
|
|||
|
|
gradient(n + i, 8) = g(3) * beta(2);
|
|||
|
|
gradient(n + i, 9) = g(3) * beta(3);
|
|||
|
|
gradient(n + i, 10) = - (g(1) * L(1,1) + g(2) * L(2,1) + g(3) * L(3,1));
|
|||
|
|
gradient(n + i, 11) = - (g(1) * L(1,2) + g(2) * L(2,2) + g(3) * L(3,2));
|
|||
|
|
gradient(n + i, 12) = - (g(1) * L(1,3) + g(2) * L(2,3) + g(3) * L(3,3));
|
|||
|
|
gradient(n + i, 13) = -norm(v) * norm(g);
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
residual = e' * e;
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
|