-
Notifications
You must be signed in to change notification settings - Fork 6
/
plot_error_state.m
61 lines (58 loc) · 1.75 KB
/
plot_error_state.m
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
57
58
59
60
61
k = size( pgi1Rec, 2 );
figure();
subplot( 3, 1 ,1 );
hold on; grid on; title(' Position '); ylabel(' m ');
plot( dpgiRec(1,1:k), 'r' );
plot( dpgiRec(2,1:k), 'g' );
plot( dpgiRec(3,1:k), 'b' );
hold off;
subplot( 3, 1 ,2 );
hold on; grid on; title(' Attitude '); ylabel(' deg ');
plot( dthgiRec(1,1:k)*180/pi, 'r' );
plot( dthgiRec(2,1:k)*180/pi, 'g' );
plot( dthgiRec(3,1:k)*180/pi, 'b' );
hold off;
subplot( 3, 1 ,3 );
hold on; grid on; title(' Velocity '); ylabel(' m/s ');
plot( dvgiRec(1,1:k), 'r' );
plot( dvgiRec(2,1:k), 'g' );
plot( dvgiRec(3,1:k), 'b' );
hold off;
figure();
subplot( 2, 1 ,1 );
hold on; grid on; title(' Acc bias '); ylabel(' m/s^2 ');
plot( dbaRec(1,1:k), 'r' );
plot( dbaRec(2,1:k), 'g' );
plot( dbaRec(3,1:k), 'b' );
hold off;
subplot( 2, 1 ,2 );
hold on; grid on; title(' Gyro bias '); ylabel(' deg/s ');
plot( dbgRec(1,1:k)*180/pi, 'r' );
plot( dbgRec(2,1:k)*180/pi, 'g' );
plot( dbgRec(3,1:k)*180/pi, 'b' );
hold off;
figure();
subplot( 4, 1 ,1 );
hold on; grid on; title(' Position '); ylabel(' m ');
plot( dpgi1Rec(1,1:k), 'r' );
plot( dpgi1Rec(2,1:k), 'g' );
plot( dpgi1Rec(3,1:k), 'b' );
hold off;
subplot( 4, 1 ,2 );
hold on; grid on; title(' Attitude '); ylabel(' deg ');
plot( dthgi1Rec(1,1:k)*180/pi, 'r' );
plot( dthgi1Rec(2,1:k)*180/pi, 'g' );
plot( dthgi1Rec(3,1:k)*180/pi, 'b' );
hold off;
subplot( 4, 1 ,3 );
hold on; grid on; title(' Position '); ylabel(' m ');
plot( dpgi2Rec(1,1:k), 'r' );
plot( dpgi2Rec(2,1:k), 'g' );
plot( dpgi2Rec(3,1:k), 'b' );
hold off;
subplot( 4, 1 ,4 );
hold on; grid on; title(' Attitude '); ylabel(' deg ');
plot( dthgi2Rec(1,1:k)*180/pi, 'r' );
plot( dthgi2Rec(2,1:k)*180/pi, 'g' );
plot( dthgi2Rec(3,1:k)*180/pi, 'b' );
hold off;