-
Notifications
You must be signed in to change notification settings - Fork 9
/
Copy pathCovariancePrediction.m
273 lines (253 loc) · 44.2 KB
/
CovariancePrediction.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
268
269
270
271
272
273
%#codegen
function nextP = CovariancePrediction(deltaAngle, ...
deltaVelocity, ...
states,...
P, ... % Previous state covariance matrix
dt) % IMU and prediction time step
dAngBiasSigma = single(dt*0.05/3600*pi/180);
dVelBiasSigma = single(dt*0.01/60);
% magEarthSigma = single(dt*10/60); % 10 mgauss per minute
% magBodySigma = single(dt*100/60); % 100 mgauss per minute - allow to adapt more rapidly due to changes in airframe
processNoise = [1e-9*ones(1,10), dAngBiasSigma*[1 1 1], dVelBiasSigma*[1 1 1]].^2;
% Specify the estimated errors on the delta angles and delta velocities
daxCov = (dt*25.0/60*pi/180)^2;
dayCov = (dt*25.0/60*pi/180)^2;
dazCov = (dt*25.0/60*pi/180)^2;
dvxCov = (dt*0.5)^2;
dvyCov = (dt*0.5)^2;
dvzCov = (dt*0.5)^2;
dvx = deltaVelocity(1);
dvy = deltaVelocity(2);
dvz = deltaVelocity(3);
dax = deltaAngle(1);
day = deltaAngle(2);
daz = deltaAngle(3);
q0 = states(1);
q1 = states(2);
q2 = states(3);
q3 = states(4);
% vn = states(5);
% ve = states(6);
% vd = states(7);
%
% pn = states(8);
% pe = states(9);
% pd = states(10);
dax_b = states(11);
day_b = states(12);
daz_b = states(13);
dvx_b = states(14);
dvy_b = states(15);
dvz_b = states(16);
%
% magN = states(17);
% magE = states(18);
% magD = states(19);
%
% magX = states(20);
% magY = states(21);
% magZ = states(22);
% Predicted covariance
SF = zeros(14,1);
SF(1) = dvz - dvz_b;
SF(2) = dvy - dvy_b;
SF(3) = dvx - dvx_b;
SF(4) = 2*q1*SF(3) + 2*q2*SF(2) + 2*q3*SF(1);
SF(5) = 2*q0*SF(2) - 2*q1*SF(1) + 2*q3*SF(3);
SF(6) = 2*q0*SF(3) + 2*q2*SF(1) - 2*q3*SF(2);
SF(7) = day/2 - day_b/2;
SF(8) = daz/2 - daz_b/2;
SF(9) = dax/2 - dax_b/2;
SF(10) = 2*q1*SF(2);
SF(11) = 2*q0*SF(1);
SF(12) = q1/2;
SF(13) = q2/2;
SF(14) = q3/2;
SG = zeros(8,1);
SG(1) = q0/2;
SG(2) = q3^2;
SG(3) = q2^2;
SG(4) = q1^2;
SG(5) = q0^2;
SG(6) = 2*q2*q3;
SG(7) = 2*q1*q3;
SG(8) = 2*q1*q2;
SQ = zeros(11,1);
SQ(1) = dvzCov*(SG(6) - 2*q0*q1)*(SG(2) - SG(3) - SG(4) + SG(5)) - dvyCov*(SG(6) + 2*q0*q1)*(SG(2) - SG(3) + SG(4) - SG(5)) + dvxCov*(SG(7) - 2*q0*q2)*(SG(8) + 2*q0*q3);
SQ(2) = dvzCov*(SG(7) + 2*q0*q2)*(SG(2) - SG(3) - SG(4) + SG(5)) - dvxCov*(SG(7) - 2*q0*q2)*(SG(2) + SG(3) - SG(4) - SG(5)) + dvyCov*(SG(6) + 2*q0*q1)*(SG(8) - 2*q0*q3);
SQ(3) = dvzCov*(SG(6) - 2*q0*q1)*(SG(7) + 2*q0*q2) - dvyCov*(SG(8) - 2*q0*q3)*(SG(2) - SG(3) + SG(4) - SG(5)) - dvxCov*(SG(8) + 2*q0*q3)*(SG(2) + SG(3) - SG(4) - SG(5));
SQ(4) = (dayCov*q1*SG(1))/2 - (dazCov*q1*SG(1))/2 - (daxCov*q2*q3)/4;
SQ(5) = (dazCov*q2*SG(1))/2 - (daxCov*q2*SG(1))/2 - (dayCov*q1*q3)/4;
SQ(6) = (daxCov*q3*SG(1))/2 - (dayCov*q3*SG(1))/2 - (dazCov*q1*q2)/4;
SQ(7) = (daxCov*q1*q2)/4 - (dazCov*q3*SG(1))/2 - (dayCov*q1*q2)/4;
SQ(8) = (dazCov*q1*q3)/4 - (daxCov*q1*q3)/4 - (dayCov*q2*SG(1))/2;
SQ(9) = (dayCov*q2*q3)/4 - (daxCov*q1*SG(1))/2 - (dazCov*q2*q3)/4;
SQ(10) = SG(1)^2;
SPP = zeros(8,1);
SPP(1) = SF(10) + SF(11) - 2*q2*SF(3);
SPP(2) = q0^2 - q1^2 + q2^2 - q3^2;
SPP(3) = q0^2 + q1^2 - q2^2 - q3^2;
SPP(4) = 2*q0*q2 - 2*q1*q3;
SPP(5) = 2*q0*q1 - 2*q2*q3;
SPP(6) = 2*q0*q3 - 2*q1*q2;
SPP(7) = SF(8);
SPP(8) = SF(9);
nextP = zeros(16,16);
nextP(1,1) = P(1,1) - P(3,1)*SF(7) + P(11,1)*SF(12) + P(12,1)*SF(13) + P(13,1)*SF(14) - P(2,1)*SPP(8) - P(4,1)*SPP(7) + (daxCov*SQ(11))/4 - SF(7)*(P(1,3) - P(3,3)*SF(7) + P(11,3)*SF(12) + P(12,3)*SF(13) + P(13,3)*SF(14) - P(2,3)*SPP(8) - P(4,3)*SPP(7)) + SF(12)*(P(1,11) - P(3,11)*SF(7) + P(11,11)*SF(12) + P(12,11)*SF(13) + P(13,11)*SF(14) - P(2,11)*SPP(8) - P(4,11)*SPP(7)) + SF(13)*(P(1,12) - P(3,12)*SF(7) + P(11,12)*SF(12) + P(12,12)*SF(13) + P(13,12)*SF(14) - P(2,12)*SPP(8) - P(4,12)*SPP(7)) + SF(14)*(P(1,13) - P(3,13)*SF(7) + P(11,13)*SF(12) + P(12,13)*SF(13) + P(13,13)*SF(14) - P(2,13)*SPP(8) - P(4,13)*SPP(7)) - SPP(8)*(P(1,2) - P(3,2)*SF(7) + P(11,2)*SF(12) + P(12,2)*SF(13) + P(13,2)*SF(14) - P(2,2)*SPP(8) - P(4,2)*SPP(7)) - SPP(7)*(P(1,4) - P(3,4)*SF(7) + P(11,4)*SF(12) + P(12,4)*SF(13) + P(13,4)*SF(14) - P(2,4)*SPP(8) - P(4,4)*SPP(7)) + (dayCov*q2^2)/4 + (dazCov*q3^2)/4;
nextP(1,2) = P(1,2) + SQ(9) - P(3,2)*SF(7) + P(11,2)*SF(12) + P(12,2)*SF(13) + P(13,2)*SF(14) - P(2,2)*SPP(8) - P(4,2)*SPP(7) - SF(7)*(P(1,4) - P(3,4)*SF(7) + P(11,4)*SF(12) + P(12,4)*SF(13) + P(13,4)*SF(14) - P(2,4)*SPP(8) - P(4,4)*SPP(7)) + SF(14)*(P(1,12) - P(3,12)*SF(7) + P(11,12)*SF(12) + P(12,12)*SF(13) + P(13,12)*SF(14) - P(2,12)*SPP(8) - P(4,12)*SPP(7)) - SF(13)*(P(1,13) - P(3,13)*SF(7) + P(11,13)*SF(12) + P(12,13)*SF(13) + P(13,13)*SF(14) - P(2,13)*SPP(8) - P(4,13)*SPP(7)) + SPP(8)*(P(1,1) - P(3,1)*SF(7) + P(11,1)*SF(12) + P(12,1)*SF(13) + P(13,1)*SF(14) - P(2,1)*SPP(8) - P(4,1)*SPP(7)) + SPP(7)*(P(1,3) - P(3,3)*SF(7) + P(11,3)*SF(12) + P(12,3)*SF(13) + P(13,3)*SF(14) - P(2,3)*SPP(8) - P(4,3)*SPP(7)) - (q0*(P(1,11) - P(3,11)*SF(7) + P(11,11)*SF(12) + P(12,11)*SF(13) + P(13,11)*SF(14) - P(2,11)*SPP(8) - P(4,11)*SPP(7)))/2;
nextP(2,2) = P(2,2) - P(4,2)*SF(7) + P(12,2)*SF(14) - P(13,2)*SF(13) + P(1,2)*SPP(8) + P(3,2)*SPP(7) + daxCov*SQ(10) - (P(11,2)*q0)/2 - SF(7)*(P(2,4) - P(4,4)*SF(7) + P(12,4)*SF(14) - P(13,4)*SF(13) + P(1,4)*SPP(8) + P(3,4)*SPP(7) - (P(11,4)*q0)/2) + SF(14)*(P(2,12) - P(4,12)*SF(7) + P(12,12)*SF(14) - P(13,12)*SF(13) + P(1,12)*SPP(8) + P(3,12)*SPP(7) - (P(11,12)*q0)/2) - SF(13)*(P(2,13) - P(4,13)*SF(7) + P(12,13)*SF(14) - P(13,13)*SF(13) + P(1,13)*SPP(8) + P(3,13)*SPP(7) - (P(11,13)*q0)/2) + SPP(8)*(P(2,1) - P(4,1)*SF(7) + P(12,1)*SF(14) - P(13,1)*SF(13) + P(1,1)*SPP(8) + P(3,1)*SPP(7) - (P(11,1)*q0)/2) + SPP(7)*(P(2,3) - P(4,3)*SF(7) + P(12,3)*SF(14) - P(13,3)*SF(13) + P(1,3)*SPP(8) + P(3,3)*SPP(7) - (P(11,3)*q0)/2) + (dayCov*q3^2)/4 + (dazCov*q2^2)/4 - (q0*(P(2,11) - P(4,11)*SF(7) + P(12,11)*SF(14) - P(13,11)*SF(13) + P(1,11)*SPP(8) + P(3,11)*SPP(7) - (P(11,11)*q0)/2))/2;
nextP(1,3) = P(1,3) + SQ(8) - P(3,3)*SF(7) + P(11,3)*SF(12) + P(12,3)*SF(13) + P(13,3)*SF(14) - P(2,3)*SPP(8) - P(4,3)*SPP(7) + SF(7)*(P(1,1) - P(3,1)*SF(7) + P(11,1)*SF(12) + P(12,1)*SF(13) + P(13,1)*SF(14) - P(2,1)*SPP(8) - P(4,1)*SPP(7)) - SF(14)*(P(1,11) - P(3,11)*SF(7) + P(11,11)*SF(12) + P(12,11)*SF(13) + P(13,11)*SF(14) - P(2,11)*SPP(8) - P(4,11)*SPP(7)) + SF(12)*(P(1,13) - P(3,13)*SF(7) + P(11,13)*SF(12) + P(12,13)*SF(13) + P(13,13)*SF(14) - P(2,13)*SPP(8) - P(4,13)*SPP(7)) - SPP(7)*(P(1,2) - P(3,2)*SF(7) + P(11,2)*SF(12) + P(12,2)*SF(13) + P(13,2)*SF(14) - P(2,2)*SPP(8) - P(4,2)*SPP(7)) + SPP(8)*(P(1,4) - P(3,4)*SF(7) + P(11,4)*SF(12) + P(12,4)*SF(13) + P(13,4)*SF(14) - P(2,4)*SPP(8) - P(4,4)*SPP(7)) - (q0*(P(1,12) - P(3,12)*SF(7) + P(11,12)*SF(12) + P(12,12)*SF(13) + P(13,12)*SF(14) - P(2,12)*SPP(8) - P(4,12)*SPP(7)))/2;
nextP(2,3) = P(2,3) + SQ(6) - P(4,3)*SF(7) + P(12,3)*SF(14) - P(13,3)*SF(13) + P(1,3)*SPP(8) + P(3,3)*SPP(7) - (P(11,3)*q0)/2 + SF(7)*(P(2,1) - P(4,1)*SF(7) + P(12,1)*SF(14) - P(13,1)*SF(13) + P(1,1)*SPP(8) + P(3,1)*SPP(7) - (P(11,1)*q0)/2) - SF(14)*(P(2,11) - P(4,11)*SF(7) + P(12,11)*SF(14) - P(13,11)*SF(13) + P(1,11)*SPP(8) + P(3,11)*SPP(7) - (P(11,11)*q0)/2) + SF(12)*(P(2,13) - P(4,13)*SF(7) + P(12,13)*SF(14) - P(13,13)*SF(13) + P(1,13)*SPP(8) + P(3,13)*SPP(7) - (P(11,13)*q0)/2) - SPP(7)*(P(2,2) - P(4,2)*SF(7) + P(12,2)*SF(14) - P(13,2)*SF(13) + P(1,2)*SPP(8) + P(3,2)*SPP(7) - (P(11,2)*q0)/2) + SPP(8)*(P(2,4) - P(4,4)*SF(7) + P(12,4)*SF(14) - P(13,4)*SF(13) + P(1,4)*SPP(8) + P(3,4)*SPP(7) - (P(11,4)*q0)/2) - (q0*(P(2,12) - P(4,12)*SF(7) + P(12,12)*SF(14) - P(13,12)*SF(13) + P(1,12)*SPP(8) + P(3,12)*SPP(7) - (P(11,12)*q0)/2))/2;
nextP(3,3) = P(3,3) + P(1,3)*SF(7) - P(11,3)*SF(14) + P(13,3)*SF(12) - P(2,3)*SPP(7) + P(4,3)*SPP(8) + dayCov*SQ(10) + (dazCov*SQ(11))/4 - (P(12,3)*q0)/2 + SF(7)*(P(3,1) + P(1,1)*SF(7) - P(11,1)*SF(14) + P(13,1)*SF(12) - P(2,1)*SPP(7) + P(4,1)*SPP(8) - (P(12,1)*q0)/2) - SF(14)*(P(3,11) + P(1,11)*SF(7) - P(11,11)*SF(14) + P(13,11)*SF(12) - P(2,11)*SPP(7) + P(4,11)*SPP(8) - (P(12,11)*q0)/2) + SF(12)*(P(3,13) + P(1,13)*SF(7) - P(11,13)*SF(14) + P(13,13)*SF(12) - P(2,13)*SPP(7) + P(4,13)*SPP(8) - (P(12,13)*q0)/2) - SPP(7)*(P(3,2) + P(1,2)*SF(7) - P(11,2)*SF(14) + P(13,2)*SF(12) - P(2,2)*SPP(7) + P(4,2)*SPP(8) - (P(12,2)*q0)/2) + SPP(8)*(P(3,4) + P(1,4)*SF(7) - P(11,4)*SF(14) + P(13,4)*SF(12) - P(2,4)*SPP(7) + P(4,4)*SPP(8) - (P(12,4)*q0)/2) + (daxCov*q3^2)/4 - (q0*(P(3,12) + P(1,12)*SF(7) - P(11,12)*SF(14) + P(13,12)*SF(12) - P(2,12)*SPP(7) + P(4,12)*SPP(8) - (P(12,12)*q0)/2))/2;
nextP(1,4) = P(1,4) + SQ(7) - P(3,4)*SF(7) + P(11,4)*SF(12) + P(12,4)*SF(13) + P(13,4)*SF(14) - P(2,4)*SPP(8) - P(4,4)*SPP(7) + SF(7)*(P(1,2) - P(3,2)*SF(7) + P(11,2)*SF(12) + P(12,2)*SF(13) + P(13,2)*SF(14) - P(2,2)*SPP(8) - P(4,2)*SPP(7)) + SF(13)*(P(1,11) - P(3,11)*SF(7) + P(11,11)*SF(12) + P(12,11)*SF(13) + P(13,11)*SF(14) - P(2,11)*SPP(8) - P(4,11)*SPP(7)) - SF(12)*(P(1,12) - P(3,12)*SF(7) + P(11,12)*SF(12) + P(12,12)*SF(13) + P(13,12)*SF(14) - P(2,12)*SPP(8) - P(4,12)*SPP(7)) + SPP(7)*(P(1,1) - P(3,1)*SF(7) + P(11,1)*SF(12) + P(12,1)*SF(13) + P(13,1)*SF(14) - P(2,1)*SPP(8) - P(4,1)*SPP(7)) - SPP(8)*(P(1,3) - P(3,3)*SF(7) + P(11,3)*SF(12) + P(12,3)*SF(13) + P(13,3)*SF(14) - P(2,3)*SPP(8) - P(4,3)*SPP(7)) - (q0*(P(1,13) - P(3,13)*SF(7) + P(11,13)*SF(12) + P(12,13)*SF(13) + P(13,13)*SF(14) - P(2,13)*SPP(8) - P(4,13)*SPP(7)))/2;
nextP(2,4) = P(2,4) + SQ(5) - P(4,4)*SF(7) + P(12,4)*SF(14) - P(13,4)*SF(13) + P(1,4)*SPP(8) + P(3,4)*SPP(7) - (P(11,4)*q0)/2 + SF(7)*(P(2,2) - P(4,2)*SF(7) + P(12,2)*SF(14) - P(13,2)*SF(13) + P(1,2)*SPP(8) + P(3,2)*SPP(7) - (P(11,2)*q0)/2) + SF(13)*(P(2,11) - P(4,11)*SF(7) + P(12,11)*SF(14) - P(13,11)*SF(13) + P(1,11)*SPP(8) + P(3,11)*SPP(7) - (P(11,11)*q0)/2) - SF(12)*(P(2,12) - P(4,12)*SF(7) + P(12,12)*SF(14) - P(13,12)*SF(13) + P(1,12)*SPP(8) + P(3,12)*SPP(7) - (P(11,12)*q0)/2) + SPP(7)*(P(2,1) - P(4,1)*SF(7) + P(12,1)*SF(14) - P(13,1)*SF(13) + P(1,1)*SPP(8) + P(3,1)*SPP(7) - (P(11,1)*q0)/2) - SPP(8)*(P(2,3) - P(4,3)*SF(7) + P(12,3)*SF(14) - P(13,3)*SF(13) + P(1,3)*SPP(8) + P(3,3)*SPP(7) - (P(11,3)*q0)/2) - (q0*(P(2,13) - P(4,13)*SF(7) + P(12,13)*SF(14) - P(13,13)*SF(13) + P(1,13)*SPP(8) + P(3,13)*SPP(7) - (P(11,13)*q0)/2))/2;
nextP(3,4) = P(3,4) + SQ(4) + P(1,4)*SF(7) - P(11,4)*SF(14) + P(13,4)*SF(12) - P(2,4)*SPP(7) + P(4,4)*SPP(8) - (P(12,4)*q0)/2 + SF(7)*(P(3,2) + P(1,2)*SF(7) - P(11,2)*SF(14) + P(13,2)*SF(12) - P(2,2)*SPP(7) + P(4,2)*SPP(8) - (P(12,2)*q0)/2) + SF(13)*(P(3,11) + P(1,11)*SF(7) - P(11,11)*SF(14) + P(13,11)*SF(12) - P(2,11)*SPP(7) + P(4,11)*SPP(8) - (P(12,11)*q0)/2) - SF(12)*(P(3,12) + P(1,12)*SF(7) - P(11,12)*SF(14) + P(13,12)*SF(12) - P(2,12)*SPP(7) + P(4,12)*SPP(8) - (P(12,12)*q0)/2) + SPP(7)*(P(3,1) + P(1,1)*SF(7) - P(11,1)*SF(14) + P(13,1)*SF(12) - P(2,1)*SPP(7) + P(4,1)*SPP(8) - (P(12,1)*q0)/2) - SPP(8)*(P(3,3) + P(1,3)*SF(7) - P(11,3)*SF(14) + P(13,3)*SF(12) - P(2,3)*SPP(7) + P(4,3)*SPP(8) - (P(12,3)*q0)/2) - (q0*(P(3,13) + P(1,13)*SF(7) - P(11,13)*SF(14) + P(13,13)*SF(12) - P(2,13)*SPP(7) + P(4,13)*SPP(8) - (P(12,13)*q0)/2))/2;
nextP(4,4) = P(4,4) + P(2,4)*SF(7) + P(11,4)*SF(13) - P(12,4)*SF(12) + P(1,4)*SPP(7) - P(3,4)*SPP(8) + (dayCov*SQ(11))/4 + dazCov*SQ(10) - (P(13,4)*q0)/2 + SF(7)*(P(4,2) + P(2,2)*SF(7) + P(11,2)*SF(13) - P(12,2)*SF(12) + P(1,2)*SPP(7) - P(3,2)*SPP(8) - (P(13,2)*q0)/2) + SF(13)*(P(4,11) + P(2,11)*SF(7) + P(11,11)*SF(13) - P(12,11)*SF(12) + P(1,11)*SPP(7) - P(3,11)*SPP(8) - (P(13,11)*q0)/2) - SF(12)*(P(4,12) + P(2,12)*SF(7) + P(11,12)*SF(13) - P(12,12)*SF(12) + P(1,12)*SPP(7) - P(3,12)*SPP(8) - (P(13,12)*q0)/2) + SPP(7)*(P(4,1) + P(2,1)*SF(7) + P(11,1)*SF(13) - P(12,1)*SF(12) + P(1,1)*SPP(7) - P(3,1)*SPP(8) - (P(13,1)*q0)/2) - SPP(8)*(P(4,3) + P(2,3)*SF(7) + P(11,3)*SF(13) - P(12,3)*SF(12) + P(1,3)*SPP(7) - P(3,3)*SPP(8) - (P(13,3)*q0)/2) + (daxCov*q2^2)/4 - (q0*(P(4,13) + P(2,13)*SF(7) + P(11,13)*SF(13) - P(12,13)*SF(12) + P(1,13)*SPP(7) - P(3,13)*SPP(8) - (P(13,13)*q0)/2))/2;
nextP(1,5) = P(1,5) - P(3,5)*SF(7) + P(11,5)*SF(12) + P(12,5)*SF(13) + P(13,5)*SF(14) - P(2,5)*SPP(8) - P(4,5)*SPP(7) + SF(6)*(P(1,1) - P(3,1)*SF(7) + P(11,1)*SF(12) + P(12,1)*SF(13) + P(13,1)*SF(14) - P(2,1)*SPP(8) - P(4,1)*SPP(7)) + SF(4)*(P(1,2) - P(3,2)*SF(7) + P(11,2)*SF(12) + P(12,2)*SF(13) + P(13,2)*SF(14) - P(2,2)*SPP(8) - P(4,2)*SPP(7)) - SF(5)*(P(1,4) - P(3,4)*SF(7) + P(11,4)*SF(12) + P(12,4)*SF(13) + P(13,4)*SF(14) - P(2,4)*SPP(8) - P(4,4)*SPP(7)) + SPP(1)*(P(1,3) - P(3,3)*SF(7) + P(11,3)*SF(12) + P(12,3)*SF(13) + P(13,3)*SF(14) - P(2,3)*SPP(8) - P(4,3)*SPP(7)) - SPP(3)*(P(1,14) - P(3,14)*SF(7) + P(11,14)*SF(12) + P(12,14)*SF(13) + P(13,14)*SF(14) - P(2,14)*SPP(8) - P(4,14)*SPP(7)) + SPP(6)*(P(1,15) - P(3,15)*SF(7) + P(11,15)*SF(12) + P(12,15)*SF(13) + P(13,15)*SF(14) - P(2,15)*SPP(8) - P(4,15)*SPP(7)) - (2*q0*q2 + 2*q1*q3)*(P(1,16) - P(3,16)*SF(7) + P(11,16)*SF(12) + P(12,16)*SF(13) + P(13,16)*SF(14) - P(2,16)*SPP(8) - P(4,16)*SPP(7));
nextP(2,5) = P(2,5) - P(4,5)*SF(7) + P(12,5)*SF(14) - P(13,5)*SF(13) + P(1,5)*SPP(8) + P(3,5)*SPP(7) - (2*q0*q2 + 2*q1*q3)*(P(2,16) - P(4,16)*SF(7) + P(12,16)*SF(14) - P(13,16)*SF(13) + P(1,16)*SPP(8) + P(3,16)*SPP(7) - (P(11,16)*q0)/2) - (P(11,5)*q0)/2 + SF(6)*(P(2,1) - P(4,1)*SF(7) + P(12,1)*SF(14) - P(13,1)*SF(13) + P(1,1)*SPP(8) + P(3,1)*SPP(7) - (P(11,1)*q0)/2) + SF(4)*(P(2,2) - P(4,2)*SF(7) + P(12,2)*SF(14) - P(13,2)*SF(13) + P(1,2)*SPP(8) + P(3,2)*SPP(7) - (P(11,2)*q0)/2) - SF(5)*(P(2,4) - P(4,4)*SF(7) + P(12,4)*SF(14) - P(13,4)*SF(13) + P(1,4)*SPP(8) + P(3,4)*SPP(7) - (P(11,4)*q0)/2) + SPP(1)*(P(2,3) - P(4,3)*SF(7) + P(12,3)*SF(14) - P(13,3)*SF(13) + P(1,3)*SPP(8) + P(3,3)*SPP(7) - (P(11,3)*q0)/2) - SPP(3)*(P(2,14) - P(4,14)*SF(7) + P(12,14)*SF(14) - P(13,14)*SF(13) + P(1,14)*SPP(8) + P(3,14)*SPP(7) - (P(11,14)*q0)/2) + SPP(6)*(P(2,15) - P(4,15)*SF(7) + P(12,15)*SF(14) - P(13,15)*SF(13) + P(1,15)*SPP(8) + P(3,15)*SPP(7) - (P(11,15)*q0)/2);
nextP(3,5) = P(3,5) + P(1,5)*SF(7) - P(11,5)*SF(14) + P(13,5)*SF(12) - P(2,5)*SPP(7) + P(4,5)*SPP(8) - (2*q0*q2 + 2*q1*q3)*(P(3,16) + P(1,16)*SF(7) - P(11,16)*SF(14) + P(13,16)*SF(12) - P(2,16)*SPP(7) + P(4,16)*SPP(8) - (P(12,16)*q0)/2) - (P(12,5)*q0)/2 + SF(6)*(P(3,1) + P(1,1)*SF(7) - P(11,1)*SF(14) + P(13,1)*SF(12) - P(2,1)*SPP(7) + P(4,1)*SPP(8) - (P(12,1)*q0)/2) + SF(4)*(P(3,2) + P(1,2)*SF(7) - P(11,2)*SF(14) + P(13,2)*SF(12) - P(2,2)*SPP(7) + P(4,2)*SPP(8) - (P(12,2)*q0)/2) - SF(5)*(P(3,4) + P(1,4)*SF(7) - P(11,4)*SF(14) + P(13,4)*SF(12) - P(2,4)*SPP(7) + P(4,4)*SPP(8) - (P(12,4)*q0)/2) + SPP(1)*(P(3,3) + P(1,3)*SF(7) - P(11,3)*SF(14) + P(13,3)*SF(12) - P(2,3)*SPP(7) + P(4,3)*SPP(8) - (P(12,3)*q0)/2) - SPP(3)*(P(3,14) + P(1,14)*SF(7) - P(11,14)*SF(14) + P(13,14)*SF(12) - P(2,14)*SPP(7) + P(4,14)*SPP(8) - (P(12,14)*q0)/2) + SPP(6)*(P(3,15) + P(1,15)*SF(7) - P(11,15)*SF(14) + P(13,15)*SF(12) - P(2,15)*SPP(7) + P(4,15)*SPP(8) - (P(12,15)*q0)/2);
nextP(4,5) = P(4,5) + P(2,5)*SF(7) + P(11,5)*SF(13) - P(12,5)*SF(12) + P(1,5)*SPP(7) - P(3,5)*SPP(8) - (2*q0*q2 + 2*q1*q3)*(P(4,16) + P(2,16)*SF(7) + P(11,16)*SF(13) - P(12,16)*SF(12) + P(1,16)*SPP(7) - P(3,16)*SPP(8) - (P(13,16)*q0)/2) - (P(13,5)*q0)/2 + SF(6)*(P(4,1) + P(2,1)*SF(7) + P(11,1)*SF(13) - P(12,1)*SF(12) + P(1,1)*SPP(7) - P(3,1)*SPP(8) - (P(13,1)*q0)/2) + SF(4)*(P(4,2) + P(2,2)*SF(7) + P(11,2)*SF(13) - P(12,2)*SF(12) + P(1,2)*SPP(7) - P(3,2)*SPP(8) - (P(13,2)*q0)/2) - SF(5)*(P(4,4) + P(2,4)*SF(7) + P(11,4)*SF(13) - P(12,4)*SF(12) + P(1,4)*SPP(7) - P(3,4)*SPP(8) - (P(13,4)*q0)/2) + SPP(1)*(P(4,3) + P(2,3)*SF(7) + P(11,3)*SF(13) - P(12,3)*SF(12) + P(1,3)*SPP(7) - P(3,3)*SPP(8) - (P(13,3)*q0)/2) - SPP(3)*(P(4,14) + P(2,14)*SF(7) + P(11,14)*SF(13) - P(12,14)*SF(12) + P(1,14)*SPP(7) - P(3,14)*SPP(8) - (P(13,14)*q0)/2) + SPP(6)*(P(4,15) + P(2,15)*SF(7) + P(11,15)*SF(13) - P(12,15)*SF(12) + P(1,15)*SPP(7) - P(3,15)*SPP(8) - (P(13,15)*q0)/2);
nextP(5,5) = P(5,5) + P(1,5)*SF(6) + P(2,5)*SF(4) - P(4,5)*SF(5) + P(3,5)*SPP(1) - P(14,5)*SPP(3) + P(15,5)*SPP(6) + dvyCov*(SG(8) - 2*q0*q3)^2 + dvzCov*(SG(7) + 2*q0*q2)^2 - (2*q0*q2 + 2*q1*q3)*(P(5,16) + P(1,16)*SF(6) + P(2,16)*SF(4) - P(4,16)*SF(5) + P(3,16)*SPP(1) - P(14,16)*SPP(3) + P(15,16)*SPP(6) - P(16,16)*(2*q0*q2 + 2*q1*q3)) + dvxCov*(SG(2) + SG(3) - SG(4) - SG(5))^2 - P(16,5)*(2*q0*q2 + 2*q1*q3) + SF(6)*(P(5,1) + P(1,1)*SF(6) + P(2,1)*SF(4) - P(4,1)*SF(5) + P(3,1)*SPP(1) - P(14,1)*SPP(3) + P(15,1)*SPP(6) - P(16,1)*(2*q0*q2 + 2*q1*q3)) + SF(4)*(P(5,2) + P(1,2)*SF(6) + P(2,2)*SF(4) - P(4,2)*SF(5) + P(3,2)*SPP(1) - P(14,2)*SPP(3) + P(15,2)*SPP(6) - P(16,2)*(2*q0*q2 + 2*q1*q3)) - SF(5)*(P(5,4) + P(1,4)*SF(6) + P(2,4)*SF(4) - P(4,4)*SF(5) + P(3,4)*SPP(1) - P(14,4)*SPP(3) + P(15,4)*SPP(6) - P(16,4)*(2*q0*q2 + 2*q1*q3)) + SPP(1)*(P(5,3) + P(1,3)*SF(6) + P(2,3)*SF(4) - P(4,3)*SF(5) + P(3,3)*SPP(1) - P(14,3)*SPP(3) + P(15,3)*SPP(6) - P(16,3)*(2*q0*q2 + 2*q1*q3)) - SPP(3)*(P(5,14) + P(1,14)*SF(6) + P(2,14)*SF(4) - P(4,14)*SF(5) + P(3,14)*SPP(1) - P(14,14)*SPP(3) + P(15,14)*SPP(6) - P(16,14)*(2*q0*q2 + 2*q1*q3)) + SPP(6)*(P(5,15) + P(1,15)*SF(6) + P(2,15)*SF(4) - P(4,15)*SF(5) + P(3,15)*SPP(1) - P(14,15)*SPP(3) + P(15,15)*SPP(6) - P(16,15)*(2*q0*q2 + 2*q1*q3));
nextP(1,6) = P(1,6) - P(3,6)*SF(7) + P(11,6)*SF(12) + P(12,6)*SF(13) + P(13,6)*SF(14) - P(2,6)*SPP(8) - P(4,6)*SPP(7) + SF(5)*(P(1,1) - P(3,1)*SF(7) + P(11,1)*SF(12) + P(12,1)*SF(13) + P(13,1)*SF(14) - P(2,1)*SPP(8) - P(4,1)*SPP(7)) + SF(4)*(P(1,3) - P(3,3)*SF(7) + P(11,3)*SF(12) + P(12,3)*SF(13) + P(13,3)*SF(14) - P(2,3)*SPP(8) - P(4,3)*SPP(7)) + SF(6)*(P(1,4) - P(3,4)*SF(7) + P(11,4)*SF(12) + P(12,4)*SF(13) + P(13,4)*SF(14) - P(2,4)*SPP(8) - P(4,4)*SPP(7)) - SPP(1)*(P(1,2) - P(3,2)*SF(7) + P(11,2)*SF(12) + P(12,2)*SF(13) + P(13,2)*SF(14) - P(2,2)*SPP(8) - P(4,2)*SPP(7)) - SPP(2)*(P(1,15) - P(3,15)*SF(7) + P(11,15)*SF(12) + P(12,15)*SF(13) + P(13,15)*SF(14) - P(2,15)*SPP(8) - P(4,15)*SPP(7)) + SPP(5)*(P(1,16) - P(3,16)*SF(7) + P(11,16)*SF(12) + P(12,16)*SF(13) + P(13,16)*SF(14) - P(2,16)*SPP(8) - P(4,16)*SPP(7)) - (2*q0*q3 + 2*q1*q2)*(P(1,14) - P(3,14)*SF(7) + P(11,14)*SF(12) + P(12,14)*SF(13) + P(13,14)*SF(14) - P(2,14)*SPP(8) - P(4,14)*SPP(7));
nextP(2,6) = P(2,6) - P(4,6)*SF(7) + P(12,6)*SF(14) - P(13,6)*SF(13) + P(1,6)*SPP(8) + P(3,6)*SPP(7) - (2*q0*q3 + 2*q1*q2)*(P(2,14) - P(4,14)*SF(7) + P(12,14)*SF(14) - P(13,14)*SF(13) + P(1,14)*SPP(8) + P(3,14)*SPP(7) - (P(11,14)*q0)/2) - (P(11,6)*q0)/2 + SF(5)*(P(2,1) - P(4,1)*SF(7) + P(12,1)*SF(14) - P(13,1)*SF(13) + P(1,1)*SPP(8) + P(3,1)*SPP(7) - (P(11,1)*q0)/2) + SF(4)*(P(2,3) - P(4,3)*SF(7) + P(12,3)*SF(14) - P(13,3)*SF(13) + P(1,3)*SPP(8) + P(3,3)*SPP(7) - (P(11,3)*q0)/2) + SF(6)*(P(2,4) - P(4,4)*SF(7) + P(12,4)*SF(14) - P(13,4)*SF(13) + P(1,4)*SPP(8) + P(3,4)*SPP(7) - (P(11,4)*q0)/2) - SPP(1)*(P(2,2) - P(4,2)*SF(7) + P(12,2)*SF(14) - P(13,2)*SF(13) + P(1,2)*SPP(8) + P(3,2)*SPP(7) - (P(11,2)*q0)/2) - SPP(2)*(P(2,15) - P(4,15)*SF(7) + P(12,15)*SF(14) - P(13,15)*SF(13) + P(1,15)*SPP(8) + P(3,15)*SPP(7) - (P(11,15)*q0)/2) + SPP(5)*(P(2,16) - P(4,16)*SF(7) + P(12,16)*SF(14) - P(13,16)*SF(13) + P(1,16)*SPP(8) + P(3,16)*SPP(7) - (P(11,16)*q0)/2);
nextP(3,6) = P(3,6) + P(1,6)*SF(7) - P(11,6)*SF(14) + P(13,6)*SF(12) - P(2,6)*SPP(7) + P(4,6)*SPP(8) - (2*q0*q3 + 2*q1*q2)*(P(3,14) + P(1,14)*SF(7) - P(11,14)*SF(14) + P(13,14)*SF(12) - P(2,14)*SPP(7) + P(4,14)*SPP(8) - (P(12,14)*q0)/2) - (P(12,6)*q0)/2 + SF(5)*(P(3,1) + P(1,1)*SF(7) - P(11,1)*SF(14) + P(13,1)*SF(12) - P(2,1)*SPP(7) + P(4,1)*SPP(8) - (P(12,1)*q0)/2) + SF(4)*(P(3,3) + P(1,3)*SF(7) - P(11,3)*SF(14) + P(13,3)*SF(12) - P(2,3)*SPP(7) + P(4,3)*SPP(8) - (P(12,3)*q0)/2) + SF(6)*(P(3,4) + P(1,4)*SF(7) - P(11,4)*SF(14) + P(13,4)*SF(12) - P(2,4)*SPP(7) + P(4,4)*SPP(8) - (P(12,4)*q0)/2) - SPP(1)*(P(3,2) + P(1,2)*SF(7) - P(11,2)*SF(14) + P(13,2)*SF(12) - P(2,2)*SPP(7) + P(4,2)*SPP(8) - (P(12,2)*q0)/2) - SPP(2)*(P(3,15) + P(1,15)*SF(7) - P(11,15)*SF(14) + P(13,15)*SF(12) - P(2,15)*SPP(7) + P(4,15)*SPP(8) - (P(12,15)*q0)/2) + SPP(5)*(P(3,16) + P(1,16)*SF(7) - P(11,16)*SF(14) + P(13,16)*SF(12) - P(2,16)*SPP(7) + P(4,16)*SPP(8) - (P(12,16)*q0)/2);
nextP(4,6) = P(4,6) + P(2,6)*SF(7) + P(11,6)*SF(13) - P(12,6)*SF(12) + P(1,6)*SPP(7) - P(3,6)*SPP(8) - (2*q0*q3 + 2*q1*q2)*(P(4,14) + P(2,14)*SF(7) + P(11,14)*SF(13) - P(12,14)*SF(12) + P(1,14)*SPP(7) - P(3,14)*SPP(8) - (P(13,14)*q0)/2) - (P(13,6)*q0)/2 + SF(5)*(P(4,1) + P(2,1)*SF(7) + P(11,1)*SF(13) - P(12,1)*SF(12) + P(1,1)*SPP(7) - P(3,1)*SPP(8) - (P(13,1)*q0)/2) + SF(4)*(P(4,3) + P(2,3)*SF(7) + P(11,3)*SF(13) - P(12,3)*SF(12) + P(1,3)*SPP(7) - P(3,3)*SPP(8) - (P(13,3)*q0)/2) + SF(6)*(P(4,4) + P(2,4)*SF(7) + P(11,4)*SF(13) - P(12,4)*SF(12) + P(1,4)*SPP(7) - P(3,4)*SPP(8) - (P(13,4)*q0)/2) - SPP(1)*(P(4,2) + P(2,2)*SF(7) + P(11,2)*SF(13) - P(12,2)*SF(12) + P(1,2)*SPP(7) - P(3,2)*SPP(8) - (P(13,2)*q0)/2) - SPP(2)*(P(4,15) + P(2,15)*SF(7) + P(11,15)*SF(13) - P(12,15)*SF(12) + P(1,15)*SPP(7) - P(3,15)*SPP(8) - (P(13,15)*q0)/2) + SPP(5)*(P(4,16) + P(2,16)*SF(7) + P(11,16)*SF(13) - P(12,16)*SF(12) + P(1,16)*SPP(7) - P(3,16)*SPP(8) - (P(13,16)*q0)/2);
nextP(5,6) = P(5,6) + SQ(3) + P(1,6)*SF(6) + P(2,6)*SF(4) - P(4,6)*SF(5) + P(3,6)*SPP(1) - P(14,6)*SPP(3) + P(15,6)*SPP(6) - (2*q0*q3 + 2*q1*q2)*(P(5,14) + P(1,14)*SF(6) + P(2,14)*SF(4) - P(4,14)*SF(5) + P(3,14)*SPP(1) - P(14,14)*SPP(3) + P(15,14)*SPP(6) - P(16,14)*(2*q0*q2 + 2*q1*q3)) - P(16,6)*(2*q0*q2 + 2*q1*q3) + SF(5)*(P(5,1) + P(1,1)*SF(6) + P(2,1)*SF(4) - P(4,1)*SF(5) + P(3,1)*SPP(1) - P(14,1)*SPP(3) + P(15,1)*SPP(6) - P(16,1)*(2*q0*q2 + 2*q1*q3)) + SF(4)*(P(5,3) + P(1,3)*SF(6) + P(2,3)*SF(4) - P(4,3)*SF(5) + P(3,3)*SPP(1) - P(14,3)*SPP(3) + P(15,3)*SPP(6) - P(16,3)*(2*q0*q2 + 2*q1*q3)) + SF(6)*(P(5,4) + P(1,4)*SF(6) + P(2,4)*SF(4) - P(4,4)*SF(5) + P(3,4)*SPP(1) - P(14,4)*SPP(3) + P(15,4)*SPP(6) - P(16,4)*(2*q0*q2 + 2*q1*q3)) - SPP(1)*(P(5,2) + P(1,2)*SF(6) + P(2,2)*SF(4) - P(4,2)*SF(5) + P(3,2)*SPP(1) - P(14,2)*SPP(3) + P(15,2)*SPP(6) - P(16,2)*(2*q0*q2 + 2*q1*q3)) - SPP(2)*(P(5,15) + P(1,15)*SF(6) + P(2,15)*SF(4) - P(4,15)*SF(5) + P(3,15)*SPP(1) - P(14,15)*SPP(3) + P(15,15)*SPP(6) - P(16,15)*(2*q0*q2 + 2*q1*q3)) + SPP(5)*(P(5,16) + P(1,16)*SF(6) + P(2,16)*SF(4) - P(4,16)*SF(5) + P(3,16)*SPP(1) - P(14,16)*SPP(3) + P(15,16)*SPP(6) - P(16,16)*(2*q0*q2 + 2*q1*q3));
nextP(6,6) = P(6,6) + P(1,6)*SF(5) + P(3,6)*SF(4) + P(4,6)*SF(6) - P(2,6)*SPP(1) - P(15,6)*SPP(2) + P(16,6)*SPP(5) + dvxCov*(SG(8) + 2*q0*q3)^2 + dvzCov*(SG(6) - 2*q0*q1)^2 - (2*q0*q3 + 2*q1*q2)*(P(6,14) + P(1,14)*SF(5) + P(3,14)*SF(4) + P(4,14)*SF(6) - P(2,14)*SPP(1) - P(15,14)*SPP(2) + P(16,14)*SPP(5) - P(14,14)*(2*q0*q3 + 2*q1*q2)) + dvyCov*(SG(2) - SG(3) + SG(4) - SG(5))^2 - P(14,6)*(2*q0*q3 + 2*q1*q2) + SF(5)*(P(6,1) + P(1,1)*SF(5) + P(3,1)*SF(4) + P(4,1)*SF(6) - P(2,1)*SPP(1) - P(15,1)*SPP(2) + P(16,1)*SPP(5) - P(14,1)*(2*q0*q3 + 2*q1*q2)) + SF(4)*(P(6,3) + P(1,3)*SF(5) + P(3,3)*SF(4) + P(4,3)*SF(6) - P(2,3)*SPP(1) - P(15,3)*SPP(2) + P(16,3)*SPP(5) - P(14,3)*(2*q0*q3 + 2*q1*q2)) + SF(6)*(P(6,4) + P(1,4)*SF(5) + P(3,4)*SF(4) + P(4,4)*SF(6) - P(2,4)*SPP(1) - P(15,4)*SPP(2) + P(16,4)*SPP(5) - P(14,4)*(2*q0*q3 + 2*q1*q2)) - SPP(1)*(P(6,2) + P(1,2)*SF(5) + P(3,2)*SF(4) + P(4,2)*SF(6) - P(2,2)*SPP(1) - P(15,2)*SPP(2) + P(16,2)*SPP(5) - P(14,2)*(2*q0*q3 + 2*q1*q2)) - SPP(2)*(P(6,15) + P(1,15)*SF(5) + P(3,15)*SF(4) + P(4,15)*SF(6) - P(2,15)*SPP(1) - P(15,15)*SPP(2) + P(16,15)*SPP(5) - P(14,15)*(2*q0*q3 + 2*q1*q2)) + SPP(5)*(P(6,16) + P(1,16)*SF(5) + P(3,16)*SF(4) + P(4,16)*SF(6) - P(2,16)*SPP(1) - P(15,16)*SPP(2) + P(16,16)*SPP(5) - P(14,16)*(2*q0*q3 + 2*q1*q2));
nextP(1,7) = P(1,7) - P(3,7)*SF(7) + P(11,7)*SF(12) + P(12,7)*SF(13) + P(13,7)*SF(14) - P(2,7)*SPP(8) - P(4,7)*SPP(7) + SF(5)*(P(1,2) - P(3,2)*SF(7) + P(11,2)*SF(12) + P(12,2)*SF(13) + P(13,2)*SF(14) - P(2,2)*SPP(8) - P(4,2)*SPP(7)) - SF(6)*(P(1,3) - P(3,3)*SF(7) + P(11,3)*SF(12) + P(12,3)*SF(13) + P(13,3)*SF(14) - P(2,3)*SPP(8) - P(4,3)*SPP(7)) + SF(4)*(P(1,4) - P(3,4)*SF(7) + P(11,4)*SF(12) + P(12,4)*SF(13) + P(13,4)*SF(14) - P(2,4)*SPP(8) - P(4,4)*SPP(7)) + SPP(1)*(P(1,1) - P(3,1)*SF(7) + P(11,1)*SF(12) + P(12,1)*SF(13) + P(13,1)*SF(14) - P(2,1)*SPP(8) - P(4,1)*SPP(7)) + SPP(4)*(P(1,14) - P(3,14)*SF(7) + P(11,14)*SF(12) + P(12,14)*SF(13) + P(13,14)*SF(14) - P(2,14)*SPP(8) - P(4,14)*SPP(7)) - (q0^2 - q1^2 - q2^2 + q3^2)*(P(1,16) - P(3,16)*SF(7) + P(11,16)*SF(12) + P(12,16)*SF(13) + P(13,16)*SF(14) - P(2,16)*SPP(8) - P(4,16)*SPP(7)) - (2*q0*q1 + 2*q2*q3)*(P(1,15) - P(3,15)*SF(7) + P(11,15)*SF(12) + P(12,15)*SF(13) + P(13,15)*SF(14) - P(2,15)*SPP(8) - P(4,15)*SPP(7));
nextP(2,7) = P(2,7) - P(4,7)*SF(7) + P(12,7)*SF(14) - P(13,7)*SF(13) + P(1,7)*SPP(8) + P(3,7)*SPP(7) - (2*q0*q1 + 2*q2*q3)*(P(2,15) - P(4,15)*SF(7) + P(12,15)*SF(14) - P(13,15)*SF(13) + P(1,15)*SPP(8) + P(3,15)*SPP(7) - (P(11,15)*q0)/2) - (P(11,7)*q0)/2 + SF(5)*(P(2,2) - P(4,2)*SF(7) + P(12,2)*SF(14) - P(13,2)*SF(13) + P(1,2)*SPP(8) + P(3,2)*SPP(7) - (P(11,2)*q0)/2) - SF(6)*(P(2,3) - P(4,3)*SF(7) + P(12,3)*SF(14) - P(13,3)*SF(13) + P(1,3)*SPP(8) + P(3,3)*SPP(7) - (P(11,3)*q0)/2) + SF(4)*(P(2,4) - P(4,4)*SF(7) + P(12,4)*SF(14) - P(13,4)*SF(13) + P(1,4)*SPP(8) + P(3,4)*SPP(7) - (P(11,4)*q0)/2) + SPP(1)*(P(2,1) - P(4,1)*SF(7) + P(12,1)*SF(14) - P(13,1)*SF(13) + P(1,1)*SPP(8) + P(3,1)*SPP(7) - (P(11,1)*q0)/2) + SPP(4)*(P(2,14) - P(4,14)*SF(7) + P(12,14)*SF(14) - P(13,14)*SF(13) + P(1,14)*SPP(8) + P(3,14)*SPP(7) - (P(11,14)*q0)/2) - (q0^2 - q1^2 - q2^2 + q3^2)*(P(2,16) - P(4,16)*SF(7) + P(12,16)*SF(14) - P(13,16)*SF(13) + P(1,16)*SPP(8) + P(3,16)*SPP(7) - (P(11,16)*q0)/2);
nextP(3,7) = P(3,7) + P(1,7)*SF(7) - P(11,7)*SF(14) + P(13,7)*SF(12) - P(2,7)*SPP(7) + P(4,7)*SPP(8) - (2*q0*q1 + 2*q2*q3)*(P(3,15) + P(1,15)*SF(7) - P(11,15)*SF(14) + P(13,15)*SF(12) - P(2,15)*SPP(7) + P(4,15)*SPP(8) - (P(12,15)*q0)/2) - (P(12,7)*q0)/2 + SF(5)*(P(3,2) + P(1,2)*SF(7) - P(11,2)*SF(14) + P(13,2)*SF(12) - P(2,2)*SPP(7) + P(4,2)*SPP(8) - (P(12,2)*q0)/2) - SF(6)*(P(3,3) + P(1,3)*SF(7) - P(11,3)*SF(14) + P(13,3)*SF(12) - P(2,3)*SPP(7) + P(4,3)*SPP(8) - (P(12,3)*q0)/2) + SF(4)*(P(3,4) + P(1,4)*SF(7) - P(11,4)*SF(14) + P(13,4)*SF(12) - P(2,4)*SPP(7) + P(4,4)*SPP(8) - (P(12,4)*q0)/2) + SPP(1)*(P(3,1) + P(1,1)*SF(7) - P(11,1)*SF(14) + P(13,1)*SF(12) - P(2,1)*SPP(7) + P(4,1)*SPP(8) - (P(12,1)*q0)/2) + SPP(4)*(P(3,14) + P(1,14)*SF(7) - P(11,14)*SF(14) + P(13,14)*SF(12) - P(2,14)*SPP(7) + P(4,14)*SPP(8) - (P(12,14)*q0)/2) - (q0^2 - q1^2 - q2^2 + q3^2)*(P(3,16) + P(1,16)*SF(7) - P(11,16)*SF(14) + P(13,16)*SF(12) - P(2,16)*SPP(7) + P(4,16)*SPP(8) - (P(12,16)*q0)/2);
nextP(4,7) = P(4,7) + P(2,7)*SF(7) + P(11,7)*SF(13) - P(12,7)*SF(12) + P(1,7)*SPP(7) - P(3,7)*SPP(8) - (2*q0*q1 + 2*q2*q3)*(P(4,15) + P(2,15)*SF(7) + P(11,15)*SF(13) - P(12,15)*SF(12) + P(1,15)*SPP(7) - P(3,15)*SPP(8) - (P(13,15)*q0)/2) - (P(13,7)*q0)/2 + SF(5)*(P(4,2) + P(2,2)*SF(7) + P(11,2)*SF(13) - P(12,2)*SF(12) + P(1,2)*SPP(7) - P(3,2)*SPP(8) - (P(13,2)*q0)/2) - SF(6)*(P(4,3) + P(2,3)*SF(7) + P(11,3)*SF(13) - P(12,3)*SF(12) + P(1,3)*SPP(7) - P(3,3)*SPP(8) - (P(13,3)*q0)/2) + SF(4)*(P(4,4) + P(2,4)*SF(7) + P(11,4)*SF(13) - P(12,4)*SF(12) + P(1,4)*SPP(7) - P(3,4)*SPP(8) - (P(13,4)*q0)/2) + SPP(1)*(P(4,1) + P(2,1)*SF(7) + P(11,1)*SF(13) - P(12,1)*SF(12) + P(1,1)*SPP(7) - P(3,1)*SPP(8) - (P(13,1)*q0)/2) + SPP(4)*(P(4,14) + P(2,14)*SF(7) + P(11,14)*SF(13) - P(12,14)*SF(12) + P(1,14)*SPP(7) - P(3,14)*SPP(8) - (P(13,14)*q0)/2) - (q0^2 - q1^2 - q2^2 + q3^2)*(P(4,16) + P(2,16)*SF(7) + P(11,16)*SF(13) - P(12,16)*SF(12) + P(1,16)*SPP(7) - P(3,16)*SPP(8) - (P(13,16)*q0)/2);
nextP(5,7) = P(5,7) + SQ(2) + P(1,7)*SF(6) + P(2,7)*SF(4) - P(4,7)*SF(5) + P(3,7)*SPP(1) - P(14,7)*SPP(3) + P(15,7)*SPP(6) - (q0^2 - q1^2 - q2^2 + q3^2)*(P(5,16) + P(1,16)*SF(6) + P(2,16)*SF(4) - P(4,16)*SF(5) + P(3,16)*SPP(1) - P(14,16)*SPP(3) + P(15,16)*SPP(6) - P(16,16)*(2*q0*q2 + 2*q1*q3)) - (2*q0*q1 + 2*q2*q3)*(P(5,15) + P(1,15)*SF(6) + P(2,15)*SF(4) - P(4,15)*SF(5) + P(3,15)*SPP(1) - P(14,15)*SPP(3) + P(15,15)*SPP(6) - P(16,15)*(2*q0*q2 + 2*q1*q3)) - P(16,7)*(2*q0*q2 + 2*q1*q3) + SF(5)*(P(5,2) + P(1,2)*SF(6) + P(2,2)*SF(4) - P(4,2)*SF(5) + P(3,2)*SPP(1) - P(14,2)*SPP(3) + P(15,2)*SPP(6) - P(16,2)*(2*q0*q2 + 2*q1*q3)) - SF(6)*(P(5,3) + P(1,3)*SF(6) + P(2,3)*SF(4) - P(4,3)*SF(5) + P(3,3)*SPP(1) - P(14,3)*SPP(3) + P(15,3)*SPP(6) - P(16,3)*(2*q0*q2 + 2*q1*q3)) + SF(4)*(P(5,4) + P(1,4)*SF(6) + P(2,4)*SF(4) - P(4,4)*SF(5) + P(3,4)*SPP(1) - P(14,4)*SPP(3) + P(15,4)*SPP(6) - P(16,4)*(2*q0*q2 + 2*q1*q3)) + SPP(1)*(P(5,1) + P(1,1)*SF(6) + P(2,1)*SF(4) - P(4,1)*SF(5) + P(3,1)*SPP(1) - P(14,1)*SPP(3) + P(15,1)*SPP(6) - P(16,1)*(2*q0*q2 + 2*q1*q3)) + SPP(4)*(P(5,14) + P(1,14)*SF(6) + P(2,14)*SF(4) - P(4,14)*SF(5) + P(3,14)*SPP(1) - P(14,14)*SPP(3) + P(15,14)*SPP(6) - P(16,14)*(2*q0*q2 + 2*q1*q3));
nextP(6,7) = P(6,7) + SQ(1) + P(1,7)*SF(5) + P(3,7)*SF(4) + P(4,7)*SF(6) - P(2,7)*SPP(1) - P(15,7)*SPP(2) + P(16,7)*SPP(5) - (q0^2 - q1^2 - q2^2 + q3^2)*(P(6,16) + P(1,16)*SF(5) + P(3,16)*SF(4) + P(4,16)*SF(6) - P(2,16)*SPP(1) - P(15,16)*SPP(2) + P(16,16)*SPP(5) - P(14,16)*(2*q0*q3 + 2*q1*q2)) - (2*q0*q1 + 2*q2*q3)*(P(6,15) + P(1,15)*SF(5) + P(3,15)*SF(4) + P(4,15)*SF(6) - P(2,15)*SPP(1) - P(15,15)*SPP(2) + P(16,15)*SPP(5) - P(14,15)*(2*q0*q3 + 2*q1*q2)) - P(14,7)*(2*q0*q3 + 2*q1*q2) + SF(5)*(P(6,2) + P(1,2)*SF(5) + P(3,2)*SF(4) + P(4,2)*SF(6) - P(2,2)*SPP(1) - P(15,2)*SPP(2) + P(16,2)*SPP(5) - P(14,2)*(2*q0*q3 + 2*q1*q2)) - SF(6)*(P(6,3) + P(1,3)*SF(5) + P(3,3)*SF(4) + P(4,3)*SF(6) - P(2,3)*SPP(1) - P(15,3)*SPP(2) + P(16,3)*SPP(5) - P(14,3)*(2*q0*q3 + 2*q1*q2)) + SF(4)*(P(6,4) + P(1,4)*SF(5) + P(3,4)*SF(4) + P(4,4)*SF(6) - P(2,4)*SPP(1) - P(15,4)*SPP(2) + P(16,4)*SPP(5) - P(14,4)*(2*q0*q3 + 2*q1*q2)) + SPP(1)*(P(6,1) + P(1,1)*SF(5) + P(3,1)*SF(4) + P(4,1)*SF(6) - P(2,1)*SPP(1) - P(15,1)*SPP(2) + P(16,1)*SPP(5) - P(14,1)*(2*q0*q3 + 2*q1*q2)) + SPP(4)*(P(6,14) + P(1,14)*SF(5) + P(3,14)*SF(4) + P(4,14)*SF(6) - P(2,14)*SPP(1) - P(15,14)*SPP(2) + P(16,14)*SPP(5) - P(14,14)*(2*q0*q3 + 2*q1*q2));
nextP(7,7) = P(7,7) + P(2,7)*SF(5) - P(3,7)*SF(6) + P(4,7)*SF(4) + P(1,7)*SPP(1) + P(14,7)*SPP(4) + SF(5)*(P(7,2) + P(2,2)*SF(5) - P(3,2)*SF(6) + P(4,2)*SF(4) + P(1,2)*SPP(1) + P(14,2)*SPP(4) - P(16,2)*(q0^2 - q1^2 - q2^2 + q3^2) - P(15,2)*(2*q0*q1 + 2*q2*q3)) - SF(6)*(P(7,3) + P(2,3)*SF(5) - P(3,3)*SF(6) + P(4,3)*SF(4) + P(1,3)*SPP(1) + P(14,3)*SPP(4) - P(16,3)*(q0^2 - q1^2 - q2^2 + q3^2) - P(15,3)*(2*q0*q1 + 2*q2*q3)) + SF(4)*(P(7,4) + P(2,4)*SF(5) - P(3,4)*SF(6) + P(4,4)*SF(4) + P(1,4)*SPP(1) + P(14,4)*SPP(4) - P(16,4)*(q0^2 - q1^2 - q2^2 + q3^2) - P(15,4)*(2*q0*q1 + 2*q2*q3)) + SPP(1)*(P(7,1) + P(2,1)*SF(5) - P(3,1)*SF(6) + P(4,1)*SF(4) + P(1,1)*SPP(1) + P(14,1)*SPP(4) - P(16,1)*(q0^2 - q1^2 - q2^2 + q3^2) - P(15,1)*(2*q0*q1 + 2*q2*q3)) + SPP(4)*(P(7,14) + P(2,14)*SF(5) - P(3,14)*SF(6) + P(4,14)*SF(4) + P(1,14)*SPP(1) + P(14,14)*SPP(4) - P(16,14)*(q0^2 - q1^2 - q2^2 + q3^2) - P(15,14)*(2*q0*q1 + 2*q2*q3)) - P(16,7)*(q0^2 - q1^2 - q2^2 + q3^2) + dvxCov*(SG(7) - 2*q0*q2)^2 + dvyCov*(SG(6) + 2*q0*q1)^2 - (q0^2 - q1^2 - q2^2 + q3^2)*(P(7,16) + P(2,16)*SF(5) - P(3,16)*SF(6) + P(4,16)*SF(4) + P(1,16)*SPP(1) + P(14,16)*SPP(4) - P(16,16)*(q0^2 - q1^2 - q2^2 + q3^2) - P(15,16)*(2*q0*q1 + 2*q2*q3)) + dvzCov*(SG(2) - SG(3) - SG(4) + SG(5))^2 - P(15,7)*(2*q0*q1 + 2*q2*q3) - (2*q0*q1 + 2*q2*q3)*(P(7,15) + P(2,15)*SF(5) - P(3,15)*SF(6) + P(4,15)*SF(4) + P(1,15)*SPP(1) + P(14,15)*SPP(4) - P(16,15)*(q0^2 - q1^2 - q2^2 + q3^2) - P(15,15)*(2*q0*q1 + 2*q2*q3));
nextP(1,8) = P(1,8) - P(3,8)*SF(7) + P(11,8)*SF(12) + P(12,8)*SF(13) + P(13,8)*SF(14) - P(2,8)*SPP(8) - P(4,8)*SPP(7) + dt*(P(1,5) - P(3,5)*SF(7) + P(11,5)*SF(12) + P(12,5)*SF(13) + P(13,5)*SF(14) - P(2,5)*SPP(8) - P(4,5)*SPP(7));
nextP(2,8) = P(2,8) - P(4,8)*SF(7) + P(12,8)*SF(14) - P(13,8)*SF(13) + P(1,8)*SPP(8) + P(3,8)*SPP(7) - (P(11,8)*q0)/2 + dt*(P(2,5) - P(4,5)*SF(7) + P(12,5)*SF(14) - P(13,5)*SF(13) + P(1,5)*SPP(8) + P(3,5)*SPP(7) - (P(11,5)*q0)/2);
nextP(3,8) = P(3,8) + P(1,8)*SF(7) - P(11,8)*SF(14) + P(13,8)*SF(12) - P(2,8)*SPP(7) + P(4,8)*SPP(8) - (P(12,8)*q0)/2 + dt*(P(3,5) + P(1,5)*SF(7) - P(11,5)*SF(14) + P(13,5)*SF(12) - P(2,5)*SPP(7) + P(4,5)*SPP(8) - (P(12,5)*q0)/2);
nextP(4,8) = P(4,8) + P(2,8)*SF(7) + P(11,8)*SF(13) - P(12,8)*SF(12) + P(1,8)*SPP(7) - P(3,8)*SPP(8) - (P(13,8)*q0)/2 + dt*(P(4,5) + P(2,5)*SF(7) + P(11,5)*SF(13) - P(12,5)*SF(12) + P(1,5)*SPP(7) - P(3,5)*SPP(8) - (P(13,5)*q0)/2);
nextP(5,8) = P(5,8) + P(1,8)*SF(6) + P(2,8)*SF(4) - P(4,8)*SF(5) + P(3,8)*SPP(1) - P(14,8)*SPP(3) + P(15,8)*SPP(6) + dt*(P(5,5) + P(1,5)*SF(6) + P(2,5)*SF(4) - P(4,5)*SF(5) + P(3,5)*SPP(1) - P(14,5)*SPP(3) + P(15,5)*SPP(6) - P(16,5)*(2*q0*q2 + 2*q1*q3)) - P(16,8)*(2*q0*q2 + 2*q1*q3);
nextP(6,8) = P(6,8) + P(1,8)*SF(5) + P(3,8)*SF(4) + P(4,8)*SF(6) - P(2,8)*SPP(1) - P(15,8)*SPP(2) + P(16,8)*SPP(5) + dt*(P(6,5) + P(1,5)*SF(5) + P(3,5)*SF(4) + P(4,5)*SF(6) - P(2,5)*SPP(1) - P(15,5)*SPP(2) + P(16,5)*SPP(5) - P(14,5)*(2*q0*q3 + 2*q1*q2)) - P(14,8)*(2*q0*q3 + 2*q1*q2);
nextP(7,8) = P(7,8) + P(2,8)*SF(5) - P(3,8)*SF(6) + P(4,8)*SF(4) + P(1,8)*SPP(1) + P(14,8)*SPP(4) - P(16,8)*(q0^2 - q1^2 - q2^2 + q3^2) + dt*(P(7,5) + P(2,5)*SF(5) - P(3,5)*SF(6) + P(4,5)*SF(4) + P(1,5)*SPP(1) + P(14,5)*SPP(4) - P(16,5)*(q0^2 - q1^2 - q2^2 + q3^2) - P(15,5)*(2*q0*q1 + 2*q2*q3)) - P(15,8)*(2*q0*q1 + 2*q2*q3);
nextP(8,8) = P(8,8) + P(5,8)*dt + dt*(P(8,5) + P(5,5)*dt);
nextP(1,9) = P(1,9) - P(3,9)*SF(7) + P(11,9)*SF(12) + P(12,9)*SF(13) + P(13,9)*SF(14) - P(2,9)*SPP(8) - P(4,9)*SPP(7) + dt*(P(1,6) - P(3,6)*SF(7) + P(11,6)*SF(12) + P(12,6)*SF(13) + P(13,6)*SF(14) - P(2,6)*SPP(8) - P(4,6)*SPP(7));
nextP(2,9) = P(2,9) - P(4,9)*SF(7) + P(12,9)*SF(14) - P(13,9)*SF(13) + P(1,9)*SPP(8) + P(3,9)*SPP(7) - (P(11,9)*q0)/2 + dt*(P(2,6) - P(4,6)*SF(7) + P(12,6)*SF(14) - P(13,6)*SF(13) + P(1,6)*SPP(8) + P(3,6)*SPP(7) - (P(11,6)*q0)/2);
nextP(3,9) = P(3,9) + P(1,9)*SF(7) - P(11,9)*SF(14) + P(13,9)*SF(12) - P(2,9)*SPP(7) + P(4,9)*SPP(8) - (P(12,9)*q0)/2 + dt*(P(3,6) + P(1,6)*SF(7) - P(11,6)*SF(14) + P(13,6)*SF(12) - P(2,6)*SPP(7) + P(4,6)*SPP(8) - (P(12,6)*q0)/2);
nextP(4,9) = P(4,9) + P(2,9)*SF(7) + P(11,9)*SF(13) - P(12,9)*SF(12) + P(1,9)*SPP(7) - P(3,9)*SPP(8) - (P(13,9)*q0)/2 + dt*(P(4,6) + P(2,6)*SF(7) + P(11,6)*SF(13) - P(12,6)*SF(12) + P(1,6)*SPP(7) - P(3,6)*SPP(8) - (P(13,6)*q0)/2);
nextP(5,9) = P(5,9) + P(1,9)*SF(6) + P(2,9)*SF(4) - P(4,9)*SF(5) + P(3,9)*SPP(1) - P(14,9)*SPP(3) + P(15,9)*SPP(6) + dt*(P(5,6) + P(1,6)*SF(6) + P(2,6)*SF(4) - P(4,6)*SF(5) + P(3,6)*SPP(1) - P(14,6)*SPP(3) + P(15,6)*SPP(6) - P(16,6)*(2*q0*q2 + 2*q1*q3)) - P(16,9)*(2*q0*q2 + 2*q1*q3);
nextP(6,9) = P(6,9) + P(1,9)*SF(5) + P(3,9)*SF(4) + P(4,9)*SF(6) - P(2,9)*SPP(1) - P(15,9)*SPP(2) + P(16,9)*SPP(5) + dt*(P(6,6) + P(1,6)*SF(5) + P(3,6)*SF(4) + P(4,6)*SF(6) - P(2,6)*SPP(1) - P(15,6)*SPP(2) + P(16,6)*SPP(5) - P(14,6)*(2*q0*q3 + 2*q1*q2)) - P(14,9)*(2*q0*q3 + 2*q1*q2);
nextP(7,9) = P(7,9) + P(2,9)*SF(5) - P(3,9)*SF(6) + P(4,9)*SF(4) + P(1,9)*SPP(1) + P(14,9)*SPP(4) - P(16,9)*(q0^2 - q1^2 - q2^2 + q3^2) + dt*(P(7,6) + P(2,6)*SF(5) - P(3,6)*SF(6) + P(4,6)*SF(4) + P(1,6)*SPP(1) + P(14,6)*SPP(4) - P(16,6)*(q0^2 - q1^2 - q2^2 + q3^2) - P(15,6)*(2*q0*q1 + 2*q2*q3)) - P(15,9)*(2*q0*q1 + 2*q2*q3);
nextP(8,9) = P(8,9) + P(5,9)*dt + dt*(P(8,6) + P(5,6)*dt);
nextP(9,9) = P(9,9) + P(6,9)*dt + dt*(P(9,6) + P(6,6)*dt);
nextP(1,10) = P(1,10) - P(3,10)*SF(7) + P(11,10)*SF(12) + P(12,10)*SF(13) + P(13,10)*SF(14) - P(2,10)*SPP(8) - P(4,10)*SPP(7) + dt*(P(1,7) - P(3,7)*SF(7) + P(11,7)*SF(12) + P(12,7)*SF(13) + P(13,7)*SF(14) - P(2,7)*SPP(8) - P(4,7)*SPP(7));
nextP(2,10) = P(2,10) - P(4,10)*SF(7) + P(12,10)*SF(14) - P(13,10)*SF(13) + P(1,10)*SPP(8) + P(3,10)*SPP(7) - (P(11,10)*q0)/2 + dt*(P(2,7) - P(4,7)*SF(7) + P(12,7)*SF(14) - P(13,7)*SF(13) + P(1,7)*SPP(8) + P(3,7)*SPP(7) - (P(11,7)*q0)/2);
nextP(3,10) = P(3,10) + P(1,10)*SF(7) - P(11,10)*SF(14) + P(13,10)*SF(12) - P(2,10)*SPP(7) + P(4,10)*SPP(8) - (P(12,10)*q0)/2 + dt*(P(3,7) + P(1,7)*SF(7) - P(11,7)*SF(14) + P(13,7)*SF(12) - P(2,7)*SPP(7) + P(4,7)*SPP(8) - (P(12,7)*q0)/2);
nextP(4,10) = P(4,10) + P(2,10)*SF(7) + P(11,10)*SF(13) - P(12,10)*SF(12) + P(1,10)*SPP(7) - P(3,10)*SPP(8) - (P(13,10)*q0)/2 + dt*(P(4,7) + P(2,7)*SF(7) + P(11,7)*SF(13) - P(12,7)*SF(12) + P(1,7)*SPP(7) - P(3,7)*SPP(8) - (P(13,7)*q0)/2);
nextP(5,10) = P(5,10) + P(1,10)*SF(6) + P(2,10)*SF(4) - P(4,10)*SF(5) + P(3,10)*SPP(1) - P(14,10)*SPP(3) + P(15,10)*SPP(6) + dt*(P(5,7) + P(1,7)*SF(6) + P(2,7)*SF(4) - P(4,7)*SF(5) + P(3,7)*SPP(1) - P(14,7)*SPP(3) + P(15,7)*SPP(6) - P(16,7)*(2*q0*q2 + 2*q1*q3)) - P(16,10)*(2*q0*q2 + 2*q1*q3);
nextP(6,10) = P(6,10) + P(1,10)*SF(5) + P(3,10)*SF(4) + P(4,10)*SF(6) - P(2,10)*SPP(1) - P(15,10)*SPP(2) + P(16,10)*SPP(5) + dt*(P(6,7) + P(1,7)*SF(5) + P(3,7)*SF(4) + P(4,7)*SF(6) - P(2,7)*SPP(1) - P(15,7)*SPP(2) + P(16,7)*SPP(5) - P(14,7)*(2*q0*q3 + 2*q1*q2)) - P(14,10)*(2*q0*q3 + 2*q1*q2);
nextP(7,10) = P(7,10) + P(2,10)*SF(5) - P(3,10)*SF(6) + P(4,10)*SF(4) + P(1,10)*SPP(1) + P(14,10)*SPP(4) - P(16,10)*(q0^2 - q1^2 - q2^2 + q3^2) + dt*(P(7,7) + P(2,7)*SF(5) - P(3,7)*SF(6) + P(4,7)*SF(4) + P(1,7)*SPP(1) + P(14,7)*SPP(4) - P(16,7)*(q0^2 - q1^2 - q2^2 + q3^2) - P(15,7)*(2*q0*q1 + 2*q2*q3)) - P(15,10)*(2*q0*q1 + 2*q2*q3);
nextP(8,10) = P(8,10) + P(5,10)*dt + dt*(P(8,7) + P(5,7)*dt);
nextP(9,10) = P(9,10) + P(6,10)*dt + dt*(P(9,7) + P(6,7)*dt);
nextP(10,10) = P(10,10) + P(7,10)*dt + dt*(P(10,7) + P(7,7)*dt);
nextP(1,11) = P(1,11) - P(3,11)*SF(7) + P(11,11)*SF(12) + P(12,11)*SF(13) + P(13,11)*SF(14) - P(2,11)*SPP(8) - P(4,11)*SPP(7);
nextP(2,11) = P(2,11) - P(4,11)*SF(7) + P(12,11)*SF(14) - P(13,11)*SF(13) + P(1,11)*SPP(8) + P(3,11)*SPP(7) - (P(11,11)*q0)/2;
nextP(3,11) = P(3,11) + P(1,11)*SF(7) - P(11,11)*SF(14) + P(13,11)*SF(12) - P(2,11)*SPP(7) + P(4,11)*SPP(8) - (P(12,11)*q0)/2;
nextP(4,11) = P(4,11) + P(2,11)*SF(7) + P(11,11)*SF(13) - P(12,11)*SF(12) + P(1,11)*SPP(7) - P(3,11)*SPP(8) - (P(13,11)*q0)/2;
nextP(5,11) = P(5,11) + P(1,11)*SF(6) + P(2,11)*SF(4) - P(4,11)*SF(5) + P(3,11)*SPP(1) - P(14,11)*SPP(3) + P(15,11)*SPP(6) - P(16,11)*(2*q0*q2 + 2*q1*q3);
nextP(6,11) = P(6,11) + P(1,11)*SF(5) + P(3,11)*SF(4) + P(4,11)*SF(6) - P(2,11)*SPP(1) - P(15,11)*SPP(2) + P(16,11)*SPP(5) - P(14,11)*(2*q0*q3 + 2*q1*q2);
nextP(7,11) = P(7,11) + P(2,11)*SF(5) - P(3,11)*SF(6) + P(4,11)*SF(4) + P(1,11)*SPP(1) + P(14,11)*SPP(4) - P(16,11)*(q0^2 - q1^2 - q2^2 + q3^2) - P(15,11)*(2*q0*q1 + 2*q2*q3);
nextP(8,11) = P(8,11) + P(5,11)*dt;
nextP(9,11) = P(9,11) + P(6,11)*dt;
nextP(10,11) = P(10,11) + P(7,11)*dt;
nextP(11,11) = P(11,11);
nextP(1,12) = P(1,12) - P(3,12)*SF(7) + P(11,12)*SF(12) + P(12,12)*SF(13) + P(13,12)*SF(14) - P(2,12)*SPP(8) - P(4,12)*SPP(7);
nextP(2,12) = P(2,12) - P(4,12)*SF(7) + P(12,12)*SF(14) - P(13,12)*SF(13) + P(1,12)*SPP(8) + P(3,12)*SPP(7) - (P(11,12)*q0)/2;
nextP(3,12) = P(3,12) + P(1,12)*SF(7) - P(11,12)*SF(14) + P(13,12)*SF(12) - P(2,12)*SPP(7) + P(4,12)*SPP(8) - (P(12,12)*q0)/2;
nextP(4,12) = P(4,12) + P(2,12)*SF(7) + P(11,12)*SF(13) - P(12,12)*SF(12) + P(1,12)*SPP(7) - P(3,12)*SPP(8) - (P(13,12)*q0)/2;
nextP(5,12) = P(5,12) + P(1,12)*SF(6) + P(2,12)*SF(4) - P(4,12)*SF(5) + P(3,12)*SPP(1) - P(14,12)*SPP(3) + P(15,12)*SPP(6) - P(16,12)*(2*q0*q2 + 2*q1*q3);
nextP(6,12) = P(6,12) + P(1,12)*SF(5) + P(3,12)*SF(4) + P(4,12)*SF(6) - P(2,12)*SPP(1) - P(15,12)*SPP(2) + P(16,12)*SPP(5) - P(14,12)*(2*q0*q3 + 2*q1*q2);
nextP(7,12) = P(7,12) + P(2,12)*SF(5) - P(3,12)*SF(6) + P(4,12)*SF(4) + P(1,12)*SPP(1) + P(14,12)*SPP(4) - P(16,12)*(q0^2 - q1^2 - q2^2 + q3^2) - P(15,12)*(2*q0*q1 + 2*q2*q3);
nextP(8,12) = P(8,12) + P(5,12)*dt;
nextP(9,12) = P(9,12) + P(6,12)*dt;
nextP(10,12) = P(10,12) + P(7,12)*dt;
nextP(11,12) = P(11,12);
nextP(12,12) = P(12,12);
nextP(1,13) = P(1,13) - P(3,13)*SF(7) + P(11,13)*SF(12) + P(12,13)*SF(13) + P(13,13)*SF(14) - P(2,13)*SPP(8) - P(4,13)*SPP(7);
nextP(2,13) = P(2,13) - P(4,13)*SF(7) + P(12,13)*SF(14) - P(13,13)*SF(13) + P(1,13)*SPP(8) + P(3,13)*SPP(7) - (P(11,13)*q0)/2;
nextP(3,13) = P(3,13) + P(1,13)*SF(7) - P(11,13)*SF(14) + P(13,13)*SF(12) - P(2,13)*SPP(7) + P(4,13)*SPP(8) - (P(12,13)*q0)/2;
nextP(4,13) = P(4,13) + P(2,13)*SF(7) + P(11,13)*SF(13) - P(12,13)*SF(12) + P(1,13)*SPP(7) - P(3,13)*SPP(8) - (P(13,13)*q0)/2;
nextP(5,13) = P(5,13) + P(1,13)*SF(6) + P(2,13)*SF(4) - P(4,13)*SF(5) + P(3,13)*SPP(1) - P(14,13)*SPP(3) + P(15,13)*SPP(6) - P(16,13)*(2*q0*q2 + 2*q1*q3);
nextP(6,13) = P(6,13) + P(1,13)*SF(5) + P(3,13)*SF(4) + P(4,13)*SF(6) - P(2,13)*SPP(1) - P(15,13)*SPP(2) + P(16,13)*SPP(5) - P(14,13)*(2*q0*q3 + 2*q1*q2);
nextP(7,13) = P(7,13) + P(2,13)*SF(5) - P(3,13)*SF(6) + P(4,13)*SF(4) + P(1,13)*SPP(1) + P(14,13)*SPP(4) - P(16,13)*(q0^2 - q1^2 - q2^2 + q3^2) - P(15,13)*(2*q0*q1 + 2*q2*q3);
nextP(8,13) = P(8,13) + P(5,13)*dt;
nextP(9,13) = P(9,13) + P(6,13)*dt;
nextP(10,13) = P(10,13) + P(7,13)*dt;
nextP(11,13) = P(11,13);
nextP(12,13) = P(12,13);
nextP(13,13) = P(13,13);
nextP(1,14) = P(1,14) - P(3,14)*SF(7) + P(11,14)*SF(12) + P(12,14)*SF(13) + P(13,14)*SF(14) - P(2,14)*SPP(8) - P(4,14)*SPP(7);
nextP(2,14) = P(2,14) - P(4,14)*SF(7) + P(12,14)*SF(14) - P(13,14)*SF(13) + P(1,14)*SPP(8) + P(3,14)*SPP(7) - (P(11,14)*q0)/2;
nextP(3,14) = P(3,14) + P(1,14)*SF(7) - P(11,14)*SF(14) + P(13,14)*SF(12) - P(2,14)*SPP(7) + P(4,14)*SPP(8) - (P(12,14)*q0)/2;
nextP(4,14) = P(4,14) + P(2,14)*SF(7) + P(11,14)*SF(13) - P(12,14)*SF(12) + P(1,14)*SPP(7) - P(3,14)*SPP(8) - (P(13,14)*q0)/2;
nextP(5,14) = P(5,14) + P(1,14)*SF(6) + P(2,14)*SF(4) - P(4,14)*SF(5) + P(3,14)*SPP(1) - P(14,14)*SPP(3) + P(15,14)*SPP(6) - P(16,14)*(2*q0*q2 + 2*q1*q3);
nextP(6,14) = P(6,14) + P(1,14)*SF(5) + P(3,14)*SF(4) + P(4,14)*SF(6) - P(2,14)*SPP(1) - P(15,14)*SPP(2) + P(16,14)*SPP(5) - P(14,14)*(2*q0*q3 + 2*q1*q2);
nextP(7,14) = P(7,14) + P(2,14)*SF(5) - P(3,14)*SF(6) + P(4,14)*SF(4) + P(1,14)*SPP(1) + P(14,14)*SPP(4) - P(16,14)*(q0^2 - q1^2 - q2^2 + q3^2) - P(15,14)*(2*q0*q1 + 2*q2*q3);
nextP(8,14) = P(8,14) + P(5,14)*dt;
nextP(9,14) = P(9,14) + P(6,14)*dt;
nextP(10,14) = P(10,14) + P(7,14)*dt;
nextP(11,14) = P(11,14);
nextP(12,14) = P(12,14);
nextP(13,14) = P(13,14);
nextP(14,14) = P(14,14);
nextP(1,15) = P(1,15) - P(3,15)*SF(7) + P(11,15)*SF(12) + P(12,15)*SF(13) + P(13,15)*SF(14) - P(2,15)*SPP(8) - P(4,15)*SPP(7);
nextP(2,15) = P(2,15) - P(4,15)*SF(7) + P(12,15)*SF(14) - P(13,15)*SF(13) + P(1,15)*SPP(8) + P(3,15)*SPP(7) - (P(11,15)*q0)/2;
nextP(3,15) = P(3,15) + P(1,15)*SF(7) - P(11,15)*SF(14) + P(13,15)*SF(12) - P(2,15)*SPP(7) + P(4,15)*SPP(8) - (P(12,15)*q0)/2;
nextP(4,15) = P(4,15) + P(2,15)*SF(7) + P(11,15)*SF(13) - P(12,15)*SF(12) + P(1,15)*SPP(7) - P(3,15)*SPP(8) - (P(13,15)*q0)/2;
nextP(5,15) = P(5,15) + P(1,15)*SF(6) + P(2,15)*SF(4) - P(4,15)*SF(5) + P(3,15)*SPP(1) - P(14,15)*SPP(3) + P(15,15)*SPP(6) - P(16,15)*(2*q0*q2 + 2*q1*q3);
nextP(6,15) = P(6,15) + P(1,15)*SF(5) + P(3,15)*SF(4) + P(4,15)*SF(6) - P(2,15)*SPP(1) - P(15,15)*SPP(2) + P(16,15)*SPP(5) - P(14,15)*(2*q0*q3 + 2*q1*q2);
nextP(7,15) = P(7,15) + P(2,15)*SF(5) - P(3,15)*SF(6) + P(4,15)*SF(4) + P(1,15)*SPP(1) + P(14,15)*SPP(4) - P(16,15)*(q0^2 - q1^2 - q2^2 + q3^2) - P(15,15)*(2*q0*q1 + 2*q2*q3);
nextP(8,15) = P(8,15) + P(5,15)*dt;
nextP(9,15) = P(9,15) + P(6,15)*dt;
nextP(10,15) = P(10,15) + P(7,15)*dt;
nextP(11,15) = P(11,15);
nextP(12,15) = P(12,15);
nextP(13,15) = P(13,15);
nextP(14,15) = P(14,15);
nextP(15,15) = P(15,15);
nextP(1,16) = P(1,16) - P(3,16)*SF(7) + P(11,16)*SF(12) + P(12,16)*SF(13) + P(13,16)*SF(14) - P(2,16)*SPP(8) - P(4,16)*SPP(7);
nextP(2,16) = P(2,16) - P(4,16)*SF(7) + P(12,16)*SF(14) - P(13,16)*SF(13) + P(1,16)*SPP(8) + P(3,16)*SPP(7) - (P(11,16)*q0)/2;
nextP(3,16) = P(3,16) + P(1,16)*SF(7) - P(11,16)*SF(14) + P(13,16)*SF(12) - P(2,16)*SPP(7) + P(4,16)*SPP(8) - (P(12,16)*q0)/2;
nextP(4,16) = P(4,16) + P(2,16)*SF(7) + P(11,16)*SF(13) - P(12,16)*SF(12) + P(1,16)*SPP(7) - P(3,16)*SPP(8) - (P(13,16)*q0)/2;
nextP(5,16) = P(5,16) + P(1,16)*SF(6) + P(2,16)*SF(4) - P(4,16)*SF(5) + P(3,16)*SPP(1) - P(14,16)*SPP(3) + P(15,16)*SPP(6) - P(16,16)*(2*q0*q2 + 2*q1*q3);
nextP(6,16) = P(6,16) + P(1,16)*SF(5) + P(3,16)*SF(4) + P(4,16)*SF(6) - P(2,16)*SPP(1) - P(15,16)*SPP(2) + P(16,16)*SPP(5) - P(14,16)*(2*q0*q3 + 2*q1*q2);
nextP(7,16) = P(7,16) + P(2,16)*SF(5) - P(3,16)*SF(6) + P(4,16)*SF(4) + P(1,16)*SPP(1) + P(14,16)*SPP(4) - P(16,16)*(q0^2 - q1^2 - q2^2 + q3^2) - P(15,16)*(2*q0*q1 + 2*q2*q3);
nextP(8,16) = P(8,16) + P(5,16)*dt;
nextP(9,16) = P(9,16) + P(6,16)*dt;
nextP(10,16) = P(10,16) + P(7,16)*dt;
nextP(11,16) = P(11,16);
nextP(12,16) = P(12,16);
nextP(13,16) = P(13,16);
nextP(14,16) = P(14,16);
nextP(15,16) = P(15,16);
nextP(16,16) = P(16,16);
% Add the process noise
for i = 1:16
nextP(i,i) = nextP(i,i) + processNoise(i);
end
% If the total position variance exceds 1E6 (1000m), then stop covariance
% growth by setting the predicted to the previous values
% This prevent an ill conditioned matrix from occurring for long periods
% without GPS
if (P(8,8) + P(9,9)) > 1E6
nextP(8:9,:) = P(8:9,:);
nextP(:,8:9) = P(:,8:9);
end
% Force symmetry on the covariance matrix to prevent ill-conditioning
% of the matrix which would cause the filter to blow-up
for i = 2:16
for j = 1:(i-1)
temp = 0.5*(nextP(i,j) + nextP(j,i));
nextP(i,j) = temp;
nextP(j,i) = temp;
end
end
end