-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathKalman_PID.m
267 lines (223 loc) · 35.2 KB
/
Kalman_PID.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
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
clear all;
close all;
dt = 0.25; % Time step
% Sattelite inertial parameters
m = 4;
w = 0.1;
d = 0.1;
h = 0.3;
Ix = (1/12)*m*(w^2+h^2);
Iy = (1/12)*m*(d^2+h^2);
Iz = (1/12)*m*(w^2+d^2);
I = [Ix, 0, 0; 0, Iy, 0; 0, 0, Iz];
% Initial conditions
P0 = zeros(7);
x0_hat = ones(7,1);
x0_hat(1) = 2.0;
x0_hat(2) = -2.0;
x0_hat(3) = 1.0;
x0_hat(4) = 0.4;
x0_hat(5) = -0.5;
x0_hat(6) = 0.5;
x0_hat(7) = 0.5;
x0_hat(4:7) = x0_hat(4:7) ./ norm(x0_hat(4:7));
x_true = x0_hat;
Pk = P0;
u = [0; 0; 0];
M = u;
% Process noise and covariance matrices
eta = 1E-4;
zeta = 1E-4;
omegaDotVar = (eta/2)^2;
qDotVar = (zeta/2)^2;
procVar(1:3) = omegaDotVar .* ones(3,1);
procVar(4:7) = qDotVar .* ones(4,1);
Q0 = diag(procVar);
H = eye(7);
% Measurement noise
delta = 0.5;
gyroVar = (pi*delta/360)^2;
epsilon = 1E-1;
qVar = (epsilon/2)^2;
obsVar(1:3) = gyroVar .* ones(3,1);
obsVar(4:7) = qVar .* ones(4,1);
R = diag(obsVar);
pred = @prediction;
upd = @update;
new_true = @next_true;
% Functions
Om = @(omega) [0, omega(3), -omega(2), omega(1);
-omega(3), 0, omega(1), omega(2);
omega(2), -omega(1), 0, omega(3);
-omega(1), -omega(2), -omega(3), 0];
quaternion_multiply = @(a,b) [a(1)*b(1)-a(2)*b(2)-a(3)*b(3)-a(4)*b(4);
a(1)*b(2)+a(2)*b(1)+a(3)*b(4)-a(4)*b(3);
a(1)*b(3)-a(2)*b(4)+a(3)*b(2)+a(4)*b(2);
a(1)*b(4)+a(2)*b(3)-a(3)*b(2)+a(4)*b(1)];
quat_to_euler = @(q) [atan2(2*(q(1)*q(2)+q(3)*q(4)),1-2*(q(2)^2+q(3)^2));
asin(2*(q(1)*q(3)-q(4)*q(2)));
atan2(2*(q(1)*q(4)+q(2)*q(3)),1-2*(q(3)^2+q(4)^2))];
euler_to_quat = @(e) [cos(e(1)/2)*cos(e(2)/2)*cos(e(3)/2)+sin(e(1)/2)*sin(e(2)/2)*sin(e(3)/2);
sin(e(1)/2)*cos(e(2)/2)*cos(e(3)/2)-cos(e(1)/2)*sin(e(2)/2)*sin(e(3)/2);
cos(e(1)/2)*sin(e(2)/2)*cos(e(3)/2)+sin(e(1)/2)*cos(e(2)/2)*sin(e(3)/2);
cos(e(1)/2)*cos(e(2)/2)*sin(e(3)/2)-sin(e(1)/2)*sin(e(2)/2)*cos(e(3)/2)];
quat_to_matrix = @(q) [1-2*(q(3)^2+q(4)^2), 2*(q(2)*q(3)-q(4)*q(1)), 2*(q(2)*q(4)+q(3)*q(1));
2*(q(2)*q(3)+q(4)*q(1)), 1-2*(q(2)^2+q(4)^2), 2*(q(3)*q(4)-q(2)*q(1));
2*(q(2)*q(4)-q(3)*q(1)), 2*(q(3)*q(4)+q(2)*q(1)), 1-2*(q(2)^2+q(3)^2)];
euler_to_matrix = @(e) [1,0,0; 0,cos(e(1)),-sin(e(1)); 0,sin(e(1)),cos(e(1))]*[cos(e(2)),0,sin(e(2)); 0,1,0; -sin(e(2)),0,cos(e(2))]*[cos(e(3)),-sin(e(3)),0; sin(e(3)),cos(e(3)),0; 0,0,1];
cal_jaco = @calc_jacobian;
N = 1000; % Number of Time Steps
% Result Data Lists
w_list = zeros(N,3);
q_list = zeros(N,4);
x_true_list = zeros(N,7);
z_list = zeros(N,7);
speed_list = zeros(N,1);
true_speed_list = zeros(N,1);
torque_list = zeros(N,1);
t_list = zeros(N,1);
% Random observation noise
errq = randn*sqrt(qVar);
errw = randn*sqrt(gyroVar);
z = x_true + [errw;errw;errw;errq;errq;errq;errq]; % Initial measurement
xk = z; % Use first measurement as first data point
% PID parameters
K_p = 0.1;
K_i = 0.1;
K_d = 0.08;
SP = [0;0;0]; % Want 0 angular velocity
q = xk(4:7);
att = quat_to_euler(q);
omega = xk(1:3);
PV = omega;
e_p = SP - PV;
e_n = SP - PV;
a = 0;
In = [0; 0; 0];
for k=1:N
% Random process noise
errqDot = randn*sqrt(qDotVar);
errwDot = randn*sqrt(omegaDotVar);
% Update true value with random noise
x_true = new_true(I,dt,x_true,u) + [errwDot;errwDot;errwDot;errqDot;errqDot;errqDot;errqDot];
x_true(4:7) = x_true(4:7)/norm(x_true(4:7)); % Normalize
% Kalman Filter
% Adjust Covariance to prevent numerical instability
if norm(omega) > 1
Q = Q0*norm(omega);
else
Q = Q0;
end
Rot = euler_to_matrix(xk(1:3)*dt); % Matrix representation of the rotation
F = cal_jaco(xk); % Update Jacobian Matrix
% Random observation noise
errq = randn*sqrt(qVar);
errw = randn*sqrt(gyroVar);
z = x_true + [errw;errw;errw;errq;errq;errq;errq]; % Measurement
[xhat_pred, P_pred] = pred(I,dt,Q,xk,u,F,Pk); % Predict next value from previous
[xk,Pk] = update(H, xhat_pred, P_pred, z, R); % Update using measured value
xk(4:7) = xk(4:7)/norm(xk(4:7)); % Renormalize quaternion
I = (Rot)*I0*transpose(Rot); % Update the inertia tensor using the recent rotation
% PID
q = xk(4:7);
att = euler_to_quat(q);
omega = xk(1:3);
PV = omega;
e_p = e_n; % Error at previous time
e_n = SP - PV; % Error at current time
In = In+(1/2)*(e_p+e_n)*dt; % Update integral using trapezoidal method
% Remove integral when passing zero error to reduce overshoot
if e_n(1)*In(1) < 0
In(1) = 0;
end
if e_n(2)*In(2) < 0
In(2) = 0;
end
if e_n(3)*In(3) < 0
In(3) = 0;
end
a = K_p*e_n + K_i*In + K_d*(e_p-e_n)/dt; % Desired angular acceleration
u = (I*a + cross(omega,(I*omega)))/100; % Required torque to achieve desired acceleration
% Store results in data lists
w_list(k,:) = xk(1:3);
q_list(k,:) = xk(4:7);
x_true_list(k,:) = x_true;
z_list(k,:) = z;
torque_list(k) = norm(u);
true_speed_list(k) = norm(x_true(1:3));
speed_list(k) = norm(omega);
t_list(k) = k*dt;
end
% Plot output
plot(t_list,true_speed_list,t_list,speed_list);
title('Speed');
legend('True','Estimate');
figure;
plot(t_list,x_true_list(:,4),t_list,q_list(:,1));
legend('True','Estimate');
title('q1');
figure;
plot(t_list,x_true_list(:,5),t_list,q_list(:,2));
legend('True','Estimate');
title('q2');
figure;
plot(t_list,x_true_list(:,6),t_list,q_list(:,3));
legend('True','Estimate');
title('q3');
figure;
plot(t_list,x_true_list(:,7),t_list,q_list(:,4));
legend('True','Estimate');
title('q4');
figure
plot(t_list,torque_list);
title('Torque');
% Calculate next true value of x
function x = next_true(I,T,x_prev,u_prev)
w_prev = x_prev(1:3);
q_prev = x_prev(4:7);
Omega = [0, x_prev(3), -x_prev(2), x_prev(1);
-x_prev(3), 0, x_prev(1), x_prev(2);
x_prev(2), -x_prev(1), 0, x_prev(3);
-x_prev(1), -x_prev(2), -x_prev(3), 0];
wNext = (T .* ((I^-1)*u_prev - (I^-1 * cross(w_prev, I * w_prev)))) + w_prev;
qNext = expm(0.5 * Omega * T) * q_prev;
x = [wNext; qNext];
end
% Predict next x value from previous
function [xhat_pred, P_pred] = prediction(I,T,Q,xhat_prev,u_prev,F,P)
what_prev = xhat_prev(1:3);
qhat_prev = xhat_prev(4:7);
Omega = [0, xhat_prev(3), -xhat_prev(2), xhat_prev(1);
-xhat_prev(3), 0, xhat_prev(1), xhat_prev(2);
xhat_prev(2), -xhat_prev(1), 0, xhat_prev(3);
-xhat_prev(1), -xhat_prev(2), -xhat_prev(3), 0];
wNext = (T .* ((I^-1)*u_prev - (I^-1 * cross(what_prev, I * what_prev)))) + what_prev;
qNext = expm(0.5 * Omega * T) * qhat_prev;
xhat_pred = [wNext; qNext];
P_pred = F * P * F' + Q;
end
% Update x value using predicted and measured
function [xk, Pk] = update(H, xhat_pred, P_pred, z, R)
y = z - xhat_pred;
S = H * P_pred * H' + R;
K = P_pred * H * S^-1;
xk = xhat_pred + K*y;
Pk = (eye(7) - K*H) * P_pred;
end
% Calculate the jacobian matrix
function j = calc_jacobian(x)
w1 = x(1);
w2 = x(2);
w3 = x(3);
q1 = x(4);
q2 = x(5);
q3 = x(6);
q4 = x(7);
j = [ 1, w3/5, w2/5, 0, 0, 0, 0;
-w3/5, 1, -w1/5, 0, 0, 0, 0;
0, 0, 1, 0, 0, 0, 0;
q1*((w1*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8))/(16*(- w1^2 - w2^2 - w3^2)^(1/2)) - (w1*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8))/(16*(- w1^2 - w2^2 - w3^2)^(1/2))) + (q3*((w1*w2*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2)) + (w1*w2*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2))))/(2*(- w1^2 - w2^2 - w3^2)^(1/2)) - (q2*((w1*w3*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2)) + (w1*w3*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2))))/(2*(- w1^2 - w2^2 - w3^2)^(1/2)) - (q4*(exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8) - exp((- w1^2 - w2^2 - w3^2)^(1/2)/8) + (w1^2*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2)) + (w1^2*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2))))/(2*(- w1^2 - w2^2 - w3^2)^(1/2)) - (q4*w1*(w1*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8) - w1*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8)))/(2*(- w1^2 - w2^2 - w3^2)^(3/2)) + (q3*w1*(w2*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8) - w2*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8)))/(2*(- w1^2 - w2^2 - w3^2)^(3/2)) - (q2*w1*(w3*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8) - w3*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8)))/(2*(- w1^2 - w2^2 - w3^2)^(3/2)), q1*((w2*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8))/(16*(- w1^2 - w2^2 - w3^2)^(1/2)) - (w2*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8))/(16*(- w1^2 - w2^2 - w3^2)^(1/2))) - (q4*((w1*w2*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2)) + (w1*w2*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2))))/(2*(- w1^2 - w2^2 - w3^2)^(1/2)) - (q2*((w2*w3*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2)) + (w2*w3*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2))))/(2*(- w1^2 - w2^2 - w3^2)^(1/2)) + (q3*(exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8) - exp((- w1^2 - w2^2 - w3^2)^(1/2)/8) + (w2^2*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2)) + (w2^2*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2))))/(2*(- w1^2 - w2^2 - w3^2)^(1/2)) - (q4*w2*(w1*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8) - w1*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8)))/(2*(- w1^2 - w2^2 - w3^2)^(3/2)) + (q3*w2*(w2*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8) - w2*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8)))/(2*(- w1^2 - w2^2 - w3^2)^(3/2)) - (q2*w2*(w3*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8) - w3*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8)))/(2*(- w1^2 - w2^2 - w3^2)^(3/2)), q1*((w3*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8))/(16*(- w1^2 - w2^2 - w3^2)^(1/2)) - (w3*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8))/(16*(- w1^2 - w2^2 - w3^2)^(1/2))) - (q4*((w1*w3*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2)) + (w1*w3*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2))))/(2*(- w1^2 - w2^2 - w3^2)^(1/2)) + (q3*((w2*w3*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2)) + (w2*w3*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2))))/(2*(- w1^2 - w2^2 - w3^2)^(1/2)) - (q2*(exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8) - exp((- w1^2 - w2^2 - w3^2)^(1/2)/8) + (w3^2*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2)) + (w3^2*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2))))/(2*(- w1^2 - w2^2 - w3^2)^(1/2)) - (q4*w3*(w1*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8) - w1*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8)))/(2*(- w1^2 - w2^2 - w3^2)^(3/2)) + (q3*w3*(w2*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8) - w2*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8)))/(2*(- w1^2 - w2^2 - w3^2)^(3/2)) - (q2*w3*(w3*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8) - w3*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8)))/(2*(- w1^2 - w2^2 - w3^2)^(3/2)), exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8)/2 + exp((- w1^2 - w2^2 - w3^2)^(1/2)/8)/2, -(w3*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8) - w3*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8))/(2*(- w1^2 - w2^2 - w3^2)^(1/2)), (w2*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8) - w2*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8))/(2*(- w1^2 - w2^2 - w3^2)^(1/2)), -(w1*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8) - w1*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8))/(2*(- w1^2 - w2^2 - w3^2)^(1/2));
q2*((w1*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8))/(16*(- w1^2 - w2^2 - w3^2)^(1/2)) - (w1*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8))/(16*(- w1^2 - w2^2 - w3^2)^(1/2))) + (q1*((w1*w3*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2)) + (w1*w3*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2))))/(2*(- w1^2 - w2^2 - w3^2)^(1/2)) - (q4*((w1*w2*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2)) + (w1*w2*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2))))/(2*(- w1^2 - w2^2 - w3^2)^(1/2)) - (q3*(exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8) - exp((- w1^2 - w2^2 - w3^2)^(1/2)/8) + (w1^2*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2)) + (w1^2*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2))))/(2*(- w1^2 - w2^2 - w3^2)^(1/2)) - (q3*w1*(w1*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8) - w1*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8)))/(2*(- w1^2 - w2^2 - w3^2)^(3/2)) + (q1*w1*(w3*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8) - w3*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8)))/(2*(- w1^2 - w2^2 - w3^2)^(3/2)) - (q4*w1*(w2*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8) - w2*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8)))/(2*(- w1^2 - w2^2 - w3^2)^(3/2)), q2*((w2*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8))/(16*(- w1^2 - w2^2 - w3^2)^(1/2)) - (w2*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8))/(16*(- w1^2 - w2^2 - w3^2)^(1/2))) - (q3*((w1*w2*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2)) + (w1*w2*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2))))/(2*(- w1^2 - w2^2 - w3^2)^(1/2)) + (q1*((w2*w3*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2)) + (w2*w3*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2))))/(2*(- w1^2 - w2^2 - w3^2)^(1/2)) - (q4*(exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8) - exp((- w1^2 - w2^2 - w3^2)^(1/2)/8) + (w2^2*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2)) + (w2^2*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2))))/(2*(- w1^2 - w2^2 - w3^2)^(1/2)) - (q3*w2*(w1*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8) - w1*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8)))/(2*(- w1^2 - w2^2 - w3^2)^(3/2)) + (q1*w2*(w3*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8) - w3*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8)))/(2*(- w1^2 - w2^2 - w3^2)^(3/2)) - (q4*w2*(w2*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8) - w2*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8)))/(2*(- w1^2 - w2^2 - w3^2)^(3/2)), q2*((w3*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8))/(16*(- w1^2 - w2^2 - w3^2)^(1/2)) - (w3*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8))/(16*(- w1^2 - w2^2 - w3^2)^(1/2))) - (q3*((w1*w3*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2)) + (w1*w3*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2))))/(2*(- w1^2 - w2^2 - w3^2)^(1/2)) - (q4*((w2*w3*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2)) + (w2*w3*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2))))/(2*(- w1^2 - w2^2 - w3^2)^(1/2)) + (q1*(exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8) - exp((- w1^2 - w2^2 - w3^2)^(1/2)/8) + (w3^2*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2)) + (w3^2*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2))))/(2*(- w1^2 - w2^2 - w3^2)^(1/2)) - (q3*w3*(w1*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8) - w1*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8)))/(2*(- w1^2 - w2^2 - w3^2)^(3/2)) + (q1*w3*(w3*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8) - w3*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8)))/(2*(- w1^2 - w2^2 - w3^2)^(3/2)) - (q4*w3*(w2*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8) - w2*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8)))/(2*(- w1^2 - w2^2 - w3^2)^(3/2)), (w3*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8) - w3*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8))/(2*(- w1^2 - w2^2 - w3^2)^(1/2)), exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8)/2 + exp((- w1^2 - w2^2 - w3^2)^(1/2)/8)/2, -(w1*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8) - w1*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8))/(2*(- w1^2 - w2^2 - w3^2)^(1/2)), -(w2*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8) - w2*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8))/(2*(- w1^2 - w2^2 - w3^2)^(1/2));
q3*((w1*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8))/(16*(- w1^2 - w2^2 - w3^2)^(1/2)) - (w1*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8))/(16*(- w1^2 - w2^2 - w3^2)^(1/2))) - (q1*((w1*w2*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2)) + (w1*w2*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2))))/(2*(- w1^2 - w2^2 - w3^2)^(1/2)) - (q4*((w1*w3*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2)) + (w1*w3*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2))))/(2*(- w1^2 - w2^2 - w3^2)^(1/2)) + (q2*(exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8) - exp((- w1^2 - w2^2 - w3^2)^(1/2)/8) + (w1^2*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2)) + (w1^2*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2))))/(2*(- w1^2 - w2^2 - w3^2)^(1/2)) + (q2*w1*(w1*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8) - w1*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8)))/(2*(- w1^2 - w2^2 - w3^2)^(3/2)) - (q1*w1*(w2*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8) - w2*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8)))/(2*(- w1^2 - w2^2 - w3^2)^(3/2)) - (q4*w1*(w3*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8) - w3*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8)))/(2*(- w1^2 - w2^2 - w3^2)^(3/2)), q3*((w2*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8))/(16*(- w1^2 - w2^2 - w3^2)^(1/2)) - (w2*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8))/(16*(- w1^2 - w2^2 - w3^2)^(1/2))) + (q2*((w1*w2*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2)) + (w1*w2*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2))))/(2*(- w1^2 - w2^2 - w3^2)^(1/2)) - (q4*((w2*w3*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2)) + (w2*w3*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2))))/(2*(- w1^2 - w2^2 - w3^2)^(1/2)) - (q1*(exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8) - exp((- w1^2 - w2^2 - w3^2)^(1/2)/8) + (w2^2*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2)) + (w2^2*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2))))/(2*(- w1^2 - w2^2 - w3^2)^(1/2)) + (q2*w2*(w1*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8) - w1*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8)))/(2*(- w1^2 - w2^2 - w3^2)^(3/2)) - (q1*w2*(w2*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8) - w2*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8)))/(2*(- w1^2 - w2^2 - w3^2)^(3/2)) - (q4*w2*(w3*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8) - w3*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8)))/(2*(- w1^2 - w2^2 - w3^2)^(3/2)), q3*((w3*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8))/(16*(- w1^2 - w2^2 - w3^2)^(1/2)) - (w3*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8))/(16*(- w1^2 - w2^2 - w3^2)^(1/2))) + (q2*((w1*w3*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2)) + (w1*w3*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2))))/(2*(- w1^2 - w2^2 - w3^2)^(1/2)) - (q1*((w2*w3*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2)) + (w2*w3*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2))))/(2*(- w1^2 - w2^2 - w3^2)^(1/2)) - (q4*(exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8) - exp((- w1^2 - w2^2 - w3^2)^(1/2)/8) + (w3^2*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2)) + (w3^2*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2))))/(2*(- w1^2 - w2^2 - w3^2)^(1/2)) + (q2*w3*(w1*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8) - w1*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8)))/(2*(- w1^2 - w2^2 - w3^2)^(3/2)) - (q1*w3*(w2*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8) - w2*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8)))/(2*(- w1^2 - w2^2 - w3^2)^(3/2)) - (q4*w3*(w3*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8) - w3*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8)))/(2*(- w1^2 - w2^2 - w3^2)^(3/2)), -(w2*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8) - w2*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8))/(2*(- w1^2 - w2^2 - w3^2)^(1/2)), (w1*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8) - w1*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8))/(2*(- w1^2 - w2^2 - w3^2)^(1/2)), exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8)/2 + exp((- w1^2 - w2^2 - w3^2)^(1/2)/8)/2, -(w3*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8) - w3*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8))/(2*(- w1^2 - w2^2 - w3^2)^(1/2));
q4*((w1*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8))/(16*(- w1^2 - w2^2 - w3^2)^(1/2)) - (w1*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8))/(16*(- w1^2 - w2^2 - w3^2)^(1/2))) + (q2*((w1*w2*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2)) + (w1*w2*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2))))/(2*(- w1^2 - w2^2 - w3^2)^(1/2)) + (q3*((w1*w3*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2)) + (w1*w3*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2))))/(2*(- w1^2 - w2^2 - w3^2)^(1/2)) + (q1*(exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8) - exp((- w1^2 - w2^2 - w3^2)^(1/2)/8) + (w1^2*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2)) + (w1^2*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2))))/(2*(- w1^2 - w2^2 - w3^2)^(1/2)) + (q1*w1*(w1*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8) - w1*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8)))/(2*(- w1^2 - w2^2 - w3^2)^(3/2)) + (q2*w1*(w2*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8) - w2*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8)))/(2*(- w1^2 - w2^2 - w3^2)^(3/2)) + (q3*w1*(w3*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8) - w3*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8)))/(2*(- w1^2 - w2^2 - w3^2)^(3/2)), q4*((w2*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8))/(16*(- w1^2 - w2^2 - w3^2)^(1/2)) - (w2*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8))/(16*(- w1^2 - w2^2 - w3^2)^(1/2))) + (q1*((w1*w2*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2)) + (w1*w2*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2))))/(2*(- w1^2 - w2^2 - w3^2)^(1/2)) + (q3*((w2*w3*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2)) + (w2*w3*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2))))/(2*(- w1^2 - w2^2 - w3^2)^(1/2)) + (q2*(exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8) - exp((- w1^2 - w2^2 - w3^2)^(1/2)/8) + (w2^2*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2)) + (w2^2*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2))))/(2*(- w1^2 - w2^2 - w3^2)^(1/2)) + (q1*w2*(w1*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8) - w1*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8)))/(2*(- w1^2 - w2^2 - w3^2)^(3/2)) + (q2*w2*(w2*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8) - w2*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8)))/(2*(- w1^2 - w2^2 - w3^2)^(3/2)) + (q3*w2*(w3*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8) - w3*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8)))/(2*(- w1^2 - w2^2 - w3^2)^(3/2)), q4*((w3*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8))/(16*(- w1^2 - w2^2 - w3^2)^(1/2)) - (w3*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8))/(16*(- w1^2 - w2^2 - w3^2)^(1/2))) + (q1*((w1*w3*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2)) + (w1*w3*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2))))/(2*(- w1^2 - w2^2 - w3^2)^(1/2)) + (q2*((w2*w3*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2)) + (w2*w3*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2))))/(2*(- w1^2 - w2^2 - w3^2)^(1/2)) + (q3*(exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8) - exp((- w1^2 - w2^2 - w3^2)^(1/2)/8) + (w3^2*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2)) + (w3^2*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8))/(8*(- w1^2 - w2^2 - w3^2)^(1/2))))/(2*(- w1^2 - w2^2 - w3^2)^(1/2)) + (q1*w3*(w1*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8) - w1*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8)))/(2*(- w1^2 - w2^2 - w3^2)^(3/2)) + (q2*w3*(w2*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8) - w2*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8)))/(2*(- w1^2 - w2^2 - w3^2)^(3/2)) + (q3*w3*(w3*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8) - w3*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8)))/(2*(- w1^2 - w2^2 - w3^2)^(3/2)), (w1*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8) - w1*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8))/(2*(- w1^2 - w2^2 - w3^2)^(1/2)), (w2*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8) - w2*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8))/(2*(- w1^2 - w2^2 - w3^2)^(1/2)), (w3*exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8) - w3*exp((- w1^2 - w2^2 - w3^2)^(1/2)/8))/(2*(- w1^2 - w2^2 - w3^2)^(1/2)), exp(-(- w1^2 - w2^2 - w3^2)^(1/2)/8)/2 + exp((- w1^2 - w2^2 - w3^2)^(1/2)/8)/2];
end