-
Notifications
You must be signed in to change notification settings - Fork 0
/
ArduinoQuaternion.m
127 lines (96 loc) · 3.37 KB
/
ArduinoQuaternion.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
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
clear; clc; close all
cd C:\Users\Andrei\Desktop\Licenta+Lucru\Personale\KalmanFilter\MatlabTracker
port = serialportlist;
a = arduino(port,'Nano3');
fs = 200;
mpu = mpu6050(a,'SampleRate',fs);
i = 1;
figure
a1 = animatedline('Color','red','linew',3);
a3 = animatedline('Color','black','linew',3);
grid on
legend('Pitch','Location','northoutside','fontsize',14)
ylabel('deg')
movegui(gcf,'west')
figure
a2 = animatedline('Color','green','linew',3);
legend('Roll','Location','northoutside','fontsize',14)
ylabel('deg')
movegui(gcf,'east')
grid on
kalman_filter = QuaternionKalmanFilterModel();
phi = 0;
theta = 0;
psi = 0;
Q = 0.09 * eye(4);
R = 0.09 * eye(4);
P = 9 * eye(4);
q1Std = Q(1,1); q2Std = Q(2,2); q3Std = Q(3,3); q4Std = Q(4,4);
a1Std = R(1,1); a2Std = R(2,2); a3Std = R(3,3); a4Std = R(4,4);
init_q1_std = P(1,1); init_q2_std = P(2,2); init_q3_std = P(3,3); init_q4_std = P(4,4);
init_on_measurement = false;
dt = 1/fs;
Accel_Roll = [];
Accel_Pitch = [];
Kalman_Roll = [];
Kalman_Pitch = [];
Gyro_Roll = [];
Gyro_Pitch = [];
for i=1:500
matr = table2array((read(mpu)));
accel = matr(1:end,1:3);
gyro = matr(1:end,4:6);
acc_x = accel(end,1);
acc_y = accel(end,2);
acc_z = accel(end,3);
gx = rad2deg(gyro(end,2));
gy = rad2deg(gyro(end,1));
gz = rad2deg(gyro(end,3));
gyro_x = gx + gy * tand(theta) * sind(phi) + gz * tand(theta) * cosd(phi);
gyro_y = gy * cosd(phi) - gz * sind(phi);
gyro_z = gz;
omega_body = ([gyro_x; gyro_y; gyro_z]);
acc_total_vector = sqrt((acc_x*acc_x)+(acc_y*acc_y)+(acc_z*acc_z));
angle_Roll_acc = (asind(double(acc_y)/acc_total_vector));
angle_Pitch_acc = -(asind(double(acc_x)/acc_total_vector));
accel_angle = [angle_Roll_acc; angle_Pitch_acc];
% accel_angle = [atan2d(acc_y,acc_z); atan2d(-acc_x, sqrt(acc_y*acc_y + acc_z*acc_z))];
if (i == 1)
kalman_filter.initialise(q1Std, q2Std, q3Std, q4Std, a1Std, a2Std, a3Std, a4Std, init_on_measurement,...
init_q1_std, init_q2_std, init_q3_std, init_q4_std);
Roll = (asin(double(acc_y)/acc_total_vector)); %atan2(acc_y,acc_z);
Pitch = -(asin(double(acc_x)/acc_total_vector)); %atan2(-acc_x, sqrt(acc_y*acc_y + acc_z*acc_z));
predicted_roll = Roll;
predicted_pitch = Pitch;
else
kalman_filter.prediction(omega_body,dt);
predicted_angle = quat2eul(kalman_filter.state');
predicted_roll = rad2deg(predicted_angle(3));
predicted_pitch = rad2deg(predicted_angle(2));
kalman_filter.update(accel_angle);
real_angle = kalman_filter.attitude_state;
phi = (real_angle(1));
theta = (real_angle(2));
psi = (real_angle(3));
Roll = phi;
Pitch = theta;
end
i = i + 1;
addpoints(a1,i,rad2deg(Roll))
addpoints(a2,i,rad2deg(Pitch))
Accel_Roll = [Accel_Roll, accel_angle(1)];
Accel_Pitch = [Accel_Pitch, accel_angle(2)];
Kalman_Roll = [Kalman_Roll, rad2deg(Roll)];
Kalman_Pitch = [Kalman_Pitch, rad2deg(Pitch)];
Gyro_Roll = [Gyro_Roll, predicted_roll];
Gyro_Pitch = [Gyro_Pitch, predicted_pitch];
drawnow limitrate
grid on
end
clearvars -except -regexp ^Accel ^Kalman ^Gyro
cd C:\Users\Andrei\Desktop
if exist('angleData.mat','file')
delete('angleData.mat')
end
warning('off')
save('angleData.mat');