-
Notifications
You must be signed in to change notification settings - Fork 0
/
rbpf.html
484 lines (469 loc) · 107 KB
/
rbpf.html
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
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
<html xmlns="http://www.w3.org/1999/xhtml">
<head>
<meta http-equiv="Content-Type" content="text/html; charset=utf-8" />
<meta http-equiv="Content-Style-Type" content="text/css" />
<meta name="generator" content="pandoc" />
<meta name="author" content="Ruben Fiszel" />
<title>Sensor fusion for drones: Rao-Blackwellized Particle filter for POSE and wind estimation</title>
<style type="text/css">code{white-space: pre;}</style>
<style type="text/css">
div.sourceCode { overflow-x: auto; }
table.sourceCode, tr.sourceCode, td.lineNumbers, td.sourceCode {
margin: 0; padding: 0; vertical-align: baseline; border: none; }
table.sourceCode { width: 100%; line-height: 100%; }
td.lineNumbers { text-align: right; padding-right: 4px; padding-left: 4px; color: #aaaaaa; border-right: 1px solid #aaaaaa; }
td.sourceCode { padding-left: 5px; }
code > span.kw { color: #007020; font-weight: bold; } /* Keyword */
code > span.dt { color: #902000; } /* DataType */
code > span.dv { color: #40a070; } /* DecVal */
code > span.bn { color: #40a070; } /* BaseN */
code > span.fl { color: #40a070; } /* Float */
code > span.ch { color: #4070a0; } /* Char */
code > span.st { color: #4070a0; } /* String */
code > span.co { color: #60a0b0; font-style: italic; } /* Comment */
code > span.ot { color: #007020; } /* Other */
code > span.al { color: #ff0000; font-weight: bold; } /* Alert */
code > span.fu { color: #06287e; } /* Function */
code > span.er { color: #ff0000; font-weight: bold; } /* Error */
code > span.wa { color: #60a0b0; font-weight: bold; font-style: italic; } /* Warning */
code > span.cn { color: #880000; } /* Constant */
code > span.sc { color: #4070a0; } /* SpecialChar */
code > span.vs { color: #4070a0; } /* VerbatimString */
code > span.ss { color: #bb6688; } /* SpecialString */
code > span.im { } /* Import */
code > span.va { color: #19177c; } /* Variable */
code > span.cf { color: #007020; font-weight: bold; } /* ControlFlow */
code > span.op { color: #666666; } /* Operator */
code > span.bu { } /* BuiltIn */
code > span.ex { } /* Extension */
code > span.pp { color: #bc7a00; } /* Preprocessor */
code > span.at { color: #7d9029; } /* Attribute */
code > span.do { color: #ba2121; font-style: italic; } /* Documentation */
code > span.an { color: #60a0b0; font-weight: bold; font-style: italic; } /* Annotation */
code > span.cv { color: #60a0b0; font-weight: bold; font-style: italic; } /* CommentVar */
code > span.in { color: #60a0b0; font-weight: bold; font-style: italic; } /* Information */
</style>
<link rel="stylesheet" href="markdown3.css" type="text/css" />
<script src="https://cdn.mathjax.org/mathjax/latest/MathJax.js?config=TeX-AMS_CHTML-full" type="text/javascript"></script>
</head>
<body>
<div id="header">
<h1 class="title">Sensor fusion for drones: Rao-Blackwellized Particle filter for POSE and wind estimation</h1>
<h2 class="author">Ruben Fiszel</h2>
<h3 class="date">June 2017</h3>
</div>
<h1 id="introduction">Introduction</h1>
<p>We will introduce here several filter for POSE estimation for highly dynamic objects and in particular drones. The order is from the most conceptually simple, to the most complex. The Rao-Blackwellized Particle filter can be viewed as a superset of many other filters, where those filters are just instances of a particle filter with only one particle.</p>
<h2 id="drone">Drone</h2>
<p><strong>TODO</strong>: Talk about drone, high-dynamic objects</p>
<h2 id="notes-on-notation-and-conventions">Notes on notation and conventions</h2>
<p>The referential by default is the fixed world frame.</p>
<ul>
<li><span class="math inline">\(\mathbf{x}\)</span> designates a vector</li>
<li><span class="math inline">\(x_t\)</span> is the random variable of x at time t</li>
<li><span class="math inline">\(x_{t1:t2}\)</span> is the product of the random variable of x between t1 included and t2 included</li>
<li><span class="math inline">\(x^{(i)}\)</span> designates the random variable x of the arbitrary particle i</li>
<li><span class="math inline">\(\hat{x}\)</span> designates an estimated variable</li>
</ul>
<h2 id="pose">POSE</h2>
<p>POSE is the task of determining the position and orientation of an object through time. It is a subroutine of SLAM (Software Localization And Mapping). We can formelize the problem as:</p>
<p>At each timestep, find the best expectation of a function of the hidden variable state (position and orientation), from their initial distribution and the history of observable random variables (such as sensor measurements).</p>
<ul>
<li>The state <span class="math inline">\(\mathbf{x}\)</span></li>
<li>The function <span class="math inline">\(g(\mathbf{x})\)</span> such that <span class="math inline">\(g(\mathbf{x}_t) = (\mathbf{p}_t, \mathbf{q}_t)\)</span> where <span class="math inline">\(\mathbf{p}\)</span> is the position and <span class="math inline">\(\mathbf{q}\)</span> is the attitude as a quaternion.</li>
<li>The observable variable <span class="math inline">\(\mathbf{y}\)</span> composed of the sensor measurements <span class="math inline">\(\mathbf{z}\)</span> and the control input <span class="math inline">\(\mathbf{u}\)</span></li>
</ul>
<p>The algorithm inputs are:</p>
<ul>
<li>control inputs <span class="math inline">\(\mathbf{u}_t\)</span> (the commands sent to the flight controller)</li>
<li>sensor measurements <span class="math inline">\(\mathbf{z}_t\)</span> coming from different sensors with different sampling rate</li>
<li>information about the sensors (sensor measurements biases and matrix of covariance)</li>
</ul>
<h3 id="wind">Wind</h3>
<p>We will add one novel subtlety over vanilla POSE is that we would like to also keep track of the wind force. This would enable us to do smooth motion planning in windy environments instead of limiting ourselves to indoors.</p>
<h2 id="quaternion">Quaternion</h2>
<p>Quaternions are extensions of complex numbers but with 3 imaginary parts. Unit quaternions can be used to represent orientation, also referred to as attitude. Quaternions algebra make rotation composition simple and quaternions avoid the issue of gimbal lock. In all filters presented, they will be used to represent the attitude.</p>
<p>Quaternion rotations composition is: <span class="math inline">\(q_2 q_1\)</span> which results in <span class="math inline">\(q_1\)</span> being rotated by the rotation represented by <span class="math inline">\(q_2\)</span>. From this, we can deduce that angular velocity integrated over time is simply <span class="math inline">\(q^t\)</span> if <span class="math inline">\(q\)</span> is the local quaternion rotation by unit of time.</p>
<p>Rotation of a vector by a quaternion is done by: <span class="math inline">\(q v q^*\)</span> where <span class="math inline">\(q\)</span> is the quaternion representing the rotation, <span class="math inline">\(q^*\)</span> its conjugate and <span class="math inline">\(v\)</span> the vector to be rotated.</p>
<h3 id="average-quaternion">Average quaternion</h3>
<p>It will be useful later to calculate the average quaternion of a set of quaternions. Unfortunately, the average of its components <span class="math inline">\(\frac{1}{N} \sum q_i\)</span> or <em>barycentric</em> mean is unsound: Indeed, attitude do not belong to a vector space but a homogenous Riemannian manifold (the four dimensional unit sphere). To convince yourself of the unsoundness of the <em>barycentric</em> mean, see that the addition and barycentric mean of two unit quaternion is not necessarily an unit quaternion (<span class="math inline">\((1, 0, 0, 0)\)</span> and <span class="math inline">\((-1, 0, 0, 0)\)</span> for instance. Furthermore, angle being periodic, the <em>barycentric</em> mean of a quaternion with angle <span class="math inline">\(-178^\circ\)</span> and another with same body-axis and angle <span class="math inline">\(180^\circ\)</span> gives <span class="math inline">\(1^\circ\)</span> instead of the expected <span class="math inline">\(-179^\circ\)</span>.</p>
<p>We present here two solutions to calculate the average quaternion:</p>
<ul>
<li>Use the barycentric mean but negate the components of the quaternion that do not belong to the same demi-sphere as an arbitrary quaternion in the set. This solves the aberrant case above and is practical. It is a simplified form of the algorithm presented in <span class="citation">(Markley et al. <a href="#ref-markley_averaging_2007">2007</a>)</span>.</li>
<li>Use the the intrinsic gradient descent algorithm <span class="citation">(Xavier <a href="#ref-xavier_computing_1998">1998</a>)</span> . Starting with an abitrary mean <span class="math inline">\(\bar{\mathbf{q}}\)</span> (we use the one from the previously described method), we can iteratively improve the convergence between the quaternions. <span class="math display">\[\mathbf{e_i} = \mathbf{q_i}\bar{\mathbf{q}}_t^{-1}\]</span> such that <span class="math display">\[\mathbf{q_i} = \mathbf{e_i}\bar{\mathbf{q}}\]</span> <span class="math display">\[\mathbf{e} = \frac{1}{2n}\sum \mathbf{e_i}\]</span> <span class="math display">\[\bar{\mathbf{q}}_{t+1} = \mathbf{e}\bar{\mathbf{q}}_t\]</span></li>
</ul>
<p>We iterate until <span class="math inline">\(e\)</span> norm is satisfying or after a fixed number of iteration.</p>
<h2 id="helper-functions-and-matrices">Helper functions and matrices</h2>
<p>We introduce some helper matrices.</p>
<ul>
<li><span class="math inline">\(\mathbf{R}_{b2f}\{\mathbf{q}\}\)</span> is the body to fixed vector rotation matrix. It transforms vector in the body frame to the fixed world frame. It takes as parameter the attitude <span class="math inline">\(q\)</span>.</li>
<li><span class="math inline">\(\mathbf{R}_{f2b}\{\mathbf{q}\}\)</span> is its inverse matrix (from fixed to body).</li>
<li><span class="math inline">\(\mathbf{T}_{2a} = (0, 0, 1/m)^T\)</span> is the scaling from thrust to acceleration (by dividing by the weight of the drone: <span class="math inline">\(\mathbf{F} = m\mathbf{a} \Rightarrow \mathbf{a} = \mathbf{F}/m)\)</span> and then multiplying by a unit vector <span class="math inline">\((0, 0, 1)\)</span></li>
<li><span class="math display">\[R2Q(\boldsymbol{\theta}) = (\cos(\| \boldsymbol{\theta} \| / 2), \sin(\| \boldsymbol{\theta} \| / 2) \frac{\boldsymbol{\theta}}{\| \boldsymbol{\theta} \|} )\]</span> is a function that convert from a local angle rotation to a local quaternion rotation. The definition of this function come from converting <span class="math inline">\(\boldsymbol{\theta}\)</span> to a body-axis angle, and then to a quaternion.</li>
<li><span class="math display">\[Q2R(\mathbf{q}) = (q.i*s, q.j*s, q.k*s) \]</span> is its inverse function where <span class="math inline">\(n = \arccos(q.w)*2\)</span> and <span class="math inline">\(s = n/\sin(n/2)\)</span></li>
<li><span class="math inline">\(\Delta t\)</span> is the lapse of time between t and the next tick (t+1)</li>
</ul>
<h2 id="model">Model</h2>
<p>The drone is submittied to the wind, the gravity and its own inertia.</p>
<ul>
<li><span class="math inline">\(\mathbf{w}\)</span> the wind acceleration</li>
<li><span class="math inline">\(\mathbf{a}\)</span> the total acceleration</li>
<li><span class="math inline">\(\mathbf{v}\)</span> the velocity</li>
<li><span class="math inline">\(\mathbf{p}\)</span> the position</li>
<li><span class="math inline">\(\boldsymbol{\omega}\)</span> the angular velocity</li>
<li><span class="math inline">\(\mathbf{q}\)</span> the attitude</li>
</ul>
<h2 id="sensors">Sensors</h2>
<p>This work was in collaboration with the ASL Stanford lab for indoor POSE of drones. The sensors at disposition are:</p>
<ul>
<li><strong>Accelerometer</strong>: It generates <span class="math inline">\(\mathbf{a_A}\)</span> a measurement of the total acceleration in the body frame referrential the drone is submitted to at a <strong>high</strong> sampling rate. The measurements model is: <span class="math display">\[\mathbf{a_A}(t) = \mathbf{R}_{f2b}\{\mathbf{q}(t)\}\mathbf{a}(t) + \mathbf{a_A}^\epsilon\]</span> where the covariance matrix of the noise of the accelerometer is <span class="math inline">\({\mathbf{R}_{\mathbf{a_A}}}_{3 \times 3}\)</span> and <span class="math display">\[\mathbf{a_A}^\epsilon \sim \mathcal{N}(\mathbf{0}, \mathbf{R}_{\mathbf{a_A}})\]</span>.</li>
<li><strong>Gyroscope</strong>:It generates <span class="math inline">\(\mathbf{\boldsymbol{\omega}_G}\)</span> a measurement of the angular velocity in the body frame of the drone at the last timestep at a <strong>high</strong> sampling rate. The measurement model is: <span class="math display">\[\mathbf{\boldsymbol{\omega}_G}(t) = \boldsymbol{\omega} + \mathbf{\boldsymbol{\omega}_G}^\epsilon\]</span> where the covariance matrix of the noise of the accelerometer is <span class="math inline">\({\mathbf{R}_{\mathbf{\boldsymbol{\omega}_G}}}_{3 \times 3}\)</span> and <span class="math display">\[\mathbf{\boldsymbol{\omega}_G}^\epsilon_t \sim \mathcal{N}(\mathbf{0}, \mathbf{R}_{\mathbf{\boldsymbol{\omega}_G}})\]</span>.</li>
<li><strong>Position</strong>: It generates <span class="math inline">\(\mathbf{p_V}\)</span> a measurement of the current positionat a <strong>low</strong> sampling rate. This is usually provided by a <strong>Vicon</strong> (for indoor), <strong>GPS</strong>, a <strong>Tango</strong> or any other position sensor. The measurement model is: <span class="math display">\[\mathbf{p_V}(t) = \mathbf{p}(t) + \mathbf{p_V}^\epsilon\]</span> where the covariance matrix of the noise of the position is <span class="math inline">\({\mathbf{R}_{\mathbf{p_V}}}_{3 \times 3}\)</span> and <span class="math display">\[\mathbf{p_V}^\epsilon \sim \mathcal{N}(\mathbf{0}, \mathbf{R}_{\mathbf{p_V}})\]</span>.</li>
<li><strong>Attitude</strong>: It generates <span class="math inline">\(\mathbf{q_V}\)</span> a measurement of the current attitute. This is usually provided in addition to the position by a <strong>Vicon</strong> or a <strong>Tango</strong> at a <strong>low</strong> sampling rate or the <strong>Magnemoter</strong> at a <strong>high</strong> sampling rate if the environment permit it (no high magnetic interference nearby like iron contamination). The measurement model is: <span class="math display">\[\mathbf{q_V}(t) = \mathbf{q}(t)*R2Q(\mathbf{q_V}^\epsilon)\]</span> where the covariance matrix of the noise of the attitude is <span class="math inline">\({\mathbf{R}_{\mathbf{q_V}}}_{3 \times 3}\)</span> and <span class="math display">\[\mathbf{q_V}^\epsilon \sim \mathcal{N}(\mathbf{0}, \mathbf{R}_{\mathbf{q_V}})\]</span>.</li>
</ul>
<p>This work adapts itself easily to other sensors. Most sensors available in the market are closely related to one of the 4 presented here. For instance an altimeter is a position estimator but for the z-axis only.</p>
<h3 id="control-inputs">Control inputs</h3>
<p>The control inputs at disposition are:</p>
<ul>
<li><strong>Thrust</strong>: The current thrust in the direction of the orientation of the drone the motors should create. It is given as <span class="math inline">\(t_C\)</span> thrust (as a scalar) in the direction of the attitude.</li>
<li><strong>Angular velocity</strong>: The angular velocity the motors should create. It is given as <span class="math inline">\(\mathbf{\boldsymbol{\omega}_C}\)</span> angular velocity in the body frame.</li>
</ul>
<p>Observations from the control input are not strictly speaking measurements but input of the state-transition model.</p>
<h3 id="assumptions">Assumptions</h3>
<p>We assume rigid body physics which is a good enough approximation for drones.</p>
<p>We assume in this work that the wind is a constant field vector that evolve over time in a “smooth” fashion such that it makes sense to keep track of it.</p>
<p>We assume that since the biases of the sensor are known, the sensor have been calibrated and output measurements with no bias. Some EKF keep track of the sensor biases but this is a state augmentation that was not deemed worthwhile.</p>
<p>Finally, we also assume that the profile (surface on which the wind applies pressure on) of the drone against the wind is approximately the same for all attitude of the drone. This is definitely not true and this could be easily fixed by using an approximate and differentiable function to measure the profile of the drone in every direction.</p>
<h3 id="model-dynamic">Model dynamic</h3>
<ul>
<li><span class="math inline">\(\mathbf{w}(t+1) = \mathbf{w} + \mathbf{a}^\epsilon_t\)</span> where <span class="math inline">\(\mathbf{w}^\epsilon_t \sim \mathcal{N}(\mathbf{0}, \mathbf{Q}_{\mathbf{w}_t })\)</span></li>
<li><span class="math inline">\(\mathbf{a}(t+1) = \mathbf{R}_{b2f}\{\mathbf{q}(t+1)\}\mathbf{T}_{2a} {t_C}(t+1) + \mathbf{w}(t) + \mathbf{a}^\epsilon_t\)</span> where <span class="math inline">\(\mathbf{a}^\epsilon_t \sim \mathcal{N}(\mathbf{0}, \mathbf{Q}_{\mathbf{a}_t })\)</span></li>
<li><span class="math inline">\(\mathbf{v}(t+1) = \mathbf{v}(t) + \Delta t \mathbf{a}(t) + \mathbf{v}^\epsilon_t\)</span> where <span class="math inline">\(\mathbf{v}^\epsilon_t \sim \mathcal{N}(\mathbf{0}, \mathbf{Q}_{\mathbf{v}_t })\)</span></li>
<li><span class="math inline">\(\mathbf{p}(t+1) = \mathbf{p}(t) + \Delta t \mathbf{v}(t) + \mathbf{p}^\epsilon_t\)</span> where <span class="math inline">\(\mathbf{p}^\epsilon_t \sim \mathcal{N}(\mathbf{0}, \mathbf{Q}_{\mathbf{p}_t })\)</span></li>
<li><span class="math inline">\(\boldsymbol{\omega}(t+1) = \mathbf{\boldsymbol{\omega}_C}(t+1) + \mathbf{p}^\epsilon_t\)</span> where <span class="math inline">\(\mathbf{p}^\epsilon_t \sim \mathcal{N}(\mathbf{0}, \mathbf{Q}_{\mathbf{p}_t })\)</span></li>
<li><span class="math inline">\(\mathbf{q}(t+1) = \mathbf{q}(t)*R2Q(\Delta t \boldsymbol{ \omega(t) })\)</span></li>
</ul>
<p>Note that in our model, <span class="math inline">\(\mathbf{q}(t+1)\)</span> must be known. Fortunately, as we will see later, our RBPF is conditionned under the attitude so it is known.</p>
<h2 id="state">State</h2>
<p>The time series of the variables of our dynamic model constitute a hidden markov chain. Indeed, the model is “memoryless” and depend only on the current state and of a sampled transition.</p>
<p>States contain variables that enable us to keep track of some of those hidden variables which is our ultimate goal (for POSE <span class="math inline">\(\mathbf{p}\)</span> and <span class="math inline">\(\mathbf{q}\)</span>). States at time <span class="math inline">\(t\)</span> are denoted by <span class="math inline">\(\mathbf{x}_t\)</span>. Different filters require different state variables depending on their structure and assumptions.</p>
<h2 id="observation">Observation</h2>
<p>Observations are revealed variables conditionned under the variables of our dynamic model. Our ultimate goal is to deduce the states from the observations.</p>
<p>Observations contain the control input <span class="math inline">\(\mathbf{u}\)</span> and the measurements <span class="math inline">\(\mathbf{z}\)</span>.</p>
<p><span class="math display">\[\mathbf{y}_t = (\mathbf{z}_t, \mathbf{u}_t)^T = ((\mathbf{a_A}_t, \mathbf{\boldsymbol{\omega}_G}_t, \mathbf{p_V}_t, \mathbf{q_V}_t), ({t_C}_t, \mathbf{\boldsymbol{\omega}_C}_t))^T\]</span></p>
<h2 id="filtering-and-smoothing">Filtering and smoothing</h2>
<p><strong>Smoothing</strong> is the statistical task of finding the expectation of the state variable from the past history of observations and multiple observation variables ahead</p>
<p><span class="math display">\[\mathbb{E}[g(\mathbf{x}_{0:t}) | \mathbf{y}_{1:t+k}]\]</span></p>
<p>Which expand to,</p>
<p><span class="math display">\[\mathbb{E}[(\mathbf{p}_{0:t}, \mathbf{q}_{0:t}) | (\mathbf{z}_{1:t+k}, \mathbf{u}_{1:t+k})]\]</span></p>
<p><span class="math inline">\(k\)</span> is a contant and the first observation is <span class="math inline">\(y_1\)</span></p>
<p><strong>Filtering</strong> is a kind of smoothing where you only have at disposal the current observation variable (<span class="math inline">\(k=0\)</span>)</p>
<h1 id="complementary-filter">Complementary Filter</h1>
<p>The complementary filter is the simplest of all and very common to retrieve the attitude on low-computing systems because of its low computational complexity. The gyroscope and accelerometer both provide a measurement that can help us to estimate the attitude. The gyroscope indeed gives us a noisy measurement of the angular velocity from which we can retrieve the new attitude from the past one by time integration: <span class="math inline">\(\mathbf{q}_t = \mathbf{q}_{t-1}*R2Q(\Delta t \mathbf{\omega})\)</span>. This is commonly called “Dead reckoning”<a href="#fn1" class="footnoteRef" id="fnref1"><sup>1</sup></a> and is prone to accumulation error, referred as drift. Indeed, like brownian motions, even if the process is unbiased, the variance grows with time. Reducing the noise cannot solve the issue entirely: even with extremely precise instruments, you are subject to floating point errors.</p>
<p>Fortunately, the accelerometer gives us a highly unprecise measurement of the orientation but is not subject to drift. Indeed, if not subject to other accelerations, the accelerometer measures the gravity field orientation. Since this field is oriented toward earth, it is possible to retrieve the current rotation from that field and by extension the attitude. However, in the case of a drone, it is subject to continuous and signifiant acceleration and vibration. Hence, the assumption that we retrieve the gravity field directly is wrong. We can solve this by substracting the acceleration deduced from the thrust control input.</p>
<p><strong>TODO</strong> pictures</p>
<p>The idea of the filter is to combine the precise short-term measurements of the gyroscope subject to drift with the “long-term” measurements of the accelerometer.</p>
<p><strong>TODO</strong>: continue</p>
<h1 id="kalman-filter">Kalman Filter</h1>
<h2 id="linearity">Linearity</h2>
<p>Kalman filters are optimal linear filters. Hence, Kalman filters are non optimal for our problem because our state has some non-linear components (attitude).Indeed, rotations and attitudes belong to <span class="math inline">\(SO(3)\)</span> which is not a vector space. Therefore, we cannot use vanilla kalman filters. However, understanding kalman filters give us a good understanding of Bayesian inference and the filters that we will present after use Kalman filters or are extensions of them to deal with non-linearity. One example of such extension is the extended kalman filter that we will present in the following section.</p>
<p><strong>TODO</strong>: Kalman explanataion</p>
<h2 id="extended-kalman-filters">Extended Kalman Filters</h2>
<p>EKF are linearized Kalman filters of the first order.</p>
<p>An EKF is semi-developped in the annex.</p>
<p><strong>TODO</strong>: EKF explanations</p>
<h2 id="unscented-kalman-filters">Unscented Kalman Filters</h2>
<p><strong>TODO</strong>: UKF explanations</p>
<h1 id="particle-filter">Particle Filter</h1>
<p>Particle filters are computionally expensive and that is why their usage is not very popular currently for low-powered embedded systems.</p>
<p>Particle filters are monte carlo methods which in their general form … <strong>TODO</strong></p>
<p><strong>TODO</strong></p>
<h2 id="resampling">Resampling</h2>
<p>When the number of effective particles is too low (<span class="math inline">\(N/10\)</span>), we apply systematic resampling</p>
<h1 id="rao-blackwellized-particle-filter">Rao-Blackwellized Particle Filter</h1>
<h2 id="introduction-1">Introduction</h2>
<p>Compared to a plain PF, RPBF leverage the linearity of some components of the state by assuming our model gaussian conditionned on a latent variable: Given the attitude <span class="math inline">\(q_t\)</span>, our model is linear. This is where RPBF shines: We use particle filtering to estimate our latent variable, the attitude, and we use the optimal kalman filter to estimate the state variable.</p>
<p>This main inspiration from this approach is <span class="citation">(Vernaza and Lee <a href="#ref-vernaza_rao-blackwellized_2006">2006</a>)</span>. However, it differs by:</p>
<ul>
<li>adding the wind as a state augmentation</li>
<li>adapt the filter to drones by taking into account that the system is too dynamic for assuming that the accelerometer simply output the gravity vector. This is solved by augmenting the state with the acceleration as shown later.</li>
<li>not use measurements of the IMU as control inputs (this is usually used for wheeled vehicles because of the drift from the wheels) but have both control inputs and measurements.</li>
<li>add an attitude sensor.</li>
</ul>
<p>We introduce the latent variable <span class="math inline">\(\boldsymbol{\theta}\)</span></p>
<p>The latent variable <span class="math inline">\(\boldsymbol{\theta}\)</span> has for sole component the attitude: <span class="math display">\[\boldsymbol{\theta} = (\mathbf{q})\]</span></p>
<p><span class="math inline">\(q_t\)</span> is estimated from the product of the attitude of all particles <span class="math inline">\(\mathbf{\theta^{(i)}} = \mathbf{q}^{(i)}_t\)</span> as the “average” quaternion <span class="math inline">\(\mathbf{q}_t = avgQuat(\mathbf{q}^n_t)\)</span>. <span class="math inline">\(x^n\)</span> designates the product of all n arbitrary particle.</p>
<p>We use importance sampling … <strong>TODO</strong></p>
<p>The weight definition is:</p>
<p><span class="math display">\[w^{(i)}_t = \frac{p(\boldsymbol{\theta}^{(i)}_{0:t} | \mathbf{y}_{1:t})}{\pi(\boldsymbol{\theta}^{(i)}_{0:t} | \mathbf{y}_{1:t})}\]</span></p>
<p>From the definition, it is proovable that:</p>
<p><span class="math display">\[w^{(i)}_t \propto \frac{p(\mathbf{y}_t | \boldsymbol{\theta}^{(i)}_{0:t-1}, \mathbf{y}_{1:t-1})p(\boldsymbol{\theta}^{(i)}_t | \boldsymbol{\theta}^{(i)}_{t-1})}{\pi(\boldsymbol{\theta}^{(i)}_t | \boldsymbol{\theta}^{(i)}_{1:t-1}, \mathbf{y}_{1:t})} w^{(i)}_{t-1}\]</span></p>
<p>We choose the dynamic of the model as the importance distribution:</p>
<p><span class="math display">\[\pi(\boldsymbol{\theta}^{(i)}_t | \boldsymbol{\theta}^{(i)}_{1:t-1}, \mathbf{y}_{1:t}) = p(\boldsymbol{\theta}^{(i)}_t | \boldsymbol{\theta}^{(i)}_{t-1}) \]</span></p>
<p>Hence,</p>
<p><span class="math display">\[w^{(i)}_t \propto p(\mathbf{y}_t | \boldsymbol{\theta}^{(i)}_{0:t-1}, \mathbf{y}_{1:t-1}) w^{(i)}_{t-1}\]</span></p>
<p>We then sum all <span class="math inline">\(w^{(i)}_t\)</span> to find the normalization constant and retrieve the actual <span class="math inline">\(w^{(i)}_t\)</span></p>
<h2 id="state-1">State</h2>
<p><span class="math display">\[\mathbf{x}_t = (\mathbf{w}_t, \mathbf{a}_t, \mathbf{v}_t, \mathbf{p}_t)^T\]</span></p>
<p>Initial state <span class="math inline">\(\mathbf{x}_0 = (\mathbf{0}, \mathbf{0}, \mathbf{0})\)</span></p>
<p>Initial covariance matrix <span class="math inline">\(\mathbf{\Sigma}_{9 \times 9} = \epsilon \mathbf{I}_{9 \times 9}\)</span></p>
<h2 id="latent-variable">Latent variable</h2>
<p><span class="math display">\[\mathbf{q}^{(i)}_{t+1} = \mathbf{q}^{(i)}_t*R2Q({\Delta t} (\mathbf{\boldsymbol{\omega}_C}_t+\mathbf{\boldsymbol{\omega}_C}^\epsilon_t))\]</span></p>
<p><span class="math inline">\(\mathbf{\boldsymbol{\omega}_C}^\epsilon_t\)</span> represents the error from the control input and is sampled from <span class="math inline">\(\mathbf{\boldsymbol{\omega}_C}^\epsilon_t \sim \mathcal{N}(\mathbf{0}, \mathbf{R}_{\mathbf{\boldsymbol{\omega}_C}_t })\)</span></p>
<p>Initial attitude <span class="math inline">\(\mathbf{q_0}\)</span> is sampled such that the drone pitch and roll are none (parralel to the ground) but the yaw is unknown and uniformly distributed.</p>
<p>Note that <span class="math inline">\(\mathbf{q}(t+1)\)</span> is known in the <a href="#model-dynamic">model dynamic</a> because the model is conditionned under <span class="math inline">\(\boldsymbol{\theta}^{(i)}_{t+1}\)</span>.</p>
<h2 id="measurement-model">Measurement model</h2>
<ol style="list-style-type: decimal">
<li>Accelerometer: <span class="math display">\[\mathbf{a_A}(t) = \mathbf{R}_{f2b}\{\mathbf{q}(t)\}\mathbf{a}(t) + \mathbf{a_A}^\epsilon_t\]</span> where <span class="math inline">\(\mathbf{a_A}^\epsilon_t \sim \mathcal{N}(\mathbf{0}, \mathbf{R}_{\mathbf{a_A}_t })\)</span></li>
<li>Gyroscope: <span class="math display">\[\mathbf{\boldsymbol{\omega}_G}(t) = Q2R({\mathbf{q}^{(i)}(t-1)}^{-1}\mathbf{q}^{(i)}(t))/\Delta t + \mathbf{\boldsymbol{\omega}_G}^\epsilon(t)\]</span> where <span class="math inline">\(\mathbf{\boldsymbol{\omega}_G}^\epsilon_t \sim \mathcal{N}(\mathbf{0}, \mathbf{R}_{\mathbf{\boldsymbol{\omega}_G}})\)</span></li>
<li>Position: <span class="math display">\[\mathbf{p_V}(t) = \mathbf{p}(t)^{(i)} + \mathbf{p_V}^\epsilon_t\]</span> where <span class="math inline">\(\mathbf{p_V}^\epsilon_t \sim \mathcal{N}(\mathbf{0}, \mathbf{R}_{\mathbf{p_V}_t })\)</span></li>
<li>Attitude: <span class="math display">\[\mathbf{q_V}(t) = \mathbf{q}(t)^{(i)}*R2Q(\mathbf{q_V}^\epsilon_t)\]</span> where <span class="math inline">\(\mathbf{q_V}^\epsilon_t \sim \mathcal{N}(\mathbf{0}, \mathbf{R}_{\mathbf{q_V}_t })\)</span></li>
</ol>
<h2 id="kalman-prediction">Kalman prediction</h2>
<p>The model dynamics define the following model, state-transition matrix <span class="math inline">\(\mathbf{F}_t\{\boldsymbol{\theta}^{(i)}_t\}\)</span>, the control-input matrix <span class="math inline">\(\mathbf{B}_t\{\boldsymbol{\theta}^{(i)}_t\}\)</span>, the process noise <span class="math inline">\(\mathbf{w}_t\{\boldsymbol{\theta}^{(i)}_t\}\)</span> for the Kalman filter and its covariance <span class="math inline">\(\mathbf{Q}_t\{\boldsymbol{\theta}^{(i)}_t\}\)</span></p>
<p><span class="math display">\[\mathbf{x}_t = \mathbf{F}_t\{\boldsymbol{\theta}^{(i)}_t\} \mathbf{x}_{t-1} + \mathbf{B}_t\{\boldsymbol{\theta}^{(i)}_t\} \mathbf{u}_t + \mathbf{w}_t\{\boldsymbol{\theta}^{(i)}_t\}\]</span></p>
<p><span class="math display">\[\mathbf{F}_t\{\boldsymbol{\theta}^{(i)}_t\}_{12 \times 12} =
\left( \begin{array}{ccc}
\mathbf{I}_{3 \times 3} & & & \\
\mathbf{I}_{3 \times 3} & & & \\
& \Delta t~\mathbf{I}_{3 \times 3} & \mathbf{I}_{3 \times 3} & \\
& & \Delta t~\mathbf{I}_{3 \times 3} & \mathbf{I}_{3 \times 3}
\end{array} \right)\]</span></p>
<p><span class="math display">\[\mathbf{B}_t\{\boldsymbol{\theta}^{(i)}_t\}_{9 \times 5} =
\left( \begin{array}{ccc}
\mathbf{0}_{3 \times 3} & \\
& \mathbf{R}_{b2f}\{\mathbf{q}^{(i)}_{t}\}\mathbf{T}_{2a} \\
& \\
&
\end{array} \right)\]</span></p>
<p><span class="math display">\[\mathbf{Q}_t\{\boldsymbol{\theta}^{(i)}_t\}_{12 \times 12} =
\left( \begin{array}{cccc}
\mathbf{Q}_{\mathbf{w}_t } & & &\\
& \mathbf{Q}_{\mathbf{a}_t } & &\\
& & \mathbf{Q}_{\mathbf{v}_t }& \\
& & &\mathbf{Q}_{\mathbf{p}_t }
\end{array} \right)\]</span></p>
<p><span class="math display">\[\hat{\mathbf{x}}^{-(i)}_t = \mathbf{F}_t\{\boldsymbol{\theta}^{(i)}_t\} \mathbf{x}^{(i)}_{t-1} + \mathbf{B}_t\{\boldsymbol{\theta}^{(i)}_t\} \mathbf{u}_t \]</span> <span class="math display">\[ \mathbf{\Sigma}^{-(i)}_t = \mathbf{F}_t\{\boldsymbol{\theta}^{(i)}_t\} \mathbf{\Sigma}^{-(i)}_{t-1} (\mathbf{F}_t\{\boldsymbol{\theta}^{(i)}_t\})^T + \mathbf{Q}_t\{\boldsymbol{\theta}^{(i)}_t\}\]</span></p>
<h2 id="kalman-measurement-update">Kalman measurement update</h2>
<p>The <a href="#measurements-model-1">measurement model</a> defines how to compute <span class="math inline">\(p(\mathbf{y}_t | \boldsymbol{\theta}^{(i)}_{0:t-1}, \mathbf{y}_{1:t-_K1})\)</span></p>
<p>In the <a href="#measurements-model-1">measurement model</a>, (1, 3) define the observation matrix <span class="math inline">\(\mathbf{H}_t\{\boldsymbol{\theta}^{(i)}_t\}\)</span>, the observation noise <span class="math inline">\(\mathbf{v}_t\{\boldsymbol{\theta}^{(i)}_t\}\)</span> and its covariance matrix <span class="math inline">\(\mathbf{R}_t\{\boldsymbol{\theta}^{(i)}_t\}\)</span> for the Kalman filter.</p>
<p><span class="math display">\[(\mathbf{a_A}_t, \mathbf{p_V}_t)^T = \mathbf{H}_t\{\boldsymbol{\theta}^{(i)}_t\} (\mathbf{w}_t, \mathbf{a}_t, \mathbf{v}_t, \mathbf{p}_t)^T + \mathbf{v}_t\{\boldsymbol{\theta}^{(i)}_t\}\]</span></p>
<p><span class="math display">\[\mathbf{H}_t\{\boldsymbol{\theta}^{(i)}_t\}_{6 \times 12} =
\left( \begin{array}{ccc}
\mathbf{I}_{3 \times 3} & \mathbf{R}_{f2b}\{\mathbf{q}^{(i)}_{t}\} & & \mathbf{0}_{3 \times 3} \\
& & \mathbf{I}_{3 \times 3} & \\
\end{array} \right)\]</span></p>
<p><span class="math display">\[\mathbf{R}_t\{\boldsymbol{\theta}^{(i)}_t\}_{6 \times 1} =
\left( \begin{array}{cc}
\mathbf{R}_{\mathbf{a_A}_t } & \\
& \mathbf{R}_{\mathbf{p_V}_t }
\end{array} \right)\]</span></p>
<h3 id="kalman-update">Kalman update</h3>
<p><span class="math display">\[\mathbf{S} = \mathbf{H}_t\{\boldsymbol{\theta}^{(i)}_t\} \mathbf{\Sigma}^{-(i)}_t (\mathbf{H}_t\{\boldsymbol{\theta}^{(i)}_t\})^T + \mathbf{R}_t\{\boldsymbol{\theta}^{(i)}_t\}\]</span> <span class="math display">\[\hat{\mathbf{z}} = \mathbf{H}_t\{\boldsymbol{\theta}^{(i)}_t\} \hat{\mathbf{x}}^{-(i)}_t\]</span> <span class="math display">\[\mathbf{K} = \mathbf{\Sigma}^{-(i)}_t \mathbf{H}_t\{\boldsymbol{\theta}^{(i)}_t\}^T \mathbf{S}^{-1}\]</span> <span class="math display">\[\mathbf{\Sigma}^{(i)}_t = \mathbf{\Sigma}^{-(i)}_t + \mathbf{K} \mathbf{S} \mathbf{K}^T\]</span> <span class="math display">\[\hat{\mathbf{x}}^{(i)}_t = \hat{\mathbf{x}}^{-(i)}_t + \mathbf{K}((\mathbf{a_A}_t, \mathbf{p_V}_t)^T - \hat{\mathbf{z}})\]</span> <span class="math display">\[p(\mathbf{y}_t | \boldsymbol{\theta}^{(i)}_{0:t-1}, \mathbf{y}_{1:t-1}) = \mathcal{N}((\mathbf{a_A}_t, \mathbf{p_V}_t)^T; \hat{\mathbf{z}}_t, \mathbf{S})\]</span></p>
<h3 id="asynchronous-measurements">Asynchronous measurements</h3>
<p>Our measurements from the Vicon and the accelerometer have different sampling rate so instead of doing full kalman update, we only apply a partial kalman update corresponding to the current type of measurement <span class="math inline">\(\mathbf{z}_t\)</span></p>
<p>For instance, we would apply the kalman update from the previous section but with:</p>
<ul>
<li><p>for <span class="math inline">\(\mathbf{a_A}_t\)</span>: <span class="math display">\[\mathbf{H}_t\{\boldsymbol{\theta}^{(i)}_t\}_{3 \times 6} = (\mathbf{R}_{f2b}\{\mathbf{q}^{(i)}_{t}\} \mathbf{0}_{3 \times 3})\]</span> <span class="math display">\[\mathbf{R}_t\{\boldsymbol{\theta}^{(i)}_t\}_{3 \times 3} = \mathbf{R}_{f2b}\{\mathbf{q}^{(i)}_{t}\}\mathbf{R}_{\mathbf{a_A}_t } \]</span> <span class="math display">\[\mathbf{x}^{(i)}_t = \mathbf{H}_t\{\boldsymbol{\theta}^{(i)}_t\} \mathbf{x}^{(i)}_{t-1} + \mathbf{K}(\mathbf{a_A}_t - \hat{\mathbf{z}})\]</span> <span class="math display">\[p(\mathbf{y}_t | \boldsymbol{\theta}^{(i)}_{0:t-1}, \mathbf{y}_{1:t-1}) = \mathcal{N}(\mathbf{a_A}_t; \hat{\mathbf{z}}_t, \mathbf{S})\]</span></p></li>
<li><p>for <span class="math inline">\(\mathbf{p_V}_t\)</span>: <span class="math display">\[\mathbf{H}_t\{\boldsymbol{\theta}^{(i)}_t\}_{3 \times 6} = (\mathbf{0}_{3 \times 3} \mathbf{I}_{3 \times 3} )\]</span> <span class="math display">\[\mathbf{R}_t\{\boldsymbol{\theta}^{(i)}_t\}_{3 \times 3} = \mathbf{R}_{\mathbf{p_V}_t }\]</span> <span class="math display">\[\mathbf{x}^{(i)}_t = \mathbf{H}_t\{\boldsymbol{\theta}^{(i)}_t\} \mathbf{x}^{(i)}_{t-1} + \mathbf{K}(\mathbf{p_V}_t - \hat{\mathbf{z}})\]</span> <span class="math display">\[p(\mathbf{y}_t | \boldsymbol{\theta}^{(i)}_{0:t-1}, \mathbf{y}_{1:t-1}) = \mathcal{N}(\mathbf{p_V}_t; \hat{\mathbf{z}}_t, \mathbf{S})\]</span></p></li>
</ul>
<h2 id="other-sources-or-reweighting">Other sources or reweighting</h2>
<p>In the <a href="#measurements-model">measurement model</a>, (2 and 4) define two other weight updates.</p>
<p><span class="math display">\[p(\mathbf{y}_t | \boldsymbol{\theta}^{(i)}_{0:t-1}, \mathbf{y}_{1:t-1}) = \mathcal{N}(\mathbf{\boldsymbol{\omega}_G}_t; Q2R({\mathbf{q}^{(i)}_{t-1}}^{-1}\mathbf{q}^{(i)}_t)/\Delta t,~ \mathbf{R}_{\mathbf{\boldsymbol{\omega}_G}})\]</span></p>
<p><span class="math display">\[p(\mathbf{y}_t | \boldsymbol{\theta}^{(i)}_{0:t-1}, \mathbf{y}_{1:t-1}) = \mathcal{N}(Q2R({\mathbf{q}^{(i)}}^{-1}\mathbf{q_V}_t);~ 0 ,~ \mathbf{R}_{\mathbf{q_V}})\]</span></p>
<h2 id="algorithm-summary">Algorithm summary</h2>
<ol style="list-style-type: decimal">
<li>Initiate <span class="math inline">\(N\)</span> particles with <span class="math inline">\(\mathbf{x}_0\)</span>, <span class="math inline">\(\mathbf{q}_0 ~ \sim p(\mathbf{q}_0)\)</span>, <span class="math inline">\(\mathbf{\Sigma}_0\)</span> and <span class="math inline">\(w = 1/N\)</span></li>
<li>While new sensor measurements <span class="math inline">\((\mathbf{z}_t, \mathbf{u}_t)\)</span></li>
</ol>
<ul>
<li>foreach <span class="math inline">\(N\)</span> particles <span class="math inline">\((i)\)</span>:
<ol style="list-style-type: decimal">
<li>sample new latent variable <span class="math inline">\(\boldsymbol{\theta_t}\)</span> from <span class="math inline">\(\mathbf{u}_t\)</span></li>
<li>Depending on the type of measurement:
<ul>
<li><strong>Accelerometer</strong>: Partial kalman update with: <span class="math display">\[\mathbf{H}_t\{\boldsymbol{\theta}^{(i)}_t\}_{3 \times 6} = (\mathbf{R}_{f2b}\{\mathbf{q}^{(i)}_{t}\} ~~~~ \mathbf{0}_{3 \times 3})\]</span> <span class="math display">\[\mathbf{R}_t\{\boldsymbol{\theta}^{(i)}_t\}_{3 \times 3} = \mathbf{R}_{f2b}\{\mathbf{q}^{(i)}_{t}\}\mathbf{R}_{\mathbf{a_A}_t } \]</span> <span class="math display">\[\mathbf{x}^{(i)}_t = \mathbf{H}_t\{\boldsymbol{\theta}^{(i)}_t\} \mathbf{x}^{(i)}_{t-1} + \mathbf{K}(\mathbf{a_A}_t - \hat{\mathbf{z}})\]</span> <span class="math display">\[p(\mathbf{y}_t | \boldsymbol{\theta}^{(i)}_{0:t-1}, \mathbf{y}_{1:t-1}) = \mathcal{N}(\mathbf{a_A}_t; \hat{\mathbf{z}}_t, \mathbf{S})\]</span></li>
<li><strong>Gyroscope</strong>: <span class="math display">\[p(\mathbf{y}_t | \boldsymbol{\theta}^{(i)}_{0:t-1}, \mathbf{y}_{1:t-1}) = \mathcal{N}(\mathbf{\boldsymbol{\omega}_G}_t; (\mathbf{q}^{(i)}_t - \mathbf{q}^{(i)}_{t-1})/\Delta t,~ \mathbf{R}_{\mathbf{\boldsymbol{\omega}_G}_t})\]</span></li>
<li><strong>Vicon</strong>: Partial kalman update with: <span class="math display">\[\mathbf{H}_t\{\boldsymbol{\theta}^{(i)}_t\}_{3 \times 6} = (\mathbf{0}_{3 \times 3} ~~~~ \mathbf{I}_{3 \times 3} )\]</span> <span class="math display">\[\mathbf{R}_t\{\boldsymbol{\theta}^{(i)}_t\}_{3 \times 3} = \mathbf{R}_{\mathbf{p_V}_t }\]</span> <span class="math display">\[\mathbf{x}^{(i)}_t = \mathbf{H}_t\{\boldsymbol{\theta}^{(i)}_t\} \mathbf{x}^{(i)}_{t-1} + \mathbf{K}(\mathbf{p_V}_t - \hat{\mathbf{z}})\]</span> <span class="math display">\[p(\mathbf{y}_t | \boldsymbol{\theta}^{(i)}_{0:t-1}, \mathbf{y}_{1:t-1})^- = \mathcal{N}(\mathbf{p_V}_t; \hat{\mathbf{z}}_t, \mathbf{S})\]</span> <span class="math display">\[p(\mathbf{y}_t | \boldsymbol{\theta}^{(i)}_{0:t-1}, \mathbf{y}_{1:t-1}) = \mathcal{N}(\mathbf{q_V}_t; \mathbf{q}^{(i)}_t,~ \mathbf{R}_{\mathbf{q_V}_t }) p(\mathbf{y}_t | \boldsymbol{\theta}^{(i)}_{0:t-1}, \mathbf{y}_{1:t-1})^-\]</span></li>
</ul></li>
<li>Update <span class="math inline">\(w^{(i)}_t\)</span>: <span class="math inline">\(w^{(i)}_t = p(\mathbf{y}_t | \boldsymbol{\theta}^{(i)}_{0:t-1}, \mathbf{y}_{1:t-1}) w^{(i)}_{t-1}\)</span><br />
</li>
</ol></li>
<li>Normalize all <span class="math inline">\(w^{(i)}\)</span> by scalaing by <span class="math inline">\(1/(\sum w^{(i)})\)</span> such that <span class="math inline">\(\sum w^{(i)}= 1\)</span></li>
<li>Compute <span class="math inline">\(\mathbf{p}_t\)</span> and <span class="math inline">\(\mathbf{q}_t\)</span> as the expectation from the distribution approximated by the N particles.</li>
<li>Resample if the number of effective particle is too low</li>
</ul>
<h1 id="appendix">Appendix</h1>
<h2 id="semi-developped-kalman-filter">Semi developped Kalman filter</h2>
<h2 id="state-2">State</h2>
<p>For the EKF, we are gonna use the following state:</p>
<p><span class="math display">\[\mathbf{x}_t = (\mathbf{w}_t, \mathbf{a}_t, \mathbf{v}_t, \mathbf{p}_t, \boldsymbol{\omega}_t, \mathbf{q}_t)^T\]</span></p>
<p>Initial state <span class="math inline">\(\mathbf{x}_0\)</span> at <span class="math inline">\((\mathbf{0}, \mathbf{0}, \mathbf{0}, \mathbf{0}, \mathbf{0}, (1, 0, 0, 0))\)</span></p>
<h2 id="measurements-model">Measurements model</h2>
<ol style="list-style-type: decimal">
<li>Accelerometer: <span class="math display">\[\mathbf{a_A}(t) = \mathbf{R}_{f2b}\{\mathbf{q}(t)\}\mathbf{a}(t) + \mathbf{w}(t) + \mathbf{a_A}^\epsilon_t\]</span> where <span class="math inline">\(\mathbf{a_A}^\epsilon_t \sim \mathcal{N}(\mathbf{0}, \mathbf{R}_{\mathbf{a_A}_t })\)</span></li>
<li>Gyroscope: <span class="math display">\[\mathbf{\boldsymbol{\omega}_G}(t) = \mathbf{\boldsymbol{\omega}_G} + \mathbf{\boldsymbol{\omega}_G}^\epsilon(t)\]</span> where <span class="math inline">\(\mathbf{\boldsymbol{\omega}_G}^\epsilon_t \sim \mathcal{N}(\mathbf{0}, \mathbf{R}_{\mathbf{\boldsymbol{\omega}_G}})\)</span></li>
<li>Position: <span class="math display">\[\mathbf{p_V}(t) = \mathbf{p}(t)^{(i)} + \mathbf{p_V}^\epsilon_t\]</span> where <span class="math inline">\(\mathbf{p_V}^\epsilon_t \sim \mathcal{N}(\mathbf{0}, \mathbf{R}_{\mathbf{p_V}_t })\)</span></li>
<li>Attitude: <span class="math display">\[\mathbf{q_V}(t) = \mathbf{q}(t)^{(i)}*R2Q(\mathbf{q_V}^\epsilon_t)\]</span> where <span class="math inline">\(\mathbf{q_V}^\epsilon_t \sim \mathcal{N}(\mathbf{0}, \mathbf{R}_{\mathbf{q_V}_t })\)</span></li>
</ol>
<h2 id="kalman-prediction-1">Kalman prediction</h2>
<p>The model dynamic defines the following model, state-transition function <span class="math inline">\(f(\mathbf{x}, \mathbf{u})\)</span> and process noise <span class="math inline">\(\mathbf{w}\)</span> with covariance matrix <span class="math inline">\(\mathbf{Q}\)</span></p>
<p><span class="math display">\[\mathbf{x}_t = f(\mathbf{x}_{t-1}, \mathbf{u}_t) + \mathbf{w}_t\]</span></p>
<p><span class="math display">\[f((\mathbf{w}, \mathbf{a}, \mathbf{v}, \mathbf{p}, \boldsymbol{\omega}, \mathbf{q}), (t_C, \mathbf{\boldsymbol{\omega}_C})) = \left( \begin{array}{c}
\mathbf{w}\\
\mathbf{R}_{b2f}\{\mathbf{q}\}\mathbf{T}_{2a} {t_C} + \mathbf{w} \\
\mathbf{v} + \Delta t \mathbf{a} \\
\mathbf{p} + \Delta t \mathbf{v} \\
\mathbf{\boldsymbol{\omega}_C} \\
\mathbf{q}*R2Q({\Delta t} \boldsymbol{\omega})
\end{array} \right)\]</span></p>
<p>Now, we need to derive the jacobian of <span class="math inline">\(f\)</span>. It is quite trivial except for the partial derivatives of <span class="math inline">\(q\)</span>. We will use sagemath to retrieve the 28 interesting different partial derivatives of <span class="math inline">\(q\)</span> as described in the appendix A.</p>
<p><span class="math display">\[{\mathbf{F}_t}_{19 \times 19} = \left . \frac{\partial f}{\partial \mathbf{x} } \right \vert _{\hat{\mathbf{x}}_{t-1},\mathbf{u}_{t-1}} = \left( \begin{array}{ccccc}
\mathbf{I}_{3 \times 3} & & & & & \\
\mathbf{I}_{3 \times 3} & & & & & \\
& \Delta t~\mathbf{I}_{3 \times 3} & \mathbf{I}_{3 \times 3} & & & \\
& & \Delta t~\mathbf{I}_{3 \times 3} & \mathbf{I}_{3 \times 3} & & \\
& & & & \mathbf{I}_{3 \times 3} & \\
& & & & & \mathbf{F_q}_t
\end{array} \right)\]</span></p>
<p>where <span class="math inline">\(\mathbf{F_q}_t\)</span> is defined by the script sagemath in the appendix.</p>
<p><span class="math display">\[\hat{\mathbf{x}}^{-(i)}_t = f(\mathbf{x}^{(i)}_{t-1}, \mathbf{u}_t)\]</span> <span class="math display">\[\mathbf{\Sigma}^{-(i)}_t = \mathbf{F}_{t-1} \mathbf{\Sigma}^{-(i)}_{t-1} \mathbf{F}_{t-1}^T + \mathbf{Q}_t\]</span></p>
<h3 id="kalman-measurements-update">Kalman measurements update</h3>
<p><span class="math display">\[\mathbf{z}_t = h(\mathbf{x}_t) + \mathbf{v}_t\]</span></p>
<p>The <a href="#measurements-model">measurement model</a> defines <span class="math inline">\(h(\mathbf{x})\)</span></p>
<p><span class="math display">\[\left( \begin{array}{c}
\mathbf{a_A}\\
\mathbf{\boldsymbol{\omega}_G}\\
\mathbf{p_V}\\
\mathbf{q_V}\\
\end{array} \right) = h((\mathbf{w}, \mathbf{a}, \mathbf{v}, \mathbf{p}, \boldsymbol{\omega}, \mathbf{q})) = \left( \begin{array}{c}
\mathbf{R}_{f2b}\{\mathbf{q}\}\mathbf{a} + \mathbf{w}\\
\boldsymbol{\omega}\\
\mathbf{p}\\
\mathbf{q}\\
\end{array} \right)\]</span></p>
<p>The only complex partial derivatives to calculate are the one of the acceleration, because they have to be rotated first. Once again, we use sagemath: <span class="math inline">\(\mathbf{H_a}\)</span> is defined by the script in the appendix B.</p>
<p><span class="math display">\[{\mathbf{H}_t}_{16 \times 16} = \left . \frac{\partial h}{\partial \mathbf{x} } \right \vert _{\hat{\mathbf{x}}_{t}} = \left( \begin{array}{ccccc}
\mathbf{H_a}_{3 \times 3} & & & & \\
& \mathbf{I}_{3 \times 3} & & & \\
& & \mathbf{I}_{3 \times 3} & & \\
& & & \mathbf{I}_{3 \times 3} & \\
& & & & \mathbf{I}_{4 \times 4} \\
\end{array} \right)\]</span></p>
<p><span class="math display">\[{\mathbf{R}_t}_{13 \times 13} =
\left( \begin{array}{cccc}
\mathbf{R}_{\mathbf{a_A}_t} & & & \\
& \mathbf{R}_{\mathbf{\boldsymbol{\omega}_G}} & & \\
& & \mathbf{R}_{\mathbf{p_V}} & \\
& & & ???\\
\end{array} \right)\]</span></p>
<p><strong>TODO</strong>: Use (Shibani, 2011) to replace ???</p>
<h3 id="kalman-update-1">Kalman update</h3>
<p><span class="math display">\[\mathbf{S} = \mathbf{H}_t \mathbf{\Sigma}^{-}_t \mathbf{H}_t^T + \mathbf{R}_t\]</span> <span class="math display">\[\hat{\mathbf{z}} = h(\hat{\mathbf{x}}^{-}_t)\]</span> <span class="math display">\[\mathbf{K} = \mathbf{\Sigma}^{-}_t \mathbf{H}_t^T \mathbf{S}^{-1}\]</span> <span class="math display">\[\mathbf{\Sigma}_t = \mathbf{\Sigma}^-_t + \mathbf{K} \mathbf{S} \mathbf{K}^T\]</span> <span class="math display">\[\hat{\mathbf{x}}_t = \hat{\mathbf{x}}^{-}_t + \mathbf{K}(\mathbf{z}_t - \hat{\mathbf{z}})\]</span></p>
<h2 id="f-partial-derivatives">F partial derivatives</h2>
<div class="sourceCode"><pre class="sourceCode python"><code class="sourceCode python">Q.<span class="op"><</span>i,j,k<span class="op">></span> <span class="op">=</span> QuaternionAlgebra(SR, <span class="op">-</span><span class="dv">1</span>, <span class="op">-</span><span class="dv">1</span>)
var(<span class="st">'q0, q1, q2, q3'</span>)
var(<span class="st">'dt'</span>)
var(<span class="st">'wx, wy, wz'</span>)
q <span class="op">=</span> q0 <span class="op">+</span> q1<span class="op">*</span>i <span class="op">+</span> q2<span class="op">*</span>j <span class="op">+</span> q3<span class="op">*</span>k
w <span class="op">=</span> vector([wx, wy, wz])<span class="op">*</span>dt
w_norm <span class="op">=</span> sqrt(w[<span class="dv">0</span>]<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> w[<span class="dv">1</span>]<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> w[<span class="dv">2</span>]<span class="op">^</span><span class="dv">2</span>)
ang <span class="op">=</span> w_norm<span class="op">/</span><span class="dv">2</span>
w_normalized <span class="op">=</span> w<span class="op">/</span>w_norm
sin2 <span class="op">=</span> sin(ang)
qd <span class="op">=</span> cos(ang) <span class="op">+</span> w_normalized[<span class="dv">0</span>]<span class="op">*</span>sin2<span class="op">*</span>i <span class="op">+</span> w_normalized[<span class="dv">1</span>]<span class="op">*</span>sin2<span class="op">*</span>j <span class="op">+</span> w_normalized[<span class="dv">2</span>]<span class="op">*</span>sin2<span class="op">*</span>k
nq <span class="op">=</span> q<span class="op">*</span>qd
v <span class="op">=</span> vector(nq.coefficient_tuple())
<span class="cf">for</span> sym <span class="kw">in</span> [wx, wy, wz, q0, q1, q2, q3]:
d <span class="op">=</span> diff(v, sym)
exps <span class="op">=</span> <span class="bu">map</span>(<span class="kw">lambda</span> x: x.canonicalize_radical().full_simplify(), d)
<span class="cf">for</span> i, e <span class="kw">in</span> <span class="bu">enumerate</span>(exps):
<span class="bu">print</span>(sym, i, e)
<span class="co"># Here are the results</span>
(wx, <span class="dv">0</span>, <span class="op">-</span><span class="dv">1</span><span class="op">/</span><span class="dv">2</span><span class="op">*</span>((dt<span class="op">*</span>q1<span class="op">*</span>wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> dt<span class="op">*</span>q2<span class="op">*</span>wx<span class="op">*</span>wy <span class="op">+</span> dt<span class="op">*</span>q3<span class="op">*</span>wx<span class="op">*</span>wz)<span class="op">*</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>cos(<span class="dv">1</span><span class="op">/</span><span class="dv">2</span><span class="op">*</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>dt) <span class="op">+</span> (dt<span class="op">*</span>q0<span class="op">*</span>wx<span class="op">^</span><span class="dv">3</span> <span class="op">-</span> <span class="dv">2</span><span class="op">*</span>q2<span class="op">*</span>wx<span class="op">*</span>wy <span class="op">+</span> (dt<span class="op">*</span>q0<span class="op">*</span>wx <span class="op">+</span> <span class="dv">2</span><span class="op">*</span>q1)<span class="op">*</span>wy<span class="op">^</span><span class="dv">2</span> <span class="op">-</span> <span class="dv">2</span><span class="op">*</span>q3<span class="op">*</span>wx<span class="op">*</span>wz <span class="op">+</span> (dt<span class="op">*</span>q0<span class="op">*</span>wx <span class="op">+</span> <span class="dv">2</span><span class="op">*</span>q1)<span class="op">*</span>wz<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>sin(<span class="dv">1</span><span class="op">/</span><span class="dv">2</span><span class="op">*</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>dt))<span class="op">/</span>(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>)<span class="op">^</span>(<span class="dv">3</span><span class="op">/</span><span class="dv">2</span>))
(wx, <span class="dv">1</span>, <span class="op">-</span><span class="dv">1</span><span class="op">/</span><span class="dv">2</span><span class="op">*</span>((dt<span class="op">*</span>q1<span class="op">*</span>wx<span class="op">^</span><span class="dv">3</span> <span class="op">-</span> <span class="dv">2</span><span class="op">*</span>q3<span class="op">*</span>wx<span class="op">*</span>wy <span class="op">+</span> (dt<span class="op">*</span>q1<span class="op">*</span>wx <span class="op">-</span> <span class="dv">2</span><span class="op">*</span>q0)<span class="op">*</span>wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> <span class="dv">2</span><span class="op">*</span>q2<span class="op">*</span>wx<span class="op">*</span>wz <span class="op">+</span> (dt<span class="op">*</span>q1<span class="op">*</span>wx <span class="op">-</span> <span class="dv">2</span><span class="op">*</span>q0)<span class="op">*</span>wz<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>sin(<span class="dv">1</span><span class="op">/</span><span class="dv">2</span><span class="op">*</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>dt) <span class="op">-</span> (dt<span class="op">*</span>q0<span class="op">*</span>wx<span class="op">^</span><span class="dv">4</span> <span class="op">-</span> dt<span class="op">*</span>q3<span class="op">*</span>wx<span class="op">^</span><span class="dv">3</span><span class="op">*</span>wy <span class="op">+</span> dt<span class="op">*</span>q0<span class="op">*</span>wx<span class="op">^</span><span class="dv">2</span><span class="op">*</span>wy<span class="op">^</span><span class="dv">2</span> <span class="op">-</span> dt<span class="op">*</span>q3<span class="op">*</span>wx<span class="op">*</span>wy<span class="op">^</span><span class="dv">3</span> <span class="op">+</span> dt<span class="op">*</span>q2<span class="op">*</span>wx<span class="op">*</span>wz<span class="op">^</span><span class="dv">3</span> <span class="op">+</span> (dt<span class="op">*</span>q0<span class="op">*</span>wx<span class="op">^</span><span class="dv">2</span> <span class="op">-</span> dt<span class="op">*</span>q3<span class="op">*</span>wx<span class="op">*</span>wy)<span class="op">*</span>wz<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> (dt<span class="op">*</span>q2<span class="op">*</span>wx<span class="op">^</span><span class="dv">3</span> <span class="op">+</span> dt<span class="op">*</span>q2<span class="op">*</span>wx<span class="op">*</span>wy<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>wz)<span class="op">*</span>cos(<span class="dv">1</span><span class="op">/</span><span class="dv">2</span><span class="op">*</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>dt))<span class="op">/</span>(wx<span class="op">^</span><span class="dv">4</span> <span class="op">+</span> <span class="dv">2</span><span class="op">*</span>wx<span class="op">^</span><span class="dv">2</span><span class="op">*</span>wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">4</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">4</span> <span class="op">+</span> <span class="dv">2</span><span class="op">*</span>(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>wz<span class="op">^</span><span class="dv">2</span>))
(wx, <span class="dv">2</span>, <span class="dv">1</span><span class="op">/</span><span class="dv">2</span><span class="op">*</span>((dt<span class="op">*</span>q3<span class="op">*</span>wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> dt<span class="op">*</span>q0<span class="op">*</span>wx<span class="op">*</span>wy <span class="op">-</span> dt<span class="op">*</span>q1<span class="op">*</span>wx<span class="op">*</span>wz)<span class="op">*</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>cos(<span class="dv">1</span><span class="op">/</span><span class="dv">2</span><span class="op">*</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>dt) <span class="op">-</span> (dt<span class="op">*</span>q2<span class="op">*</span>wx<span class="op">^</span><span class="dv">3</span> <span class="op">+</span> <span class="dv">2</span><span class="op">*</span>q0<span class="op">*</span>wx<span class="op">*</span>wy <span class="op">+</span> (dt<span class="op">*</span>q2<span class="op">*</span>wx <span class="op">-</span> <span class="dv">2</span><span class="op">*</span>q3)<span class="op">*</span>wy<span class="op">^</span><span class="dv">2</span> <span class="op">-</span> <span class="dv">2</span><span class="op">*</span>q1<span class="op">*</span>wx<span class="op">*</span>wz <span class="op">+</span> (dt<span class="op">*</span>q2<span class="op">*</span>wx <span class="op">-</span> <span class="dv">2</span><span class="op">*</span>q3)<span class="op">*</span>wz<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>sin(<span class="dv">1</span><span class="op">/</span><span class="dv">2</span><span class="op">*</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>dt))<span class="op">/</span>(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>)<span class="op">^</span>(<span class="dv">3</span><span class="op">/</span><span class="dv">2</span>))
(wx, <span class="dv">3</span>, <span class="op">-</span><span class="dv">1</span><span class="op">/</span><span class="dv">2</span><span class="op">*</span>((dt<span class="op">*</span>q3<span class="op">*</span>wx<span class="op">^</span><span class="dv">3</span> <span class="op">+</span> <span class="dv">2</span><span class="op">*</span>q1<span class="op">*</span>wx<span class="op">*</span>wy <span class="op">+</span> (dt<span class="op">*</span>q3<span class="op">*</span>wx <span class="op">+</span> <span class="dv">2</span><span class="op">*</span>q2)<span class="op">*</span>wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> <span class="dv">2</span><span class="op">*</span>q0<span class="op">*</span>wx<span class="op">*</span>wz <span class="op">+</span> (dt<span class="op">*</span>q3<span class="op">*</span>wx <span class="op">+</span> <span class="dv">2</span><span class="op">*</span>q2)<span class="op">*</span>wz<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>sin(<span class="dv">1</span><span class="op">/</span><span class="dv">2</span><span class="op">*</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>dt) <span class="op">+</span> (dt<span class="op">*</span>q2<span class="op">*</span>wx<span class="op">^</span><span class="dv">4</span> <span class="op">-</span> dt<span class="op">*</span>q1<span class="op">*</span>wx<span class="op">^</span><span class="dv">3</span><span class="op">*</span>wy <span class="op">+</span> dt<span class="op">*</span>q2<span class="op">*</span>wx<span class="op">^</span><span class="dv">2</span><span class="op">*</span>wy<span class="op">^</span><span class="dv">2</span> <span class="op">-</span> dt<span class="op">*</span>q1<span class="op">*</span>wx<span class="op">*</span>wy<span class="op">^</span><span class="dv">3</span> <span class="op">-</span> dt<span class="op">*</span>q0<span class="op">*</span>wx<span class="op">*</span>wz<span class="op">^</span><span class="dv">3</span> <span class="op">+</span> (dt<span class="op">*</span>q2<span class="op">*</span>wx<span class="op">^</span><span class="dv">2</span> <span class="op">-</span> dt<span class="op">*</span>q1<span class="op">*</span>wx<span class="op">*</span>wy)<span class="op">*</span>wz<span class="op">^</span><span class="dv">2</span> <span class="op">-</span> (dt<span class="op">*</span>q0<span class="op">*</span>wx<span class="op">^</span><span class="dv">3</span> <span class="op">+</span> dt<span class="op">*</span>q0<span class="op">*</span>wx<span class="op">*</span>wy<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>wz)<span class="op">*</span>cos(<span class="dv">1</span><span class="op">/</span><span class="dv">2</span><span class="op">*</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>dt))<span class="op">/</span>(wx<span class="op">^</span><span class="dv">4</span> <span class="op">+</span> <span class="dv">2</span><span class="op">*</span>wx<span class="op">^</span><span class="dv">2</span><span class="op">*</span>wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">4</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">4</span> <span class="op">+</span> <span class="dv">2</span><span class="op">*</span>(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>wz<span class="op">^</span><span class="dv">2</span>))
(wy, <span class="dv">0</span>, <span class="op">-</span><span class="dv">1</span><span class="op">/</span><span class="dv">2</span><span class="op">*</span>((dt<span class="op">*</span>q1<span class="op">*</span>wx<span class="op">*</span>wy <span class="op">+</span> dt<span class="op">*</span>q2<span class="op">*</span>wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> dt<span class="op">*</span>q3<span class="op">*</span>wy<span class="op">*</span>wz)<span class="op">*</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>cos(<span class="dv">1</span><span class="op">/</span><span class="dv">2</span><span class="op">*</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>dt) <span class="op">+</span> (dt<span class="op">*</span>q0<span class="op">*</span>wy<span class="op">^</span><span class="dv">3</span> <span class="op">+</span> <span class="dv">2</span><span class="op">*</span>q2<span class="op">*</span>wx<span class="op">^</span><span class="dv">2</span> <span class="op">-</span> <span class="dv">2</span><span class="op">*</span>q3<span class="op">*</span>wy<span class="op">*</span>wz <span class="op">+</span> (dt<span class="op">*</span>q0<span class="op">*</span>wy <span class="op">+</span> <span class="dv">2</span><span class="op">*</span>q2)<span class="op">*</span>wz<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> (dt<span class="op">*</span>q0<span class="op">*</span>wx<span class="op">^</span><span class="dv">2</span> <span class="op">-</span> <span class="dv">2</span><span class="op">*</span>q1<span class="op">*</span>wx)<span class="op">*</span>wy)<span class="op">*</span>sin(<span class="dv">1</span><span class="op">/</span><span class="dv">2</span><span class="op">*</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>dt))<span class="op">/</span>(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>)<span class="op">^</span>(<span class="dv">3</span><span class="op">/</span><span class="dv">2</span>))
(wy, <span class="dv">1</span>, <span class="op">-</span><span class="dv">1</span><span class="op">/</span><span class="dv">2</span><span class="op">*</span>((dt<span class="op">*</span>q1<span class="op">*</span>wy<span class="op">^</span><span class="dv">3</span> <span class="op">+</span> <span class="dv">2</span><span class="op">*</span>q3<span class="op">*</span>wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> <span class="dv">2</span><span class="op">*</span>q2<span class="op">*</span>wy<span class="op">*</span>wz <span class="op">+</span> (dt<span class="op">*</span>q1<span class="op">*</span>wy <span class="op">+</span> <span class="dv">2</span><span class="op">*</span>q3)<span class="op">*</span>wz<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> (dt<span class="op">*</span>q1<span class="op">*</span>wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> <span class="dv">2</span><span class="op">*</span>q0<span class="op">*</span>wx)<span class="op">*</span>wy)<span class="op">*</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>sin(<span class="dv">1</span><span class="op">/</span><span class="dv">2</span><span class="op">*</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>dt) <span class="op">-</span> (dt<span class="op">*</span>q0<span class="op">*</span>wx<span class="op">^</span><span class="dv">3</span><span class="op">*</span>wy <span class="op">-</span> dt<span class="op">*</span>q3<span class="op">*</span>wx<span class="op">^</span><span class="dv">2</span><span class="op">*</span>wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> dt<span class="op">*</span>q0<span class="op">*</span>wx<span class="op">*</span>wy<span class="op">^</span><span class="dv">3</span> <span class="op">-</span> dt<span class="op">*</span>q3<span class="op">*</span>wy<span class="op">^</span><span class="dv">4</span> <span class="op">+</span> dt<span class="op">*</span>q2<span class="op">*</span>wy<span class="op">*</span>wz<span class="op">^</span><span class="dv">3</span> <span class="op">+</span> (dt<span class="op">*</span>q0<span class="op">*</span>wx<span class="op">*</span>wy <span class="op">-</span> dt<span class="op">*</span>q3<span class="op">*</span>wy<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>wz<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> (dt<span class="op">*</span>q2<span class="op">*</span>wx<span class="op">^</span><span class="dv">2</span><span class="op">*</span>wy <span class="op">+</span> dt<span class="op">*</span>q2<span class="op">*</span>wy<span class="op">^</span><span class="dv">3</span>)<span class="op">*</span>wz)<span class="op">*</span>cos(<span class="dv">1</span><span class="op">/</span><span class="dv">2</span><span class="op">*</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>dt))<span class="op">/</span>(wx<span class="op">^</span><span class="dv">4</span> <span class="op">+</span> <span class="dv">2</span><span class="op">*</span>wx<span class="op">^</span><span class="dv">2</span><span class="op">*</span>wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">4</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">4</span> <span class="op">+</span> <span class="dv">2</span><span class="op">*</span>(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>wz<span class="op">^</span><span class="dv">2</span>))
(wy, <span class="dv">2</span>, <span class="dv">1</span><span class="op">/</span><span class="dv">2</span><span class="op">*</span>((dt<span class="op">*</span>q3<span class="op">*</span>wx<span class="op">*</span>wy <span class="op">+</span> dt<span class="op">*</span>q0<span class="op">*</span>wy<span class="op">^</span><span class="dv">2</span> <span class="op">-</span> dt<span class="op">*</span>q1<span class="op">*</span>wy<span class="op">*</span>wz)<span class="op">*</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>cos(<span class="dv">1</span><span class="op">/</span><span class="dv">2</span><span class="op">*</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>dt) <span class="op">-</span> (dt<span class="op">*</span>q2<span class="op">*</span>wy<span class="op">^</span><span class="dv">3</span> <span class="op">-</span> <span class="dv">2</span><span class="op">*</span>q0<span class="op">*</span>wx<span class="op">^</span><span class="dv">2</span> <span class="op">-</span> <span class="dv">2</span><span class="op">*</span>q1<span class="op">*</span>wy<span class="op">*</span>wz <span class="op">+</span> (dt<span class="op">*</span>q2<span class="op">*</span>wy <span class="op">-</span> <span class="dv">2</span><span class="op">*</span>q0)<span class="op">*</span>wz<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> (dt<span class="op">*</span>q2<span class="op">*</span>wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> <span class="dv">2</span><span class="op">*</span>q3<span class="op">*</span>wx)<span class="op">*</span>wy)<span class="op">*</span>sin(<span class="dv">1</span><span class="op">/</span><span class="dv">2</span><span class="op">*</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>dt))<span class="op">/</span>(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>)<span class="op">^</span>(<span class="dv">3</span><span class="op">/</span><span class="dv">2</span>))
(wy, <span class="dv">3</span>, <span class="op">-</span><span class="dv">1</span><span class="op">/</span><span class="dv">2</span><span class="op">*</span>((dt<span class="op">*</span>q3<span class="op">*</span>wy<span class="op">^</span><span class="dv">3</span> <span class="op">-</span> <span class="dv">2</span><span class="op">*</span>q1<span class="op">*</span>wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> <span class="dv">2</span><span class="op">*</span>q0<span class="op">*</span>wy<span class="op">*</span>wz <span class="op">+</span> (dt<span class="op">*</span>q3<span class="op">*</span>wy <span class="op">-</span> <span class="dv">2</span><span class="op">*</span>q1)<span class="op">*</span>wz<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> (dt<span class="op">*</span>q3<span class="op">*</span>wx<span class="op">^</span><span class="dv">2</span> <span class="op">-</span> <span class="dv">2</span><span class="op">*</span>q2<span class="op">*</span>wx)<span class="op">*</span>wy)<span class="op">*</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>sin(<span class="dv">1</span><span class="op">/</span><span class="dv">2</span><span class="op">*</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>dt) <span class="op">+</span> (dt<span class="op">*</span>q2<span class="op">*</span>wx<span class="op">^</span><span class="dv">3</span><span class="op">*</span>wy <span class="op">-</span> dt<span class="op">*</span>q1<span class="op">*</span>wx<span class="op">^</span><span class="dv">2</span><span class="op">*</span>wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> dt<span class="op">*</span>q2<span class="op">*</span>wx<span class="op">*</span>wy<span class="op">^</span><span class="dv">3</span> <span class="op">-</span> dt<span class="op">*</span>q1<span class="op">*</span>wy<span class="op">^</span><span class="dv">4</span> <span class="op">-</span> dt<span class="op">*</span>q0<span class="op">*</span>wy<span class="op">*</span>wz<span class="op">^</span><span class="dv">3</span> <span class="op">+</span> (dt<span class="op">*</span>q2<span class="op">*</span>wx<span class="op">*</span>wy <span class="op">-</span> dt<span class="op">*</span>q1<span class="op">*</span>wy<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>wz<span class="op">^</span><span class="dv">2</span> <span class="op">-</span> (dt<span class="op">*</span>q0<span class="op">*</span>wx<span class="op">^</span><span class="dv">2</span><span class="op">*</span>wy <span class="op">+</span> dt<span class="op">*</span>q0<span class="op">*</span>wy<span class="op">^</span><span class="dv">3</span>)<span class="op">*</span>wz)<span class="op">*</span>cos(<span class="dv">1</span><span class="op">/</span><span class="dv">2</span><span class="op">*</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>dt))<span class="op">/</span>(wx<span class="op">^</span><span class="dv">4</span> <span class="op">+</span> <span class="dv">2</span><span class="op">*</span>wx<span class="op">^</span><span class="dv">2</span><span class="op">*</span>wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">4</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">4</span> <span class="op">+</span> <span class="dv">2</span><span class="op">*</span>(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>wz<span class="op">^</span><span class="dv">2</span>))
(wz, <span class="dv">0</span>, <span class="op">-</span><span class="dv">1</span><span class="op">/</span><span class="dv">2</span><span class="op">*</span>((dt<span class="op">*</span>q3<span class="op">*</span>wz<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> (dt<span class="op">*</span>q1<span class="op">*</span>wx <span class="op">+</span> dt<span class="op">*</span>q2<span class="op">*</span>wy)<span class="op">*</span>wz)<span class="op">*</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>cos(<span class="dv">1</span><span class="op">/</span><span class="dv">2</span><span class="op">*</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>dt) <span class="op">+</span> (dt<span class="op">*</span>q0<span class="op">*</span>wz<span class="op">^</span><span class="dv">3</span> <span class="op">+</span> <span class="dv">2</span><span class="op">*</span>q3<span class="op">*</span>wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> <span class="dv">2</span><span class="op">*</span>q3<span class="op">*</span>wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> (dt<span class="op">*</span>q0<span class="op">*</span>wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> dt<span class="op">*</span>q0<span class="op">*</span>wy<span class="op">^</span><span class="dv">2</span> <span class="op">-</span> <span class="dv">2</span><span class="op">*</span>q1<span class="op">*</span>wx <span class="op">-</span> <span class="dv">2</span><span class="op">*</span>q2<span class="op">*</span>wy)<span class="op">*</span>wz)<span class="op">*</span>sin(<span class="dv">1</span><span class="op">/</span><span class="dv">2</span><span class="op">*</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>dt))<span class="op">/</span>(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>)<span class="op">^</span>(<span class="dv">3</span><span class="op">/</span><span class="dv">2</span>))
(wz, <span class="dv">1</span>, <span class="op">-</span><span class="dv">1</span><span class="op">/</span><span class="dv">2</span><span class="op">*</span>((dt<span class="op">*</span>q1<span class="op">*</span>wz<span class="op">^</span><span class="dv">3</span> <span class="op">-</span> <span class="dv">2</span><span class="op">*</span>q2<span class="op">*</span>wx<span class="op">^</span><span class="dv">2</span> <span class="op">-</span> <span class="dv">2</span><span class="op">*</span>q2<span class="op">*</span>wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> (dt<span class="op">*</span>q1<span class="op">*</span>wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> dt<span class="op">*</span>q1<span class="op">*</span>wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> <span class="dv">2</span><span class="op">*</span>q0<span class="op">*</span>wx <span class="op">-</span> <span class="dv">2</span><span class="op">*</span>q3<span class="op">*</span>wy)<span class="op">*</span>wz)<span class="op">*</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>sin(<span class="dv">1</span><span class="op">/</span><span class="dv">2</span><span class="op">*</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>dt) <span class="op">-</span> (dt<span class="op">*</span>q2<span class="op">*</span>wz<span class="op">^</span><span class="dv">4</span> <span class="op">+</span> (dt<span class="op">*</span>q0<span class="op">*</span>wx <span class="op">-</span> dt<span class="op">*</span>q3<span class="op">*</span>wy)<span class="op">*</span>wz<span class="op">^</span><span class="dv">3</span> <span class="op">+</span> (dt<span class="op">*</span>q2<span class="op">*</span>wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> dt<span class="op">*</span>q2<span class="op">*</span>wy<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>wz<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> (dt<span class="op">*</span>q0<span class="op">*</span>wx<span class="op">^</span><span class="dv">3</span> <span class="op">-</span> dt<span class="op">*</span>q3<span class="op">*</span>wx<span class="op">^</span><span class="dv">2</span><span class="op">*</span>wy <span class="op">+</span> dt<span class="op">*</span>q0<span class="op">*</span>wx<span class="op">*</span>wy<span class="op">^</span><span class="dv">2</span> <span class="op">-</span> dt<span class="op">*</span>q3<span class="op">*</span>wy<span class="op">^</span><span class="dv">3</span>)<span class="op">*</span>wz)<span class="op">*</span>cos(<span class="dv">1</span><span class="op">/</span><span class="dv">2</span><span class="op">*</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>dt))<span class="op">/</span>(wx<span class="op">^</span><span class="dv">4</span> <span class="op">+</span> <span class="dv">2</span><span class="op">*</span>wx<span class="op">^</span><span class="dv">2</span><span class="op">*</span>wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">4</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">4</span> <span class="op">+</span> <span class="dv">2</span><span class="op">*</span>(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>wz<span class="op">^</span><span class="dv">2</span>))
(wz, <span class="dv">2</span>, <span class="op">-</span><span class="dv">1</span><span class="op">/</span><span class="dv">2</span><span class="op">*</span>((dt<span class="op">*</span>q1<span class="op">*</span>wz<span class="op">^</span><span class="dv">2</span> <span class="op">-</span> (dt<span class="op">*</span>q3<span class="op">*</span>wx <span class="op">+</span> dt<span class="op">*</span>q0<span class="op">*</span>wy)<span class="op">*</span>wz)<span class="op">*</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>cos(<span class="dv">1</span><span class="op">/</span><span class="dv">2</span><span class="op">*</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>dt) <span class="op">+</span> (dt<span class="op">*</span>q2<span class="op">*</span>wz<span class="op">^</span><span class="dv">3</span> <span class="op">+</span> <span class="dv">2</span><span class="op">*</span>q1<span class="op">*</span>wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> <span class="dv">2</span><span class="op">*</span>q1<span class="op">*</span>wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> (dt<span class="op">*</span>q2<span class="op">*</span>wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> dt<span class="op">*</span>q2<span class="op">*</span>wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> <span class="dv">2</span><span class="op">*</span>q3<span class="op">*</span>wx <span class="op">+</span> <span class="dv">2</span><span class="op">*</span>q0<span class="op">*</span>wy)<span class="op">*</span>wz)<span class="op">*</span>sin(<span class="dv">1</span><span class="op">/</span><span class="dv">2</span><span class="op">*</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>dt))<span class="op">/</span>(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>)<span class="op">^</span>(<span class="dv">3</span><span class="op">/</span><span class="dv">2</span>))
(wz, <span class="dv">3</span>, <span class="op">-</span><span class="dv">1</span><span class="op">/</span><span class="dv">2</span><span class="op">*</span>((dt<span class="op">*</span>q3<span class="op">*</span>wz<span class="op">^</span><span class="dv">3</span> <span class="op">-</span> <span class="dv">2</span><span class="op">*</span>q0<span class="op">*</span>wx<span class="op">^</span><span class="dv">2</span> <span class="op">-</span> <span class="dv">2</span><span class="op">*</span>q0<span class="op">*</span>wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> (dt<span class="op">*</span>q3<span class="op">*</span>wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> dt<span class="op">*</span>q3<span class="op">*</span>wy<span class="op">^</span><span class="dv">2</span> <span class="op">-</span> <span class="dv">2</span><span class="op">*</span>q2<span class="op">*</span>wx <span class="op">+</span> <span class="dv">2</span><span class="op">*</span>q1<span class="op">*</span>wy)<span class="op">*</span>wz)<span class="op">*</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>sin(<span class="dv">1</span><span class="op">/</span><span class="dv">2</span><span class="op">*</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>dt) <span class="op">-</span> (dt<span class="op">*</span>q0<span class="op">*</span>wz<span class="op">^</span><span class="dv">4</span> <span class="op">-</span> (dt<span class="op">*</span>q2<span class="op">*</span>wx <span class="op">-</span> dt<span class="op">*</span>q1<span class="op">*</span>wy)<span class="op">*</span>wz<span class="op">^</span><span class="dv">3</span> <span class="op">+</span> (dt<span class="op">*</span>q0<span class="op">*</span>wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> dt<span class="op">*</span>q0<span class="op">*</span>wy<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>wz<span class="op">^</span><span class="dv">2</span> <span class="op">-</span> (dt<span class="op">*</span>q2<span class="op">*</span>wx<span class="op">^</span><span class="dv">3</span> <span class="op">-</span> dt<span class="op">*</span>q1<span class="op">*</span>wx<span class="op">^</span><span class="dv">2</span><span class="op">*</span>wy <span class="op">+</span> dt<span class="op">*</span>q2<span class="op">*</span>wx<span class="op">*</span>wy<span class="op">^</span><span class="dv">2</span> <span class="op">-</span> dt<span class="op">*</span>q1<span class="op">*</span>wy<span class="op">^</span><span class="dv">3</span>)<span class="op">*</span>wz)<span class="op">*</span>cos(<span class="dv">1</span><span class="op">/</span><span class="dv">2</span><span class="op">*</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>dt))<span class="op">/</span>(wx<span class="op">^</span><span class="dv">4</span> <span class="op">+</span> <span class="dv">2</span><span class="op">*</span>wx<span class="op">^</span><span class="dv">2</span><span class="op">*</span>wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">4</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">4</span> <span class="op">+</span> <span class="dv">2</span><span class="op">*</span>(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>wz<span class="op">^</span><span class="dv">2</span>))
(q0, <span class="dv">0</span>, cos(<span class="dv">1</span><span class="op">/</span><span class="dv">2</span><span class="op">*</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>dt))
(q0, <span class="dv">1</span>, wx<span class="op">*</span>sin(<span class="dv">1</span><span class="op">/</span><span class="dv">2</span><span class="op">*</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>dt)<span class="op">/</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>))
(q0, <span class="dv">2</span>, wy<span class="op">*</span>sin(<span class="dv">1</span><span class="op">/</span><span class="dv">2</span><span class="op">*</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>dt)<span class="op">/</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>))
(q0, <span class="dv">3</span>, wz<span class="op">*</span>sin(<span class="dv">1</span><span class="op">/</span><span class="dv">2</span><span class="op">*</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>dt)<span class="op">/</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>))
(q1, <span class="dv">0</span>, <span class="op">-</span>wx<span class="op">*</span>sin(<span class="dv">1</span><span class="op">/</span><span class="dv">2</span><span class="op">*</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>dt)<span class="op">/</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>))
(q1, <span class="dv">1</span>, cos(<span class="dv">1</span><span class="op">/</span><span class="dv">2</span><span class="op">*</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>dt))
(q1, <span class="dv">2</span>, <span class="op">-</span>wz<span class="op">*</span>sin(<span class="dv">1</span><span class="op">/</span><span class="dv">2</span><span class="op">*</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>dt)<span class="op">/</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>))
(q1, <span class="dv">3</span>, wy<span class="op">*</span>sin(<span class="dv">1</span><span class="op">/</span><span class="dv">2</span><span class="op">*</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>dt)<span class="op">/</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>))
(q2, <span class="dv">0</span>, <span class="op">-</span>wy<span class="op">*</span>sin(<span class="dv">1</span><span class="op">/</span><span class="dv">2</span><span class="op">*</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>dt)<span class="op">/</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>))
(q2, <span class="dv">1</span>, wz<span class="op">*</span>sin(<span class="dv">1</span><span class="op">/</span><span class="dv">2</span><span class="op">*</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>dt)<span class="op">/</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>))
(q2, <span class="dv">2</span>, cos(<span class="dv">1</span><span class="op">/</span><span class="dv">2</span><span class="op">*</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>dt))
(q2, <span class="dv">3</span>, <span class="op">-</span>wx<span class="op">*</span>sin(<span class="dv">1</span><span class="op">/</span><span class="dv">2</span><span class="op">*</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>dt)<span class="op">/</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>))
(q3, <span class="dv">0</span>, <span class="op">-</span>wz<span class="op">*</span>sin(<span class="dv">1</span><span class="op">/</span><span class="dv">2</span><span class="op">*</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>dt)<span class="op">/</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>))
(q3, <span class="dv">1</span>, <span class="op">-</span>wy<span class="op">*</span>sin(<span class="dv">1</span><span class="op">/</span><span class="dv">2</span><span class="op">*</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>dt)<span class="op">/</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>))
(q3, <span class="dv">2</span>, wx<span class="op">*</span>sin(<span class="dv">1</span><span class="op">/</span><span class="dv">2</span><span class="op">*</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>dt)<span class="op">/</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>))
(q3, <span class="dv">3</span>, cos(<span class="dv">1</span><span class="op">/</span><span class="dv">2</span><span class="op">*</span>sqrt(wx<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wy<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> wz<span class="op">^</span><span class="dv">2</span>)<span class="op">*</span>dt))</code></pre></div>
<h2 id="h-partial-derivatives">H partial derivatives</h2>
<div class="sourceCode"><pre class="sourceCode python"><code class="sourceCode python">Q.<span class="op"><</span>i,j,k<span class="op">></span> <span class="op">=</span> QuaternionAlgebra(SR, <span class="op">-</span><span class="dv">1</span>, <span class="op">-</span><span class="dv">1</span>)
var(<span class="st">'q0, q1, q2, q3'</span>)
var(<span class="st">'ax, ay, az'</span>)
q <span class="op">=</span> q0 <span class="op">+</span> q1<span class="op">*</span>i <span class="op">+</span> q2<span class="op">*</span>j <span class="op">+</span> q3<span class="op">*</span>k
<span class="co">#vector to quaternion</span>
v <span class="op">=</span> ax<span class="op">*</span>i <span class="op">+</span> ay<span class="op">*</span>j <span class="op">+</span> az<span class="op">*</span>k
<span class="co">#Do a rotation of the vector v</span>
nq <span class="op">=</span> q<span class="op">*</span>v<span class="op">*</span>q.conjugate()
v <span class="op">=</span> vector(nq.coefficient_tuple())
vs <span class="op">=</span> <span class="bu">map</span>(<span class="kw">lambda</span> x: x.canonicalize_radical().full_simplify(), v)
<span class="cf">for</span> sym <span class="kw">in</span> [ax, ay, az, q0, q1, q2, q3]:
d <span class="op">=</span> diff(v, sym)
exps <span class="op">=</span> <span class="bu">map</span>(<span class="kw">lambda</span> x: x.canonicalize_radical().full_simplify(), d)[<span class="dv">1</span>:]
<span class="cf">for</span> i, e <span class="kw">in</span> <span class="bu">enumerate</span>(exps):
<span class="bu">print</span>(sym, i, e)
<span class="co">#Here are the results</span>
(ax, <span class="dv">0</span>, q0<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> q1<span class="op">^</span><span class="dv">2</span> <span class="op">-</span> q2<span class="op">^</span><span class="dv">2</span> <span class="op">-</span> q3<span class="op">^</span><span class="dv">2</span>)
(ax, <span class="dv">1</span>, <span class="dv">2</span><span class="op">*</span>q1<span class="op">*</span>q2 <span class="op">+</span> <span class="dv">2</span><span class="op">*</span>q0<span class="op">*</span>q3)
(ax, <span class="dv">2</span>, <span class="op">-</span><span class="dv">2</span><span class="op">*</span>q0<span class="op">*</span>q2 <span class="op">+</span> <span class="dv">2</span><span class="op">*</span>q1<span class="op">*</span>q3)
(ay, <span class="dv">0</span>, <span class="dv">2</span><span class="op">*</span>q1<span class="op">*</span>q2 <span class="op">-</span> <span class="dv">2</span><span class="op">*</span>q0<span class="op">*</span>q3)
(ay, <span class="dv">1</span>, q0<span class="op">^</span><span class="dv">2</span> <span class="op">-</span> q1<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> q2<span class="op">^</span><span class="dv">2</span> <span class="op">-</span> q3<span class="op">^</span><span class="dv">2</span>)
(ay, <span class="dv">2</span>, <span class="dv">2</span><span class="op">*</span>q0<span class="op">*</span>q1 <span class="op">+</span> <span class="dv">2</span><span class="op">*</span>q2<span class="op">*</span>q3)
(az, <span class="dv">0</span>, <span class="dv">2</span><span class="op">*</span>q0<span class="op">*</span>q2 <span class="op">+</span> <span class="dv">2</span><span class="op">*</span>q1<span class="op">*</span>q3)
(az, <span class="dv">1</span>, <span class="op">-</span><span class="dv">2</span><span class="op">*</span>q0<span class="op">*</span>q1 <span class="op">+</span> <span class="dv">2</span><span class="op">*</span>q2<span class="op">*</span>q3)
(az, <span class="dv">2</span>, q0<span class="op">^</span><span class="dv">2</span> <span class="op">-</span> q1<span class="op">^</span><span class="dv">2</span> <span class="op">-</span> q2<span class="op">^</span><span class="dv">2</span> <span class="op">+</span> q3<span class="op">^</span><span class="dv">2</span>)
(q0, <span class="dv">0</span>, <span class="dv">2</span><span class="op">*</span>ax<span class="op">*</span>q0 <span class="op">+</span> <span class="dv">2</span><span class="op">*</span>az<span class="op">*</span>q2 <span class="op">-</span> <span class="dv">2</span><span class="op">*</span>ay<span class="op">*</span>q3)
(q0, <span class="dv">1</span>, <span class="dv">2</span><span class="op">*</span>ay<span class="op">*</span>q0 <span class="op">-</span> <span class="dv">2</span><span class="op">*</span>az<span class="op">*</span>q1 <span class="op">+</span> <span class="dv">2</span><span class="op">*</span>ax<span class="op">*</span>q3)
(q0, <span class="dv">2</span>, <span class="dv">2</span><span class="op">*</span>az<span class="op">*</span>q0 <span class="op">+</span> <span class="dv">2</span><span class="op">*</span>ay<span class="op">*</span>q1 <span class="op">-</span> <span class="dv">2</span><span class="op">*</span>ax<span class="op">*</span>q2)
(q1, <span class="dv">0</span>, <span class="dv">2</span><span class="op">*</span>ax<span class="op">*</span>q1 <span class="op">+</span> <span class="dv">2</span><span class="op">*</span>ay<span class="op">*</span>q2 <span class="op">+</span> <span class="dv">2</span><span class="op">*</span>az<span class="op">*</span>q3)
(q1, <span class="dv">1</span>, <span class="op">-</span><span class="dv">2</span><span class="op">*</span>az<span class="op">*</span>q0 <span class="op">-</span> <span class="dv">2</span><span class="op">*</span>ay<span class="op">*</span>q1 <span class="op">+</span> <span class="dv">2</span><span class="op">*</span>ax<span class="op">*</span>q2)
(q1, <span class="dv">2</span>, <span class="dv">2</span><span class="op">*</span>ay<span class="op">*</span>q0 <span class="op">-</span> <span class="dv">2</span><span class="op">*</span>az<span class="op">*</span>q1 <span class="op">+</span> <span class="dv">2</span><span class="op">*</span>ax<span class="op">*</span>q3)
(q2, <span class="dv">0</span>, <span class="dv">2</span><span class="op">*</span>az<span class="op">*</span>q0 <span class="op">+</span> <span class="dv">2</span><span class="op">*</span>ay<span class="op">*</span>q1 <span class="op">-</span> <span class="dv">2</span><span class="op">*</span>ax<span class="op">*</span>q2)
(q2, <span class="dv">1</span>, <span class="dv">2</span><span class="op">*</span>ax<span class="op">*</span>q1 <span class="op">+</span> <span class="dv">2</span><span class="op">*</span>ay<span class="op">*</span>q2 <span class="op">+</span> <span class="dv">2</span><span class="op">*</span>az<span class="op">*</span>q3)
(q2, <span class="dv">2</span>, <span class="op">-</span><span class="dv">2</span><span class="op">*</span>ax<span class="op">*</span>q0 <span class="op">-</span> <span class="dv">2</span><span class="op">*</span>az<span class="op">*</span>q2 <span class="op">+</span> <span class="dv">2</span><span class="op">*</span>ay<span class="op">*</span>q3)
(q3, <span class="dv">0</span>, <span class="op">-</span><span class="dv">2</span><span class="op">*</span>ay<span class="op">*</span>q0 <span class="op">+</span> <span class="dv">2</span><span class="op">*</span>az<span class="op">*</span>q1 <span class="op">-</span> <span class="dv">2</span><span class="op">*</span>ax<span class="op">*</span>q3)
(q3, <span class="dv">1</span>, <span class="dv">2</span><span class="op">*</span>ax<span class="op">*</span>q0 <span class="op">+</span> <span class="dv">2</span><span class="op">*</span>az<span class="op">*</span>q2 <span class="op">-</span> <span class="dv">2</span><span class="op">*</span>ay<span class="op">*</span>q3)
(q3, <span class="dv">2</span>, <span class="dv">2</span><span class="op">*</span>ax<span class="op">*</span>q1 <span class="op">+</span> <span class="dv">2</span><span class="op">*</span>ay<span class="op">*</span>q2 <span class="op">+</span> <span class="dv">2</span><span class="op">*</span>az<span class="op">*</span>q3)</code></pre></div>
<h1 id="references" class="unnumbered">References</h1>
<div id="refs" class="references">
<div id="ref-markley_averaging_2007">
<p>Markley, F. Landis, Yang Cheng, John Lucas Crassidis, and Yaakov Oshman. 2007. “Averaging Quaternions.” <em>Journal of Guidance, Control, and Dynamics</em> 30 (4): 1193–7. <a href="http://arc.aiaa.org/doi/abs/10.2514/1.28949" class="uri">http://arc.aiaa.org/doi/abs/10.2514/1.28949</a>.</p>
</div>
<div id="ref-vernaza_rao-blackwellized_2006">
<p>Vernaza, Paul, and Daniel D. Lee. 2006. “Rao-Blackwellized Particle Filtering for 6-DOF Estimation of Attitude and Position via GPS and Inertial Sensors.” In <em>Robotics and Automation, 2006. ICRA 2006. Proceedings 2006 IEEE International Conference on</em>, 1571–8. IEEE. <a href="http://ieeexplore.ieee.org/abstract/document/1641931/" class="uri">http://ieeexplore.ieee.org/abstract/document/1641931/</a>.</p>
</div>
<div id="ref-xavier_computing_1998">
<p>Xavier, Pennec. 1998. “Computing the Mean of Geometric Features Application to the Mean Rotation.” INRIA.</p>
</div>
</div>
<div class="footnotes">
<hr />
<ol>
<li id="fn1"><p>The etymology for “Dead reckoning” comes from the mariners of the XVIIth century that used to calculate the position of the vessel with log book. The interpretation of “dead” is subject to debate. Some argue that it is a mispelling of “ded” as in “deduced”. Others argue that it should be read by its old meaning: <em>absolute</em>.<a href="#fnref1">↩</a></p></li>
</ol>
</div>
</body>
</html>