-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathvelocityCalc.m
148 lines (121 loc) · 5.39 KB
/
velocityCalc.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
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
clear; clc;
time = [0 2 4 6 8 10]; % Time values
latitude = [39.132844 39.132662 39.132353 39.132076 39.132001 39.131964]; % Latitude values
longitude = [-84.516888 -84.516955 -84.516878 -84.516945 -84.517047 -84.517095]; %Longitude Values
% Initialize vectors for distance and position
distance_values = zeros(1,length(time)); position_values = zeros(1,length(time));
% Calculate Distance Values
for n = 2:length(time)
distance_values(n) = 222240*asind(sqrt((sind((latitude(n)-latitude(n-1))/2)^2) + cosd(latitude(n-1))*cosd(latitude(n))*(sind((longitude(n)-longitude(n-1))/2)^2)));
% Calculate Position Values
position_values(n) = sum(distance_values);
end
delta_t = time(2)-time(1); %Initialize delta for time values
f_prime_f = zeros(1,length(time)); %Initialize vector for first derivative
% Calculate First Derivate (Velocity)
for n = 2:length(time);
f_prime_f(n) = (position_values(n)-position_values(n-1))/delta_t;
end
f_prime_f_three_point = zeros(1,length(time)); % Initialize vector for 3-pt derivative
% Calculate 3-pt derivative
for n = 2:length(time)-1;
f_prime_f_three_point(n) = (position_values(n+1)-position_values(n-1))/(2*delta_t);
end
f_double_prime_f = zeros(1,length(time)); % Initialize vector for second derivative (acceleration)
% Calculate second derivative (acceleration)
for n = 2:length(time)-1;
f_double_prime_f(n) = (position_values(n+1)-2*position_values(n)+position_values(n-1))/(delta_t^2);
end
%%
% , Lab 2, Problem B
clear; clc;
load GPS_Data1;
% load GPS_Data2;
time = zeros(1,7); % Initialize usable time vector
% Converting time values into seconds
% Forget the hours, since the hour is always 9, therfore no change
for n = 1:length(minute)
time(n) = (((minute(n)*60)+(second(n))) - 840);
end
distance_values = zeros(1,length(time));
position_values = zeros(1,length(time));
% Calculate distance vectors
for n = 2:length(time)
distance_values(n) = 222240*asind(sqrt((sind((latitude(n)-latitude(n-1))/2)^2) + cosd(latitude(n-1))*cosd(latitude(n))*(sind((longitude(n)-longitude(n-1))/2)^2)));
% Calculate Position Values
position_values(n) = sum(distance_values);
end
% Calculate delta_T for selected data set
delta_t = time(2)-time(1);
% Vector of time already exists previously in code, copied again below:
% % Converting time values into seconds
% % Forget the hours, since the hour is always 9, therfore no change
% % for n = 1:length(minute)
% % time(n) = (((minute(n)*60)+(second(n))) - 840);
% % end
f_prime_f = zeros(1,length(time)); % Initialize vector for first derivative (velocity)
% Calculate First Derivate (Velocity)
for n = 2:length(time);
f_prime_f(n) = (position_values(n)-position_values(n-1))/delta_t;
end
% Initialize vector for absolute error (calculated/actual velocity)
abs_error_velocity = zeros(1,length(time));
% Calculate absolute error for calculated velocity vs. actual velocity
for n = 1:length(time)
abs_error_velocity(n) = abs(f_prime_f(n)-actual_velocity(n));
end
f_prime_f_three_point = zeros(1,length(time)); % Initialize vector for 3-pt derivative
% Calculate 3-pt derivative
for n = 2:length(time)-1;
f_prime_f_three_point(n) = (position_values(n+1)-position_values(n-1))/(2*delta_t);
end
% Initialize vector for absolute error (calculated/actual 3-pt velocity)
abs_error_3pt_velocity = zeros(1,length(time));
% Calculate absolute error for calculated 3-pt velocity vs. actual velocity
for n = 1:length(time)
abs_error_3pt_velocity(n) = abs(f_prime_f_three_point(n)-actual_velocity(n));
end
% Plotting Velocity Graphs
figure;
subplot(2,1,1);
plot(time,f_prime_f,'r*'); hold on;
plot(time,actual_velocity,'k-'); hold on;
plot(time,f_prime_f_three_point,'b-');
title('Estimated Velocity'); xlabel('time (sec)'); ylabel('Velocity (m/s)');
legend('Estimated Velocity','Actual Velocity','Estimated 3pt Velocity');
subplot(2,1,2);
plot(time,abs_error_velocity,'r*'); hold on;
plot(time,abs_error_3pt_velocity,'b*');
title('Velocity Error'); xlabel('time (sec)'); ylabel('Absolute Error (m/s)');
legend('Estimated Velocity Error','3pt Velocity Error');
% Initialize vector for second derivate
f_doubleprime_f = zeros(1,length(time));
% Calculate second derivative (acceleration)
for n = 2:length(time)-1;
f_doubleprime_f(n) = (position_values(n+1)-2*position_values(n)+position_values(n-1))/(delta_t^2);
end
% Initialize vector for absolute error (doubleprime/actual acceleration)
abs_error_acceleration = zeros(1,length(time)-1);
% Calculate absolute error of second derivative
for n = 1:length(time)-1;
abs_error_acceleration(n) = abs(f_doubleprime_f(n)-actual_acceleration(n));
end
% Initialize special time vector for actual_acceleration
time_acceleration = time;
% Fixing time vector for actual_acceleration
for n = 1:length(time)
if n == length(time)
time_acceleration(n) = [];
f_doubleprime_f(n) = [];
end
end
% Plotting Acceleration Graphs
figure;
subplot(2,1,1);
plot(time_acceleration,f_doubleprime_f,'k-'); hold on;
plot(time_acceleration,actual_acceleration,'r*')
title('Estimated Acceleration'); xlabel('time (sec)'); ylabel('Acceleration (m/s^2)');
legend('Actual Acceleration','Estimated Acceleration');
subplot(2,1,2);
plot(time_acceleration,abs_error_acceleration,'r*');
title('Estimated Acceleration Error'); xlabel('time(sec)'); ylabel('Absolute Error (m/s)');