98 lines
2.1 KiB
Matlab
98 lines
2.1 KiB
Matlab
|
% using DIP mehold, only compute bias, norm_h, lamda(cos(inclination)),
|
||
|
|
% modifed from DIP13
|
||
|
|
|
||
|
|
% %% moudle DIP5
|
||
|
|
% syms b1 b2 b3 real
|
||
|
|
% syms g1 g2 g3 real
|
||
|
|
% syms v1 v2 v3 real
|
||
|
|
% syms lambda real
|
||
|
|
% syms norm_h norm_g real
|
||
|
|
% B = [b1 b2 b3]';
|
||
|
|
% V = [v1 v2 v3]';
|
||
|
|
% G = [g1 g2 g3]';
|
||
|
|
% % %G'*(V - B) - CONST
|
||
|
|
%
|
||
|
|
% f1= (V - B)' * (V - B) - norm_h^2;
|
||
|
|
% j1 = jacobian(f1, [B' norm_h lambda]);
|
||
|
|
% collect(j1, B) ;
|
||
|
|
%
|
||
|
|
% f2 = G' * (V - B) - norm_g * norm_h * lambda;
|
||
|
|
%
|
||
|
|
% f = f1 + f2;
|
||
|
|
% j2 = jacobian(f2, [B' norm_h lambda]);
|
||
|
|
% collect(j2, B);
|
||
|
|
%
|
||
|
|
%
|
||
|
|
% f1
|
||
|
|
% j1
|
||
|
|
%
|
||
|
|
% f2
|
||
|
|
% j2
|
||
|
|
|
||
|
|
function [mis, bias, norm_h, lamda, inter, J] = dip5 (acc, mag, x, epsilon)
|
||
|
|
% initalize value
|
||
|
|
mis = eye(3);
|
||
|
|
bias = zeros(1,3);
|
||
|
|
lamda = cos(deg2rad(60));
|
||
|
|
|
||
|
|
mu = 0.000; % damp
|
||
|
|
inter = 100;
|
||
|
|
last_e = 100;
|
||
|
|
|
||
|
|
for i = 1:inter
|
||
|
|
[J(i), Jacobi, e] = compute_cost_and_jacob (x, mag, acc);
|
||
|
|
x = x - (inv((Jacobi' * Jacobi + mu * eye(length(x)))) * Jacobi' * e)';
|
||
|
|
|
||
|
|
if((abs(norm(e) - norm(last_e))) < epsilon)
|
||
|
|
bias = x(1:3)';
|
||
|
|
lamda = x(5);
|
||
|
|
inter = i;
|
||
|
|
norm_h = x(4);
|
||
|
|
break;
|
||
|
|
end
|
||
|
|
last_e = e;
|
||
|
|
end
|
||
|
|
|
||
|
|
end
|
||
|
|
|
||
|
|
%% Function
|
||
|
|
% residual: error
|
||
|
|
% J : gradient of cost func
|
||
|
|
% e: bx by bz lamada
|
||
|
|
% X: [Bx By Bz norm_h lambada]
|
||
|
|
|
||
|
|
function[residual, J, e]=compute_cost_and_jacob(x, mB, gB)
|
||
|
|
n = length(mB);
|
||
|
|
e = zeros(n * 2, 1);
|
||
|
|
J = zeros(n * 2, 5);
|
||
|
|
b = x(1:3)';
|
||
|
|
norm_h = x(4);
|
||
|
|
lamda = x(5);
|
||
|
|
|
||
|
|
for i = 1:n
|
||
|
|
v = mB(i, :)';
|
||
|
|
g = gB(i , :)';
|
||
|
|
|
||
|
|
% caluate e
|
||
|
|
e(i , :) = (v - b)' * (v - b) - norm_h^2;
|
||
|
|
e(n+ i, :) = g' * (v - b) - (norm_h * norm(g) * lamda);
|
||
|
|
|
||
|
|
% caluate J 1- r
|
||
|
|
J(i, 1) = 2 * (b(1) - v(1));
|
||
|
|
J(i, 2) = 2 * (b(2) - v(2));
|
||
|
|
J(i, 3) = 2 * (b(3) - v(3));
|
||
|
|
J(i, 4) = -2 * norm_h;
|
||
|
|
J(i, 5) = 0;
|
||
|
|
|
||
|
|
% caluate J r- 2r
|
||
|
|
J(n + i, 1) = -g(1);
|
||
|
|
J(n + i, 2) = -g(2);
|
||
|
|
J(n + i, 3) = -g(3);
|
||
|
|
J(n + i, 4) = -lamda * norm(g);
|
||
|
|
J(n + i, 5) = -norm_h * norm(g);
|
||
|
|
end
|
||
|
|
|
||
|
|
residual = e' * e;
|
||
|
|
end
|
||
|
|
|
||
|
|
|