58 lines
1.4 KiB
Matlab
58 lines
1.4 KiB
Matlab
%% A new calibration method for tri-axial field sensors in strap-down navigation systems µã»ý²»±ä·¨
|
|
% Accelerometer: acc is sensor frame, n= number of obs, m = 3(x y z)
|
|
% Magnetometer: raw mag in sensor frame
|
|
% A: mag misalignment matrix
|
|
% b: mag bias
|
|
|
|
function [mis bias] = dpi (acc, mag, dot_product)
|
|
|
|
nbos = length(acc);
|
|
|
|
% x = l11 l12 l13 l21 l22 l23 l31 l32 l33 b1 b2 b3
|
|
A(:,1) = acc(:,1).* mag(:,1);
|
|
A(:,2) = acc(:,1).* mag(:,2);
|
|
A(:,3) = acc(:,1).* mag(:,3);
|
|
A(:,4) = acc(:,2).* mag(:,1);
|
|
A(:,5) = acc(:,2).* mag(:,2);
|
|
A(:,6) = acc(:,2).* mag(:,3);
|
|
A(:,7) = acc(:,3).* mag(:,1);
|
|
A(:,8) = acc(:,3).* mag(:,2);
|
|
A(:,9) = acc(:,3).* mag(:,3);
|
|
A(:,10) = -acc(:,1);
|
|
A(:,11) = -acc(:,2);
|
|
A(:,12) = -acc(:,3);
|
|
|
|
%Y = ones(length(gB),1) * dot(gmOb, gReference);
|
|
Y = ones(nbos,1) * dot_product;
|
|
B = inv(A'*A)*A'*Y;
|
|
% or you can use lsqlin(A,Y)
|
|
|
|
mis = [B(1:3)'; B(4:6)'; B(7:9)'];
|
|
bias = [B(10) B(11) B(12)];
|
|
end
|
|
|
|
%% DPI module
|
|
% syms l11 l12 l13 l21 l22 l23 l31 l32 l33 real
|
|
% syms b1 b2 b3 real
|
|
% syms g1 g2 g3 real
|
|
% syms v1 v2 v3 real
|
|
% syms CONST real
|
|
|
|
% b: bias
|
|
% l11 - l33: mis align
|
|
% v magnormetor reading
|
|
% g graivity
|
|
|
|
% model
|
|
% L = sym('l%d%d', [3 3]);
|
|
% B = [b1 b2 b3]';
|
|
% V = [v1 v2 v3]';
|
|
% G = [g1 g2 g3]';
|
|
% G'*(L*V - B) - CONST
|
|
% %G'*(V - B) - CONST
|
|
%
|
|
% collect(ans, [L B])
|
|
|
|
|
|
|