0001 function BS = vb_sensor_mag(Vs,Fs,s0,X,Q)
0002
0003
0004
0005
0006
0007
0008
0009
0010
0011
0012
0013
0014
0015
0016 NS = size(X,1);
0017 NV = size(Vs,1);
0018 NF = size(Fs,1);
0019
0020 BS = zeros(NV,NS);
0021
0022
0023 x1 = Vs(Fs(:,1),:);
0024 x2 = Vs(Fs(:,2),:);
0025 x3 = Vs(Fs(:,3),:);
0026
0027
0028
0029 for i=1:NS,
0030
0031 G0 = Q(i,:)';
0032 XP = X(i,:);
0033 XX = XP(ones(NF,1),:);
0034
0035
0036
0037
0038 y1 = x1 - XX;
0039 y2 = x2 - XX;
0040 y3 = x3 - XX;
0041
0042
0043
0044 ix = find( sum(vb_cross2(y2-y1,y3-y1).*y1, 2) < 0);
0045 tmp = y2(ix,:);
0046 y2(ix,:) = y3(ix,:);
0047 y3(ix,:) = tmp;
0048 Fn = Fs;
0049 Ftemp = Fn(ix,2);
0050 Fn(ix,2) = Fn(ix,3);
0051 Fn(ix,3) = Ftemp;
0052
0053
0054 yy1 = sqrt(sum(y1.^2 ,2));
0055 yy2 = sqrt(sum(y2.^2 ,2));
0056 yy3 = sqrt(sum(y3.^2 ,2));
0057
0058
0059 z1 = vb_cross2(y2,y3);
0060 z2 = vb_cross2(y3,y1);
0061 z3 = vb_cross2(y1,y2);
0062
0063 d = sum(z1.*y1,2);
0064 s = z1+z2+z3;
0065 ss = sqrt(sum(s.^2,2));
0066 n = s./ss(:,ones(1,3));
0067
0068
0069 w1 = y2 - y3;
0070 w2 = y3 - y1;
0071 w3 = y1 - y2;
0072
0073
0074 ww1 = sqrt(sum(w1.^2 ,2));
0075 ww2 = sqrt(sum(w2.^2 ,2));
0076 ww3 = sqrt(sum(w3.^2 ,2));
0077
0078 wy1 = ( ww3.*yy2 - sum(w3.*y2, 2) )./( ww3.*yy1 - sum(w3.*y1, 2) );
0079 wy2 = ( ww1.*yy3 - sum(w1.*y3, 2) )./( ww1.*yy2 - sum(w1.*y2, 2) );
0080 wy3 = ( ww2.*yy1 - sum(w2.*y1, 2) )./( ww2.*yy3 - sum(w2.*y3, 2) );
0081
0082 g1 = log(wy1)./ww3;
0083 g2 = log(wy2)./ww1;
0084 g3 = log(wy3)./ww2;
0085
0086 cc = sum(y1.*n,2);
0087 c = cc(:,ones(1,3)).*n;
0088 c1 = y1-c;
0089 c2 = y2-c;
0090 c3 = y3-c;
0091
0092
0093 omega = 2*atan(d./((yy1.*yy2.*yy3) ...
0094 + yy1.*sum(y2.*y3,2) ...
0095 + yy2.*sum(y3.*y1,2) ...
0096 + yy3.*sum(y1.*y2,2)));
0097
0098 g = sign(sum(s0.*n,2)) ...
0099 .*(g1.*sum(n.*vb_cross2(c1,c2),2) ...
0100 + g2.*sum(n.*vb_cross2(c2,c3),2) ...
0101 + g3.*sum(n.*vb_cross2(c3,c1),2) ...
0102 - sum(n.*c,2).*omega)./ss;
0103
0104
0105 omegaF1 = w1.*g(:,ones(1,3));
0106 omegaF2 = w2.*g(:,ones(1,3));
0107 omegaF3 = w3.*g(:,ones(1,3));
0108
0109 for j = 1:NF
0110 BS(Fn(j,1),i) = BS(Fn(j,1),i)+omegaF1(j,:)*G0;
0111 BS(Fn(j,2),i) = BS(Fn(j,2),i)+omegaF2(j,:)*G0;
0112 BS(Fn(j,3),i) = BS(Fn(j,3),i)+omegaF3(j,:)*G0;
0113 end
0114 end;
0115
0116
0117
0118 BS = BS';