Designing-INS-GNSS-integrated-navigation-systems-by-using-IPO-algorithms/hd_update.m at main · ali-ece/Designing-INS-GNSS-integrated-navigation-systems-by-using-IPO-algorithms · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
function yawm = hd_update( mag, roll, pitch, D)
% Ali Mohammadi_INS/GNSS

% hd_update: estimates magnetic heading from magnetometer data.
%%

B = [ cos(pitch) sin(pitch)*sin(roll) -cos(roll)*sin(pitch); ...
    0 cos(roll) sin(roll); ];

magh = (B * mag');

yh = magh(2);
xh = magh(1);
atanh = atan2(yh, xh);

if xh < 0

    yawm =  pi - atanh ;

elseif (xh > 0 && yh < 0)

    yawm = -atanh ;

elseif (xh > 0 && yh > 0)

    yawm = 2*pi - atanh;

elseif (xh == 0 && yh < 0)

    yawm = pi/2;

elseif (xh == 0 && yh > 0)

    yawm = (3/2) * pi;

else

    warning('hd_update: default option!') ;
end

% Correct magnetic declination
yawm = yawm + D;

% Yaw must be in the range of [-pi pi]
if yawm < -pi

    yawm = yawm + 2*pi;

elseif yawm > pi

    yawm = yawm - 2*pi;

else
    % dumb
    yawm = yawm;
end