ethzasl-msf - Modular Sensor Fusion
Time delay compensated single and multi sensor fusion framework based on an EKF
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
calcQCore.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2011-2012 Stephan Weiss, ASL, ETH Zurich, Switzerland
3  * You can contact the author at <stephan dot weiss at ieee dot org>
4  *
5  * Licensed under the Apache License, Version 2.0 (the "License");
6  * you may not use this file except in compliance with the License.
7  * You may obtain a copy of the License at
8  *
9  * http://www.apache.org/licenses/LICENSE-2.0
10  *
11  * Unless required by applicable law or agreed to in writing, software
12  * distributed under the License is distributed on an "AS IS" BASIS,
13  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14  * See the License for the specific language governing permissions and
15  * limitations under the License.
16  */
17 #ifndef CALCQ_H_
18 #define CALCQ_H_
19 
20 #include <Eigen/Eigen>
21 #include <msf_core/msf_core.h>
22 
28 template<typename StateSequence_T, typename StateDefinition_T, class Derived,
29  class DerivedQ>
30 void calc_QCore(const double dt, const Eigen::Quaternion<double> & q,
31  const Eigen::MatrixBase<Derived> & ew,
32  const Eigen::MatrixBase<Derived> & ea,
33  const Eigen::MatrixBase<Derived> & n_a,
34  const Eigen::MatrixBase<Derived> & n_ba,
35  const Eigen::MatrixBase<Derived> & n_w,
36  const Eigen::MatrixBase<Derived> & n_bw,
37  Eigen::MatrixBase<DerivedQ> & Qd) {
38 
39  const double q1 = q.w(), q2 = q.x(), q3 = q.y(), q4 = q.z();
40  const double ew1 = ew(0), ew2 = ew(1), ew3 = ew(2);
41  const double ea1 = ea(0), ea2 = ea(1), ea3 = ea(2);
42  const double n_a1 = n_a(0), n_a2 = n_a(1), n_a3 = n_a(2);
43  const double n_ba1 = n_ba(0), n_ba2 = n_ba(1), n_ba3 = n_ba(2);
44  const double n_w1 = n_w(0), n_w2 = n_w(1), n_w3 = n_w(2);
45  const double n_bw1 = n_bw(0), n_bw2 = n_bw(1), n_bw3 = n_bw(2);
46 
47  const double t343 = dt * dt;
48  const double t348 = q1 * q4 * 2.0;
49  const double t349 = q2 * q3 * 2.0;
50  const double t344 = t348 - t349;
51  const double t356 = q1 * q3 * 2.0;
52  const double t357 = q2 * q4 * 2.0;
53  const double t345 = t356 + t357;
54  const double t350 = q1 * q1;
55  const double t351 = q2 * q2;
56  const double t352 = q3 * q3;
57  const double t353 = q4 * q4;
58  const double t346 = t350 + t351 - t352 - t353;
59  const double t347 = n_a1 * n_a1;
60  const double t354 = n_a2 * n_a2;
61  const double t355 = n_a3 * n_a3;
62  const double t358 = q1 * q2 * 2.0;
63  const double t359 = t344 * t344;
64  const double t360 = t345 * t345;
65  const double t361 = t346 * t346;
66  const double t363 = ea2 * t345;
67  const double t364 = ea3 * t344;
68  const double t362 = t363 + t364;
69  const double t365 = t362 * t362;
70  const double t366 = t348 + t349;
71  const double t367 = t350 - t351 + t352 - t353;
72  const double t368 = q3 * q4 * 2.0;
73  const double t369 = t356 - t357;
74  const double t370 = t350 - t351 - t352 + t353;
75  const double t371 = n_w3 * n_w3;
76  const double t372 = t358 + t368;
77  const double t373 = n_w2 * n_w2;
78  const double t374 = n_w1 * n_w1;
79  const double t375 = dt * t343 * t346 * t347 * t366 * (1.0 / 3.0);
80  const double t376 = t358 - t368;
81  const double t377 = t343 * t346 * t347 * t366 * (1.0 / 2.0);
82  const double t378 = t366 * t366;
83  const double t379 = t376 * t376;
84  const double t380 = ea1 * t367;
85  const double t391 = ea2 * t366;
86  const double t381 = t380 - t391;
87  const double t382 = ea3 * t367;
88  const double t383 = ea2 * t376;
89  const double t384 = t382 + t383;
90  const double t385 = t367 * t367;
91  const double t386 = ea1 * t376;
92  const double t387 = ea3 * t366;
93  const double t388 = t386 + t387;
94  const double t389 = ea2 * t370;
95  const double t407 = ea3 * t372;
96  const double t390 = t389 - t407;
97  const double t392 = ea1 * t372;
98  const double t393 = ea2 * t369;
99  const double t394 = t392 + t393;
100  const double t395 = ea1 * t370;
101  const double t396 = ea3 * t369;
102  const double t397 = t395 + t396;
103  const double t398 = n_ba1 * n_ba1;
104  const double t399 = n_ba2 * n_ba2;
105  const double t400 = n_ba3 * n_ba3;
106  const double t401 = dt * t343 * t345 * t355 * t370 * (1.0 / 3.0);
107  const double t402 = t401 - dt * t343 * t346 * t347 * t369 * (1.0 / 3.0)
108  - dt * t343 * t344 * t354 * t372 * (1.0 / 3.0);
109  const double t403 = dt * t343 * t354 * t367 * t372 * (1.0 / 3.0);
110  const double t404 = t403 - dt * t343 * t347 * t366 * t369 * (1.0 / 3.0)
111  - dt * t343 * t355 * t370 * t376 * (1.0 / 3.0);
112  const double t405 = t343 * t345 * t355 * t370 * (1.0 / 2.0);
113  const double t406 = dt * t343 * t362 * t373 * t397 * (1.0 / 6.0);
114  const double t421 = t343 * t346 * t347 * t369 * (1.0 / 2.0);
115  const double t422 = dt * t343 * t362 * t371 * t394 * (1.0 / 6.0);
116  const double t423 = t343 * t344 * t354 * t372 * (1.0 / 2.0);
117  const double t424 = dt * t343 * t362 * t374 * t390 * (1.0 / 6.0);
118  const double t408 = t405 + t406 - t421 - t422 - t423 - t424;
119  const double t409 = t343 * t354 * t367 * t372 * (1.0 / 2.0);
120  const double t410 = dt * t343 * t374 * t384 * t390 * (1.0 / 6.0);
121  const double t411 = dt * t343 * t373 * t388 * t397 * (1.0 / 6.0);
122  const double t463 = t343 * t355 * t370 * t376 * (1.0 / 2.0);
123  const double t464 = t343 * t347 * t366 * t369 * (1.0 / 2.0);
124  const double t465 = dt * t343 * t371 * t381 * t394 * (1.0 / 6.0);
125  const double t412 = t409 + t410 + t411 - t463 - t464 - t465;
126  const double t413 = t369 * t369;
127  const double t414 = t372 * t372;
128  const double t415 = t370 * t370;
129  const double t416 = t343 * t354 * t359 * (1.0 / 2.0);
130  const double t417 = t343 * t355 * t360 * (1.0 / 2.0);
131  const double t418 = t343 * t347 * t361 * (1.0 / 2.0);
132  const double t419 = t416 + t417 + t418 - dt * t343 * t365 * t371 * (1.0 / 6.0)
133  - dt * t343 * t365 * t373 * (1.0 / 6.0)
134  - dt * t343 * t365 * t374 * (1.0 / 6.0);
135  const double t453 = t343 * t344 * t354 * t367 * (1.0 / 2.0);
136  const double t454 = t343 * t345 * t355 * t376 * (1.0 / 2.0);
137  const double t420 = t377 - t453 - t454;
138  const double t426 = ew2 * t362;
139  const double t427 = ew3 * t362;
140  const double t425 = t426 - t427;
141  const double t428 = dt * t365;
142  const double t429 = ew1 * ew1;
143  const double t430 = ew2 * ew2;
144  const double t431 = ew3 * ew3;
145  const double t432 = t430 + t431;
146  const double t433 = t362 * t432;
147  const double t434 = ew1 * t343 * t365;
148  const double t435 = t429 + t431;
149  const double t436 = t362 * t435;
150  const double t443 = ew2 * ew3 * t362;
151  const double t437 = t433 + t436 - t443;
152  const double t438 = ew1 * t362 * t394;
153  const double t511 = ew1 * t362 * t397;
154  const double t439 = t438 - t511;
155  const double t440 = t343 * t439 * (1.0 / 2.0);
156  const double t441 = t429 + t430;
157  const double t442 = t362 * t441;
158  const double t444 = t390 * t432;
159  const double t445 = ew2 * t394;
160  const double t446 = ew3 * t397;
161  const double t447 = t445 + t446;
162  const double t448 = ew1 * ew2 * t362;
163  const double t449 = ew1 * ew3 * t362;
164  const double t450 = ew1 * ew3 * t362 * (1.0 / 2.0);
165  const double t451 = dt * t362;
166  const double t452 = ew1 * t343 * t362 * (1.0 / 2.0);
167  const double t455 = dt * t343 * t362 * t374 * t384 * (1.0 / 6.0);
168  const double t456 = t343 * t347 * t378 * (1.0 / 2.0);
169  const double t457 = t343 * t355 * t379 * (1.0 / 2.0);
170  const double t458 = t381 * t381;
171  const double t459 = t384 * t384;
172  const double t460 = t343 * t354 * t385 * (1.0 / 2.0);
173  const double t461 = t388 * t388;
174  const double t462 = t456 + t457 + t460 - dt * t343 * t371 * t458 * (1.0 / 6.0)
175  - dt * t343 * t374 * t459 * (1.0 / 6.0)
176  - dt * t343 * t373 * t461 * (1.0 / 6.0);
177  const double t466 = t433 + t442 - t443;
178  const double t467 = ew1 * t362 * t388;
179  const double t468 = ew1 * t362 * t381;
180  const double t469 = t467 + t468;
181  const double t470 = t343 * t469 * (1.0 / 2.0);
182  const double t471 = t384 * t432;
183  const double t472 = ew2 * t381;
184  const double t479 = ew3 * t388;
185  const double t473 = t472 - t479;
186  const double t474 = -t433 + t448 + t449;
187  const double t475 = dt * t343 * t346 * t366 * t398 * (1.0 / 3.0);
188  const double t476 = dt * t346 * t347 * t366;
189  const double t477 = ew2 * ew3 * t381;
190  const double t492 = t388 * t435;
191  const double t478 = t471 + t477 - t492;
192  const double t480 = t472 - t479;
193  const double t481 = ew1 * ew3 * t381;
194  const double t482 = ew1 * ew2 * t388;
195  const double t483 = t471 + t481 + t482;
196  const double t484 = ew2 * ew3 * t388;
197  const double t486 = t381 * t441;
198  const double t485 = t471 + t484 - t486;
199  const double t487 = t394 * t441;
200  const double t488 = ew2 * ew3 * t397;
201  const double t489 = t444 + t487 + t488;
202  const double t490 = t397 * t435;
203  const double t491 = ew2 * ew3 * t394;
204  const double t493 = ew1 * t381 * t397;
205  const double t541 = ew1 * t388 * t394;
206  const double t494 = t493 - t541;
207  const double t495 = t343 * t494 * (1.0 / 2.0);
208  const double t496 = ew1 * ew2 * t397;
209  const double t527 = ew1 * ew3 * t394;
210  const double t497 = t444 + t496 - t527;
211  const double t498 = ew2 * ew3 * t381 * (1.0 / 2.0);
212  const double t499 = ew1 * t343 * t381 * (1.0 / 2.0);
213  const double t500 = t384 * t432 * (1.0 / 2.0);
214  const double t501 = ew2 * ew3 * t388 * (1.0 / 2.0);
215  const double t502 = n_bw1 * n_bw1;
216  const double t503 = n_bw3 * n_bw3;
217  const double t504 = t343 * t347 * t413 * (1.0 / 2.0);
218  const double t505 = t343 * t354 * t414 * (1.0 / 2.0);
219  const double t506 = t397 * t397;
220  const double t507 = t390 * t390;
221  const double t508 = t343 * t355 * t415 * (1.0 / 2.0);
222  const double t509 = t394 * t394;
223  const double t510 = t504 + t505 + t508 - dt * t343 * t373 * t506 * (1.0 / 6.0)
224  - dt * t343 * t371 * t509 * (1.0 / 6.0)
225  - dt * t343 * t374 * t507 * (1.0 / 6.0);
226  const double t512 = -t444 + t490 + t491;
227  const double t513 = t397 * t437 * (1.0 / 2.0);
228  const double t514 = t362 * t394 * t429;
229  const double t515 = dt * t362 * t397;
230  const double t516 = t362 * t489 * (1.0 / 2.0);
231  const double t517 = t394 * t466 * (1.0 / 2.0);
232  const double t518 = t362 * t397 * t429;
233  const double t519 = t516 + t517 + t518;
234  const double t520 = dt * t362 * t394;
235  const double t521 = t440 + t520 - dt * t343 * t519 * (1.0 / 3.0);
236  const double t522 = t371 * t521;
237  const double t523 = t362 * t447;
238  const double t524 = t390 * t425;
239  const double t525 = t523 + t524;
240  const double t526 = t343 * t525 * (1.0 / 2.0);
241  const double t528 = t425 * t447;
242  const double t529 = t390 * t474 * (1.0 / 2.0);
243  const double t530 = t528 + t529 - t362 * t497 * (1.0 / 2.0);
244  const double t531 = dt * t343 * t530 * (1.0 / 3.0);
245  const double t532 = dt * t362 * t390;
246  const double t533 = t526 + t531 + t532;
247  const double t534 = t374 * t533;
248  const double t535 = dt * t343 * t345 * t370 * t400 * (1.0 / 3.0);
249  const double t536 = dt * t345 * t355 * t370;
250  const double t537 = t381 * t489 * (1.0 / 2.0);
251  const double t538 = t388 * t397 * t429;
252  const double t539 = t537 + t538 - t394 * t485 * (1.0 / 2.0);
253  const double t540 = dt * t343 * t539 * (1.0 / 3.0);
254  const double t542 = t495 + t540 - dt * t381 * t394;
255  const double t543 = t388 * t512 * (1.0 / 2.0);
256  const double t544 = t381 * t394 * t429;
257  const double t545 = t543 + t544 - t397 * t478 * (1.0 / 2.0);
258  const double t546 = dt * t343 * t545 * (1.0 / 3.0);
259  const double t547 = t495 + t546 - dt * t388 * t397;
260  const double t548 = t373 * t547;
261  const double t549 = t384 * t447;
262  const double t550 = t549 - t390 * t473;
263  const double t551 = t343 * t550 * (1.0 / 2.0);
264  const double t552 = t384 * t497 * (1.0 / 2.0);
265  const double t553 = t390 * t483 * (1.0 / 2.0);
266  const double t554 = t447 * t473;
267  const double t555 = t552 + t553 + t554;
268  const double t556 = dt * t384 * t390;
269  const double t557 = t551 + t556 - dt * t343 * t555 * (1.0 / 3.0);
270  const double t558 = dt * t343 * t367 * t372 * t399 * (1.0 / 3.0);
271  const double t559 = dt * t354 * t367 * t372;
272  const double t560 = t548 + t558 + t559 - t371 * t542 - t374 * t557
273  - dt * t347 * t366 * t369 - dt * t355 * t370 * t376
274  - dt * t343 * t366 * t369 * t398 * (1.0 / 3.0)
275  - dt * t343 * t370 * t376 * t400 * (1.0 / 3.0);
276  const double t561 = ew1 * t343 * t394 * t397;
277  const double t562 = ew1 * t343 * t397 * (1.0 / 2.0);
278  const double t563 = n_bw2 * n_bw2;
279  const double t564 = dt * t343 * t362 * t374 * (1.0 / 6.0);
280  const double t565 = dt * t343 * t374 * t390 * (1.0 / 6.0);
281  const double t566 = ew1 * ew2 * t362 * (1.0 / 2.0);
282  const double t567 = -t433 + t450 + t566;
283  const double t568 = dt * t343 * t567 * (1.0 / 3.0);
284  const double t569 = t343 * t425 * (1.0 / 2.0);
285  const double t570 = t451 + t568 + t569;
286  const double t571 = dt * t343 * t362 * t373 * t432 * (1.0 / 6.0);
287  const double t572 = dt * t343 * t362 * t371 * t432 * (1.0 / 6.0);
288  const double t573 = t571 + t572 - t374 * t570;
289  const double t574 = ew1 * ew2 * t397 * (1.0 / 2.0);
290  const double t575 = t444 + t574 - ew1 * ew3 * t394 * (1.0 / 2.0);
291  const double t576 = t343 * t447 * (1.0 / 2.0);
292  const double t577 = dt * t390;
293  const double t578 = t576 + t577 - dt * t343 * t575 * (1.0 / 3.0);
294  const double t579 = dt * t343 * t371 * t394 * t432 * (1.0 / 6.0);
295  const double t580 = t579 - t374 * t578
296  - dt * t343 * t373 * t397 * t432 * (1.0 / 6.0);
297  const double t581 = dt * t343 * t373 * t388 * (1.0 / 6.0);
298  const double t582 = t362 * t432 * (1.0 / 2.0);
299  const double t583 = ew2 * ew3 * t362 * (1.0 / 2.0);
300  const double t584 = t362 * t429;
301  const double t585 = t583 + t584;
302  const double t586 = ew3 * t473;
303  const double t587 = ew1 * ew2 * t384 * (1.0 / 2.0);
304  const double t588 = t586 + t587;
305  const double t589 = dt * t343 * t588 * (1.0 / 3.0);
306  const double t590 = t374 * (t589 - ew3 * t343 * t384 * (1.0 / 2.0));
307  const double t591 = t388 * t429;
308  const double t592 = t498 + t591;
309  const double t593 = dt * t343 * t592 * (1.0 / 3.0);
310  const double t594 = t499 + t593;
311  const double t595 = -t492 + t498 + t500;
312  const double t596 = dt * t343 * t595 * (1.0 / 3.0);
313  const double t597 = dt * t388;
314  const double t598 = -t499 + t596 + t597;
315  const double t599 = t590 - t371 * t594 - t373 * t598;
316  const double t600 = t397 * t429;
317  const double t601 = ew2 * ew3 * t394 * (1.0 / 2.0);
318  const double t602 = ew1 * t343 * t394 * (1.0 / 2.0);
319  const double t603 = ew3 * t447;
320  const double t604 = t603 - ew1 * ew2 * t390 * (1.0 / 2.0);
321  const double t605 = dt * t343 * t604 * (1.0 / 3.0);
322  const double t606 = ew3 * t343 * t390 * (1.0 / 2.0);
323  const double t607 = t605 + t606;
324  const double t608 = t374 * t607;
325  const double t609 = t390 * t432 * (1.0 / 2.0);
326  const double t610 = dt * t397;
327  const double t611 = t430 * (1.0 / 2.0);
328  const double t612 = t431 * (1.0 / 2.0);
329  const double t613 = t611 + t612;
330  const double t614 = ew1 * t343 * (1.0 / 2.0);
331  const double t615 = dt * t343 * t362 * t371 * (1.0 / 6.0);
332  const double t616 = dt * t343 * t371 * t381 * (1.0 / 6.0);
333  const double t617 = dt * t343 * t371 * t394 * (1.0 / 6.0);
334  const double t618 = ew2 * t425;
335  const double t619 = t450 + t618;
336  const double t620 = dt * t343 * t619 * (1.0 / 3.0);
337  const double t621 = ew2 * t343 * t362 * (1.0 / 2.0);
338  const double t622 = t620 + t621;
339  const double t623 = dt * t343 * t585 * (1.0 / 3.0);
340  const double t624 = t381 * t429;
341  const double t625 = t501 + t624;
342  const double t626 = dt * t343 * t625 * (1.0 / 3.0);
343  const double t627 = ew1 * t343 * t388 * (1.0 / 2.0);
344  const double t628 = ew2 * t473;
345  const double t629 = t628 - ew1 * ew3 * t384 * (1.0 / 2.0);
346  const double t630 = dt * t343 * t629 * (1.0 / 3.0);
347  const double t631 = t630 - ew2 * t343 * t384 * (1.0 / 2.0);
348  const double t632 = -t486 + t500 + t501;
349  const double t633 = dt * t343 * t632 * (1.0 / 3.0);
350  const double t634 = dt * t381;
351  const double t635 = t627 + t633 + t634;
352  const double t636 = ew2 * t447;
353  const double t637 = ew1 * ew3 * t390 * (1.0 / 2.0);
354  const double t638 = t636 + t637;
355  const double t639 = dt * t343 * t638 * (1.0 / 3.0);
356  const double t640 = ew2 * t343 * t390 * (1.0 / 2.0);
357  const double t641 = t639 + t640;
358  const double t642 = t394 * t429;
359  const double t643 = ew2 * ew3 * t397 * (1.0 / 2.0);
360  const double t644 = t487 + t609 + t643;
361  const double t645 = dt * t343 * t644 * (1.0 / 3.0);
362  const double t646 = t562 + t645 - dt * t394;
363  const double t647 = t371 * t646;
364  const double t648 = ew2 * t343 * (1.0 / 2.0);
365  const double t649 = dt * ew1 * ew3 * t343 * (1.0 / 6.0);
366  const double t650 = t648 + t649;
367  const double t651 = t374 * t650;
368  const double t652 = t651 - dt * t343 * t371 * t613 * (1.0 / 3.0);
369  const double t653 = dt * ew2 * ew3 * t343 * (1.0 / 6.0);
370  const double t654 = t614 + t653;
371  const double t655 = t371 * t654;
372  const double t656 = dt * t343 * t397 * t563 * (1.0 / 6.0);
373  const double t657 = dt * ew1 * t343 * t563 * (1.0 / 6.0);
374  const double t658 = dt * t343 * t369 * t398 * (1.0 / 6.0);
375  const double t659 = t343 * t369 * t398 * (1.0 / 2.0);
376  const double t660 = dt * t343 * t344 * t399 * (1.0 / 6.0);
377  const double t661 = t343 * t344 * t399 * (1.0 / 2.0);
378  const double t662 = dt * t343 * t376 * t400 * (1.0 / 6.0);
379  const double t663 = t343 * t376 * t400 * (1.0 / 2.0);
380 
381  enum {
382  idxstartcorr_p = msf_tmp::getStartIndexInCorrection<StateSequence_T,
383  StateDefinition_T::p>::value,
384  idxstartcorr_v = msf_tmp::getStartIndexInCorrection<StateSequence_T,
385  StateDefinition_T::v>::value,
386  idxstartcorr_q = msf_tmp::getStartIndexInCorrection<StateSequence_T,
387  StateDefinition_T::q>::value,
388  idxstartcorr_b_w = msf_tmp::getStartIndexInCorrection<StateSequence_T,
389  StateDefinition_T::b_w>::value,
390  idxstartcorr_b_a = msf_tmp::getStartIndexInCorrection<StateSequence_T,
391  StateDefinition_T::b_a>::value
392  };
393 
394  Qd(idxstartcorr_p + 0, idxstartcorr_p + 0) = dt * t343 * t347 * t361
395  * (1.0 / 3.0) + dt * t343 * t354 * t359 * (1.0 / 3.0)
396  + dt * t343 * t355 * t360 * (1.0 / 3.0);
397  Qd(idxstartcorr_p + 0, idxstartcorr_p + 1) = t375
398  - dt * t343 * t345 * t355 * (t358 - q3 * q4 * 2.0) * (1.0 / 3.0)
399  - dt * t343 * t344 * t354 * t367 * (1.0 / 3.0);
400  Qd(idxstartcorr_p + 0, idxstartcorr_p + 2) = t402;
401  Qd(idxstartcorr_p + 0, idxstartcorr_v + 0) = t419;
402  Qd(idxstartcorr_p + 0, idxstartcorr_v + 1) = t420;
403  Qd(idxstartcorr_p + 0, idxstartcorr_v + 2) = t408;
404  Qd(idxstartcorr_p + 0, idxstartcorr_q + 0) = t564;
405  Qd(idxstartcorr_p + 0, idxstartcorr_q + 2) = t615;
406  Qd(idxstartcorr_p + 0, idxstartcorr_b_a + 0) = dt * t343 * t346 * t398
407  * (-1.0 / 6.0);
408  Qd(idxstartcorr_p + 0, idxstartcorr_b_a + 1) = t660;
409  Qd(idxstartcorr_p + 0, idxstartcorr_b_a + 2) = dt * t343 * t345 * t400
410  * (-1.0 / 6.0);
411  Qd(idxstartcorr_p + 1, idxstartcorr_p + 0) = t375
412  - dt * t343 * t344 * t354 * t367 * (1.0 / 3.0)
413  - dt * t343 * t345 * t355 * t376 * (1.0 / 3.0);
414  Qd(idxstartcorr_p + 1, idxstartcorr_p + 1) = dt * t343 * t347 * t378
415  * (1.0 / 3.0) + dt * t343 * t355 * t379 * (1.0 / 3.0)
416  + dt * t343 * t354 * t385 * (1.0 / 3.0);
417  Qd(idxstartcorr_p + 1, idxstartcorr_p + 2) = t404;
418  Qd(idxstartcorr_p + 1, idxstartcorr_v + 0) = t377 + t455
419  - t343 * t344 * t354 * t367 * (1.0 / 2.0)
420  - t343 * t345 * t355 * t376 * (1.0 / 2.0)
421  - dt * t343 * t362 * t371 * t381 * (1.0 / 6.0)
422  - dt * t343 * t362 * t373 * t388 * (1.0 / 6.0);
423  Qd(idxstartcorr_p + 1, idxstartcorr_v + 1) = t462;
424  Qd(idxstartcorr_p + 1, idxstartcorr_v + 2) = t412;
425  Qd(idxstartcorr_p + 1, idxstartcorr_q + 0) = dt * t343 * t374 * t384
426  * (-1.0 / 6.0);
427  Qd(idxstartcorr_p + 1, idxstartcorr_q + 1) = t581;
428  Qd(idxstartcorr_p + 1, idxstartcorr_q + 2) = t616;
429  Qd(idxstartcorr_p + 1, idxstartcorr_b_a + 0) = dt * t343 * t366 * t398
430  * (-1.0 / 6.0);
431  Qd(idxstartcorr_p + 1, idxstartcorr_b_a + 1) = dt * t343 * t367 * t399
432  * (-1.0 / 6.0);
433  Qd(idxstartcorr_p + 1, idxstartcorr_b_a + 2) = t662;
434  Qd(idxstartcorr_p + 2, idxstartcorr_p + 0) = t402;
435  Qd(idxstartcorr_p + 2, idxstartcorr_p + 1) = t404;
436  Qd(idxstartcorr_p + 2, idxstartcorr_p + 2) = dt * t343 * t347 * t413
437  * (1.0 / 3.0) + dt * t343 * t354 * t414 * (1.0 / 3.0)
438  + dt * t343 * t355 * t415 * (1.0 / 3.0);
439  Qd(idxstartcorr_p + 2, idxstartcorr_v + 0) = t408;
440  Qd(idxstartcorr_p + 2, idxstartcorr_v + 1) = t412;
441  Qd(idxstartcorr_p + 2, idxstartcorr_v + 2) = t510;
442  Qd(idxstartcorr_p + 2, idxstartcorr_q + 0) = t565;
443  Qd(idxstartcorr_p + 2, idxstartcorr_q + 1) = dt * t343 * t373 * t397
444  * (-1.0 / 6.0);
445  Qd(idxstartcorr_p + 2, idxstartcorr_q + 2) = t617;
446  Qd(idxstartcorr_p + 2, idxstartcorr_b_a + 0) = t658;
447  Qd(idxstartcorr_p + 2, idxstartcorr_b_a + 1) = dt * t343 * t372 * t399
448  * (-1.0 / 6.0);
449  Qd(idxstartcorr_p + 2, idxstartcorr_b_a + 2) = dt * t343 * t370 * t400
450  * (-1.0 / 6.0);
451  Qd(idxstartcorr_v + 0, idxstartcorr_p + 0) = t419;
452  Qd(idxstartcorr_v + 0, idxstartcorr_p + 1) = t420;
453  Qd(idxstartcorr_v + 0, idxstartcorr_p + 2) = t408;
454  Qd(idxstartcorr_v + 0, idxstartcorr_v + 0) =
455  t374
456  * (t428 + t343 * t362 * t425
457  + dt * t343 * (t362 * (t448 + t449 - t362 * t432) + t425 * t425)
458  * (1.0 / 3.0))
459  + t373
460  * (t428 - t434
461  + dt * t343 * (t365 * t429 - t362 * t437) * (1.0 / 3.0))
462  + t371
463  * (t428 + t434
464  + dt * t343
465  * (t365 * t429 - t362 * (t433 + t442 - ew2 * ew3 * t362))
466  * (1.0 / 3.0)) + dt * t347 * t361 + dt * t354 * t359
467  + dt * t355 * t360 + dt * t343 * t359 * t399 * (1.0 / 3.0)
468  + dt * t343 * t361 * t398 * (1.0 / 3.0)
469  + dt * t343 * t360 * t400 * (1.0 / 3.0);
470  Qd(idxstartcorr_v + 0, idxstartcorr_v + 1) = t475 + t476
471  - dt * t344 * t354 * t367 - dt * t345 * t355 * t376
472  - dt * t343 * t344 * t367 * t399 * (1.0 / 3.0)
473  - dt * t343 * t345 * t376 * t400 * (1.0 / 3.0);
474  Qd(idxstartcorr_v + 0, idxstartcorr_v + 2) = t522 + t534 + t535 + t536
475  - t373
476  * (t440 + t515
477  - dt * t343
478  * (t513 + t514
479  + t362 * (t490 + t491 - t390 * t432) * (1.0 / 2.0))
480  * (1.0 / 3.0)) - dt * t346 * t347 * t369
481  - dt * t344 * t354 * t372 - dt * t343 * t346 * t369 * t398 * (1.0 / 3.0)
482  - dt * t343 * t344 * t372 * t399 * (1.0 / 3.0);
483  Qd(idxstartcorr_v + 0, idxstartcorr_q + 0) = t573;
484  Qd(idxstartcorr_v + 0, idxstartcorr_q + 2) = -t371
485  * (t451 + t452
486  - dt * t343 * (t442 + t582 - ew2 * ew3 * t362 * (1.0 / 2.0))
487  * (1.0 / 3.0)) - t374 * t622
488  + t373 * (t452 - dt * t343 * t585 * (1.0 / 3.0));
489  Qd(idxstartcorr_v + 0, idxstartcorr_b_w + 0) = dt * t343 * t362 * t502
490  * (-1.0 / 6.0);
491  Qd(idxstartcorr_v + 0, idxstartcorr_b_w + 2) = dt * t343 * t362 * t503
492  * (-1.0 / 6.0);
493  Qd(idxstartcorr_v + 0, idxstartcorr_b_a + 0) = t343 * t346 * t398
494  * (-1.0 / 2.0);
495  Qd(idxstartcorr_v + 0, idxstartcorr_b_a + 1) = t661;
496  Qd(idxstartcorr_v + 0, idxstartcorr_b_a + 2) = t343 * t345 * t400
497  * (-1.0 / 2.0);
498  Qd(idxstartcorr_v + 1, idxstartcorr_p + 0) = t377 - t453 - t454 + t455
499  - dt * t343 * t362 * t371 * t381 * (1.0 / 6.0)
500  - dt * t343 * t362 * t373 * t388 * (1.0 / 6.0);
501  Qd(idxstartcorr_v + 1, idxstartcorr_p + 1) = t462;
502  Qd(idxstartcorr_v + 1, idxstartcorr_p + 2) = t412;
503  Qd(idxstartcorr_v + 1, idxstartcorr_v + 0) = t475 + t476
504  - t374
505  * (t343 * (t384 * t425 - t362 * t473) * (1.0 / 2.0)
506  - dt * t343
507  * (t362 * t483 * (1.0 / 2.0) - t384 * t474 * (1.0 / 2.0)
508  + t425 * t473) * (1.0 / 3.0) + dt * t362 * t384)
509  + t371
510  * (t470 + dt * t362 * t381
511  + dt * t343
512  * (t362 * t485 * (1.0 / 2.0) - t381 * t466 * (1.0 / 2.0)
513  + t362 * t388 * t429) * (1.0 / 3.0))
514  + t373
515  * (-t470 + dt * t362 * t388
516  + dt * t343
517  * (t388 * t437 * (-1.0 / 2.0) + t362 * t478 * (1.0 / 2.0)
518  + t362 * t381 * t429) * (1.0 / 3.0))
519  - dt * t344 * t354 * t367 - dt * t345 * t355 * t376
520  - dt * t343 * t344 * t367 * t399 * (1.0 / 3.0)
521  - dt * t343 * t345 * t376 * t400 * (1.0 / 3.0);
522  Qd(idxstartcorr_v + 1, idxstartcorr_v + 1) = -t374
523  * (-dt * t459 + dt * t343 * (t384 * t483 - t480 * t480) * (1.0 / 3.0)
524  + t343 * t384 * t473)
525  + t373
526  * (dt * t461 + dt * t343 * (t388 * t478 + t429 * t458) * (1.0 / 3.0)
527  - ew1 * t343 * t381 * t388)
528  + t371
529  * (dt * t458 + dt * t343 * (t381 * t485 + t429 * t461) * (1.0 / 3.0)
530  + ew1 * t343 * t381 * t388) + dt * t347 * t378 + dt * t355 * t379
531  + dt * t354 * t385 + dt * t343 * t378 * t398 * (1.0 / 3.0)
532  + dt * t343 * t385 * t399 * (1.0 / 3.0)
533  + dt * t343 * t379 * t400 * (1.0 / 3.0);
534  Qd(idxstartcorr_v + 1, idxstartcorr_v + 2) = t560;
535  Qd(idxstartcorr_v + 1, idxstartcorr_q + 0) = -t374
536  * (-dt * t384 + t343 * t473 * (1.0 / 2.0)
537  + dt * t343
538  * (t471 + ew1 * ew2 * t388 * (1.0 / 2.0)
539  + ew1 * ew3 * t381 * (1.0 / 2.0)) * (1.0 / 3.0))
540  + dt * t343 * t371 * t381 * t432 * (1.0 / 6.0)
541  + dt * t343 * t373 * t388 * t432 * (1.0 / 6.0);
542  Qd(idxstartcorr_v + 1, idxstartcorr_q + 1) = t599;
543  Qd(idxstartcorr_v + 1, idxstartcorr_q + 2) = -t374 * t631 - t371 * t635
544  - t373 * (t626 - ew1 * t343 * t388 * (1.0 / 2.0));
545  Qd(idxstartcorr_v + 1, idxstartcorr_b_w + 0) = dt * t343 * t384 * t502
546  * (1.0 / 6.0);
547  Qd(idxstartcorr_v + 1, idxstartcorr_b_w + 1) = dt * t343 * t388 * t563
548  * (-1.0 / 6.0);
549  Qd(idxstartcorr_v + 1, idxstartcorr_b_w + 2) = dt * t343 * t381 * t503
550  * (-1.0 / 6.0);
551  Qd(idxstartcorr_v + 1, idxstartcorr_b_a + 0) = t343 * t366 * t398
552  * (-1.0 / 2.0);
553  Qd(idxstartcorr_v + 1, idxstartcorr_b_a + 1) = t343 * t367 * t399
554  * (-1.0 / 2.0);
555  Qd(idxstartcorr_v + 1, idxstartcorr_b_a + 2) = t663;
556  Qd(idxstartcorr_v + 2, idxstartcorr_p + 0) = t408;
557  Qd(idxstartcorr_v + 2, idxstartcorr_p + 1) = t412;
558  Qd(idxstartcorr_v + 2, idxstartcorr_p + 2) = t510;
559  Qd(idxstartcorr_v + 2, idxstartcorr_v + 0) = t522 + t534 + t535 + t536
560  - t373
561  * (t440 + t515
562  - dt * t343 * (t513 + t514 + t362 * t512 * (1.0 / 2.0))
563  * (1.0 / 3.0)) - dt * t346 * t347 * t369
564  - dt * t344 * t354 * t372 - dt * t343 * t346 * t369 * t398 * (1.0 / 3.0)
565  - dt * t343 * t344 * t372 * t399 * (1.0 / 3.0);
566  Qd(idxstartcorr_v + 2, idxstartcorr_v + 1) = t560;
567  Qd(idxstartcorr_v + 2, idxstartcorr_v + 2) = -t371
568  * (t561 - dt * t509
569  + dt * t343 * (t394 * t489 - t429 * t506) * (1.0 / 3.0))
570  + t373
571  * (t561 + dt * t506
572  - dt * t343 * (t397 * t512 - t429 * t509) * (1.0 / 3.0))
573  + t374
574  * (dt * t507 - dt * t343 * (t390 * t497 - t447 * t447) * (1.0 / 3.0)
575  + t343 * t390 * t447) + dt * t347 * t413 + dt * t354 * t414
576  + dt * t355 * t415 + dt * t343 * t398 * t413 * (1.0 / 3.0)
577  + dt * t343 * t399 * t414 * (1.0 / 3.0)
578  + dt * t343 * t400 * t415 * (1.0 / 3.0);
579  Qd(idxstartcorr_v + 2, idxstartcorr_q + 0) = t580;
580  Qd(idxstartcorr_v + 2, idxstartcorr_q + 1) = t608
581  + t371
582  * (dt * t343 * (t600 - ew2 * ew3 * t394 * (1.0 / 2.0)) * (1.0 / 3.0)
583  - ew1 * t343 * t394 * (1.0 / 2.0))
584  + t373
585  * (t602 + t610
586  - dt * t343 * (t490 + t601 - t390 * t432 * (1.0 / 2.0))
587  * (1.0 / 3.0));
588  Qd(idxstartcorr_v + 2, idxstartcorr_q + 2) = t647 - t374 * t641
589  - t373
590  * (t562
591  + dt * t343 * (t642 - ew2 * ew3 * t397 * (1.0 / 2.0))
592  * (1.0 / 3.0));
593  Qd(idxstartcorr_v + 2, idxstartcorr_b_w + 0) = dt * t343 * t390 * t502
594  * (-1.0 / 6.0);
595  Qd(idxstartcorr_v + 2, idxstartcorr_b_w + 1) = t656;
596  Qd(idxstartcorr_v + 2, idxstartcorr_b_w + 2) = dt * t343 * t394 * t503
597  * (-1.0 / 6.0);
598  Qd(idxstartcorr_v + 2, idxstartcorr_b_a + 0) = t659;
599  Qd(idxstartcorr_v + 2, idxstartcorr_b_a + 1) = t343 * t372 * t399
600  * (-1.0 / 2.0);
601  Qd(idxstartcorr_v + 2, idxstartcorr_b_a + 2) = t343 * t370 * t400
602  * (-1.0 / 2.0);
603  Qd(idxstartcorr_q + 0, idxstartcorr_p + 0) = t564;
604  Qd(idxstartcorr_q + 0, idxstartcorr_p + 2) = t565;
605  Qd(idxstartcorr_q + 0, idxstartcorr_v + 0) = t573;
606  Qd(idxstartcorr_q + 0, idxstartcorr_v + 2) = t580;
607  Qd(idxstartcorr_q + 0, idxstartcorr_q + 0) = t374
608  * (dt - dt * t343 * t432 * (1.0 / 3.0)) + dt * t343 * t502 * (1.0 / 3.0);
609  Qd(idxstartcorr_q + 0, idxstartcorr_q + 2) = t652;
610  Qd(idxstartcorr_q + 0, idxstartcorr_b_w + 0) = t343 * t502 * (-1.0 / 2.0);
611  Qd(idxstartcorr_q + 1, idxstartcorr_p + 0) = dt * t343 * t362 * t373
612  * (1.0 / 6.0);
613  Qd(idxstartcorr_q + 1, idxstartcorr_p + 1) = t581;
614  Qd(idxstartcorr_q + 1, idxstartcorr_p + 2) = dt * t343 * t373 * t397
615  * (-1.0 / 6.0);
616  Qd(idxstartcorr_q + 1, idxstartcorr_v + 0) = -t371 * (t452 + t623)
617  - t374
618  * (dt * t343 * (t566 - ew3 * t425) * (1.0 / 3.0)
619  - ew3 * t343 * t362 * (1.0 / 2.0))
620  + t373 * (-t451 + t452 + dt * t343 * (t436 + t582 - t583) * (1.0 / 3.0));
621  Qd(idxstartcorr_q + 1, idxstartcorr_v + 1) = t599;
622  Qd(idxstartcorr_q + 1, idxstartcorr_v + 2) = t608
623  + t373 * (t602 + t610 - dt * t343 * (t490 + t601 - t609) * (1.0 / 3.0))
624  - t371 * (t602 - dt * t343 * (t600 - t601) * (1.0 / 3.0));
625  Qd(idxstartcorr_q + 1, idxstartcorr_q + 0) = -t374
626  * (ew3 * t343 * (1.0 / 2.0) - dt * ew1 * ew2 * t343 * (1.0 / 6.0))
627  - dt * t343 * t373 * t613 * (1.0 / 3.0);
628  Qd(idxstartcorr_q + 1, idxstartcorr_q + 1) = t373
629  * (dt - dt * t343 * t435 * (1.0 / 3.0)) + dt * t343 * t563 * (1.0 / 3.0)
630  + dt * t343 * t371 * t429 * (1.0 / 3.0)
631  + dt * t343 * t374 * t431 * (1.0 / 3.0);
632  Qd(idxstartcorr_q + 1, idxstartcorr_q + 2) = t655
633  - t373 * (t614 - dt * ew2 * ew3 * t343 * (1.0 / 6.0))
634  - dt * ew2 * ew3 * t343 * t374 * (1.0 / 3.0);
635  Qd(idxstartcorr_q + 1, idxstartcorr_b_w + 0) = dt * ew3 * t343 * t502
636  * (1.0 / 6.0);
637  Qd(idxstartcorr_q + 1, idxstartcorr_b_w + 1) = t343 * t563 * (-1.0 / 2.0);
638  Qd(idxstartcorr_q + 1, idxstartcorr_b_w + 2) = dt * ew1 * t343 * t503
639  * (-1.0 / 6.0);
640  Qd(idxstartcorr_q + 2, idxstartcorr_p + 0) = t615;
641  Qd(idxstartcorr_q + 2, idxstartcorr_p + 1) = t616;
642  Qd(idxstartcorr_q + 2, idxstartcorr_p + 2) = t617;
643  Qd(idxstartcorr_q + 2, idxstartcorr_v + 0) = -t374 * t622
644  - t371 * (t451 + t452 - dt * t343 * (t442 + t582 - t583) * (1.0 / 3.0))
645  + t373 * (t452 - t623);
646  Qd(idxstartcorr_q + 2, idxstartcorr_v + 1) = -t374 * t631 - t371 * t635
647  - t373 * (t626 - t627);
648  Qd(idxstartcorr_q + 2, idxstartcorr_v + 2) = t647 - t374 * t641
649  - t373 * (t562 + dt * t343 * (t642 - t643) * (1.0 / 3.0));
650  Qd(idxstartcorr_q + 2, idxstartcorr_q + 0) = t652;
651  Qd(idxstartcorr_q + 2, idxstartcorr_q + 1) = t655 - t373 * (t614 - t653)
652  - dt * ew2 * ew3 * t343 * t374 * (1.0 / 3.0);
653  Qd(idxstartcorr_q + 2, idxstartcorr_q + 2) = t371
654  * (dt - dt * t343 * t441 * (1.0 / 3.0)) + dt * t343 * t503 * (1.0 / 3.0)
655  + dt * t343 * t373 * t429 * (1.0 / 3.0)
656  + dt * t343 * t374 * t430 * (1.0 / 3.0);
657  Qd(idxstartcorr_q + 2, idxstartcorr_b_w + 0) = dt * ew2 * t343 * t502
658  * (-1.0 / 6.0);
659  Qd(idxstartcorr_q + 2, idxstartcorr_b_w + 1) = t657;
660  Qd(idxstartcorr_q + 2, idxstartcorr_b_w + 2) = t343 * t503 * (-1.0 / 2.0);
661  Qd(idxstartcorr_b_w + 0, idxstartcorr_v + 0) = dt * t343 * t362 * t502
662  * (-1.0 / 6.0);
663  Qd(idxstartcorr_b_w + 0, idxstartcorr_v + 2) = dt * t343 * t390 * t502
664  * (-1.0 / 6.0);
665  Qd(idxstartcorr_b_w + 0, idxstartcorr_q + 0) = t343 * t502 * (-1.0 / 2.0);
666  Qd(idxstartcorr_b_w + 0, idxstartcorr_q + 2) = dt * ew2 * t343 * t502
667  * (-1.0 / 6.0);
668  Qd(idxstartcorr_b_w + 0, idxstartcorr_b_w + 0) = dt * t502;
669  Qd(idxstartcorr_b_w + 1, idxstartcorr_v + 0) = dt * t343 * t362 * t563
670  * (-1.0 / 6.0);
671  Qd(idxstartcorr_b_w + 1, idxstartcorr_v + 1) = dt * t343 * t388 * t563
672  * (-1.0 / 6.0);
673  Qd(idxstartcorr_b_w + 1, idxstartcorr_v + 2) = t656;
674  Qd(idxstartcorr_b_w + 1, idxstartcorr_q + 1) = t343 * t563 * (-1.0 / 2.0);
675  Qd(idxstartcorr_b_w + 1, idxstartcorr_q + 2) = t657;
676  Qd(idxstartcorr_b_w + 1, idxstartcorr_b_w + 1) = dt * t563;
677  Qd(idxstartcorr_b_w + 2, idxstartcorr_v + 0) = dt * t343 * t362 * t503
678  * (-1.0 / 6.0);
679  Qd(idxstartcorr_b_w + 2, idxstartcorr_v + 1) = dt * t343 * t381 * t503
680  * (-1.0 / 6.0);
681  Qd(idxstartcorr_b_w + 2, idxstartcorr_v + 2) = dt * t343 * t394 * t503
682  * (-1.0 / 6.0);
683  Qd(idxstartcorr_b_w + 2, idxstartcorr_q + 1) = dt * ew1 * t343 * t503
684  * (-1.0 / 6.0);
685  Qd(idxstartcorr_b_w + 2, idxstartcorr_q + 2) = t343 * t503 * (-1.0 / 2.0);
686  Qd(idxstartcorr_b_w + 2, idxstartcorr_b_w + 2) = dt * t503;
687  Qd(idxstartcorr_b_a + 0, idxstartcorr_p + 0) = dt * t343 * t346 * t398
688  * (-1.0 / 6.0);
689  Qd(idxstartcorr_b_a + 0, idxstartcorr_p + 1) = dt * t343 * t366 * t398
690  * (-1.0 / 6.0);
691  Qd(idxstartcorr_b_a + 0, idxstartcorr_p + 2) = t658;
692  Qd(idxstartcorr_b_a + 0, idxstartcorr_v + 0) = t343 * t346 * t398
693  * (-1.0 / 2.0);
694  Qd(idxstartcorr_b_a + 0, idxstartcorr_v + 1) = t343 * t366 * t398
695  * (-1.0 / 2.0);
696  Qd(idxstartcorr_b_a + 0, idxstartcorr_v + 2) = t659;
697  Qd(idxstartcorr_b_a + 0, idxstartcorr_b_a + 0) = dt * t398;
698  Qd(idxstartcorr_b_a + 1, idxstartcorr_p + 0) = t660;
699  Qd(idxstartcorr_b_a + 1, idxstartcorr_p + 1) = dt * t343 * t367 * t399
700  * (-1.0 / 6.0);
701  Qd(idxstartcorr_b_a + 1, idxstartcorr_p + 2) = dt * t343 * t372 * t399
702  * (-1.0 / 6.0);
703  Qd(idxstartcorr_b_a + 1, idxstartcorr_v + 0) = t661;
704  Qd(idxstartcorr_b_a + 1, idxstartcorr_v + 1) = t343 * t367 * t399
705  * (-1.0 / 2.0);
706  Qd(idxstartcorr_b_a + 1, idxstartcorr_v + 2) = t343 * t372 * t399
707  * (-1.0 / 2.0);
708  Qd(idxstartcorr_b_a + 1, idxstartcorr_b_a + 1) = dt * t399;
709  Qd(idxstartcorr_b_a + 2, idxstartcorr_p + 0) = dt * t343 * t345 * t400
710  * (-1.0 / 6.0);
711  Qd(idxstartcorr_b_a + 2, idxstartcorr_p + 1) = t662;
712  Qd(idxstartcorr_b_a + 2, idxstartcorr_p + 2) = dt * t343 * t370 * t400
713  * (-1.0 / 6.0);
714  Qd(idxstartcorr_b_a + 2, idxstartcorr_v + 0) = t343 * t345 * t400
715  * (-1.0 / 2.0);
716  Qd(idxstartcorr_b_a + 2, idxstartcorr_v + 1) = t663;
717  Qd(idxstartcorr_b_a + 2, idxstartcorr_v + 2) = t343 * t370 * t400
718  * (-1.0 / 2.0);
719  Qd(idxstartcorr_b_a + 2, idxstartcorr_b_a + 2) = dt * t400;
720 
721 }
722 ;
723 
724 #endif /* CALCQ_H_ */
Generated on Wed Nov 6 2013 22:21:53 for ethzasl-msf - Modular Sensor Fusion by   doxygen 1.8.1.2