calcQ.h
Go to the documentation of this file.
00001 /*
00002 
00003 Copyright (c) 2010, Stephan Weiss, ASL, ETH Zurich, Switzerland
00004 You can contact the author at <stephan dot weiss at ieee dot org>
00005 
00006 All rights reserved.
00007 
00008 Redistribution and use in source and binary forms, with or without
00009 modification, are permitted provided that the following conditions are met:
00010 * Redistributions of source code must retain the above copyright
00011 notice, this list of conditions and the following disclaimer.
00012 * Redistributions in binary form must reproduce the above copyright
00013 notice, this list of conditions and the following disclaimer in the
00014 documentation and/or other materials provided with the distribution.
00015 * Neither the name of ETHZ-ASL nor the
00016 names of its contributors may be used to endorse or promote products
00017 derived from this software without specific prior written permission.
00018 
00019 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00020 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00021 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00022 DISCLAIMED. IN NO EVENT SHALL ETHZ-ASL BE LIABLE FOR ANY
00023 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00024 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00025 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00026 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00027 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00028 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00029 
00030 */
00031 
00032 #ifndef CALCQ_H_
00033 #define CALCQ_H_
00034 
00035 
00036 #include <Eigen/Eigen>
00037 
00038 
00039 
00040 template <class Derived, class DerivedQ> void calc_Q(
00041             double dt,
00042             const Eigen::Quaternion<double> & q,
00043             const Eigen::MatrixBase<Derived> & ew,
00044             const Eigen::MatrixBase<Derived> & ea,
00045             const Eigen::MatrixBase<Derived> & n_a,
00046             const Eigen::MatrixBase<Derived> & n_ba,
00047             const Eigen::MatrixBase<Derived> & n_w,
00048             const Eigen::MatrixBase<Derived> & n_bw,
00049             double n_L,
00050             const Eigen::MatrixBase<Derived> & n_qvw,
00051             const Eigen::MatrixBase<Derived> & n_qci,
00052             const Eigen::MatrixBase<Derived> & n_pic,
00053             Eigen::MatrixBase<DerivedQ> &  Qd)
00054 {
00055 
00056         double q1=q.w(), q2=q.x(), q3=q.y(), q4=q.z();
00057         double ew1=ew(0), ew2=ew(1), ew3=ew(2);
00058         double ea1=ea(0), ea2=ea(1), ea3=ea(2);
00059         double n_a1=n_a(0), n_a2=n_a(1), n_a3=n_a(2);
00060         double n_ba1=n_ba(0), n_ba2=n_ba(1), n_ba3=n_ba(2);
00061         double n_w1=n_w(0), n_w2=n_w(1), n_w3=n_w(2);
00062         double n_bw1=n_bw(0), n_bw2=n_bw(1), n_bw3=n_bw(2);
00063         double n_qvw1=n_qvw(0), n_qvw2=n_qvw(1), n_qvw3=n_qvw(2);
00064 
00065         double t343 = dt*dt;
00066         double t348 = q1*q4*2.0;
00067         double t349 = q2*q3*2.0;
00068         double t344 = t348-t349;
00069         double t356 = q1*q3*2.0;
00070         double t357 = q2*q4*2.0;
00071         double t345 = t356+t357;
00072         double t350 = q1*q1;
00073         double t351 = q2*q2;
00074         double t352 = q3*q3;
00075         double t353 = q4*q4;
00076         double t346 = t350+t351-t352-t353;
00077         double t347 = n_a1*n_a1;
00078         double t354 = n_a2*n_a2;
00079         double t355 = n_a3*n_a3;
00080         double t358 = q1*q2*2.0;
00081         double t359 = t344*t344;
00082         double t360 = t345*t345;
00083         double t361 = t346*t346;
00084         double t363 = ea2*t345;
00085         double t364 = ea3*t344;
00086         double t362 = t363+t364;
00087         double t365 = t362*t362;
00088         double t366 = t348+t349;
00089         double t367 = t350-t351+t352-t353;
00090         double t368 = q3*q4*2.0;
00091         double t369 = t356-t357;
00092         double t370 = t350-t351-t352+t353;
00093         double t371 = n_w3*n_w3;
00094         double t372 = t358+t368;
00095         double t373 = n_w2*n_w2;
00096         double t374 = n_w1*n_w1;
00097         double t375 = dt*t343*t346*t347*t366*(1.0/3.0);
00098         double t376 = t358-t368;
00099         double t377 = t343*t346*t347*t366*(1.0/2.0);
00100         double t378 = t366*t366;
00101         double t379 = t376*t376;
00102         double t380 = ea1*t367;
00103         double t391 = ea2*t366;
00104         double t381 = t380-t391;
00105         double t382 = ea3*t367;
00106         double t383 = ea2*t376;
00107         double t384 = t382+t383;
00108         double t385 = t367*t367;
00109         double t386 = ea1*t376;
00110         double t387 = ea3*t366;
00111         double t388 = t386+t387;
00112         double t389 = ea2*t370;
00113         double t407 = ea3*t372;
00114         double t390 = t389-t407;
00115         double t392 = ea1*t372;
00116         double t393 = ea2*t369;
00117         double t394 = t392+t393;
00118         double t395 = ea1*t370;
00119         double t396 = ea3*t369;
00120         double t397 = t395+t396;
00121         double t398 = n_ba1*n_ba1;
00122         double t399 = n_ba2*n_ba2;
00123         double t400 = n_ba3*n_ba3;
00124         double t401 = dt*t343*t345*t355*t370*(1.0/3.0);
00125         double t402 = t401-dt*t343*t346*t347*t369*(1.0/3.0)-dt*t343*t344*t354*t372*(1.0/3.0);
00126         double t403 = dt*t343*t354*t367*t372*(1.0/3.0);
00127         double t404 = t403-dt*t343*t347*t366*t369*(1.0/3.0)-dt*t343*t355*t370*t376*(1.0/3.0);
00128         double t405 = t343*t345*t355*t370*(1.0/2.0);
00129         double t406 = dt*t343*t362*t373*t397*(1.0/6.0);
00130         double t421 = t343*t346*t347*t369*(1.0/2.0);
00131         double t422 = dt*t343*t362*t371*t394*(1.0/6.0);
00132         double t423 = t343*t344*t354*t372*(1.0/2.0);
00133         double t424 = dt*t343*t362*t374*t390*(1.0/6.0);
00134         double t408 = t405+t406-t421-t422-t423-t424;
00135         double t409 = t343*t354*t367*t372*(1.0/2.0);
00136         double t410 = dt*t343*t374*t384*t390*(1.0/6.0);
00137         double t411 = dt*t343*t373*t388*t397*(1.0/6.0);
00138         double t463 = t343*t355*t370*t376*(1.0/2.0);
00139         double t464 = t343*t347*t366*t369*(1.0/2.0);
00140         double t465 = dt*t343*t371*t381*t394*(1.0/6.0);
00141         double t412 = t409+t410+t411-t463-t464-t465;
00142         double t413 = t369*t369;
00143         double t414 = t372*t372;
00144         double t415 = t370*t370;
00145         double t416 = t343*t354*t359*(1.0/2.0);
00146         double t417 = t343*t355*t360*(1.0/2.0);
00147         double t418 = t343*t347*t361*(1.0/2.0);
00148         double t419 = t416+t417+t418-dt*t343*t365*t371*(1.0/6.0)-dt*t343*t365*t373*(1.0/6.0)-dt*t343*t365*t374*(1.0/6.0);
00149         double t453 = t343*t344*t354*t367*(1.0/2.0);
00150         double t454 = t343*t345*t355*t376*(1.0/2.0);
00151         double t420 = t377-t453-t454;
00152         double t426 = ew2*t362;
00153         double t427 = ew3*t362;
00154         double t425 = t426-t427;
00155         double t428 = dt*t365;
00156         double t429 = ew1*ew1;
00157         double t430 = ew2*ew2;
00158         double t431 = ew3*ew3;
00159         double t432 = t430+t431;
00160         double t433 = t362*t432;
00161         double t434 = ew1*t343*t365;
00162         double t435 = t429+t431;
00163         double t436 = t362*t435;
00164         double t443 = ew2*ew3*t362;
00165         double t437 = t433+t436-t443;
00166         double t438 = ew1*t362*t394;
00167         double t511 = ew1*t362*t397;
00168         double t439 = t438-t511;
00169         double t440 = t343*t439*(1.0/2.0);
00170         double t441 = t429+t430;
00171         double t442 = t362*t441;
00172         double t444 = t390*t432;
00173         double t445 = ew2*t394;
00174         double t446 = ew3*t397;
00175         double t447 = t445+t446;
00176         double t448 = ew1*ew2*t362;
00177         double t449 = ew1*ew3*t362;
00178         double t450 = ew1*ew3*t362*(1.0/2.0);
00179         double t451 = dt*t362;
00180         double t452 = ew1*t343*t362*(1.0/2.0);
00181         double t455 = dt*t343*t362*t374*t384*(1.0/6.0);
00182         double t456 = t343*t347*t378*(1.0/2.0);
00183         double t457 = t343*t355*t379*(1.0/2.0);
00184         double t458 = t381*t381;
00185         double t459 = t384*t384;
00186         double t460 = t343*t354*t385*(1.0/2.0);
00187         double t461 = t388*t388;
00188         double t462 = t456+t457+t460-dt*t343*t371*t458*(1.0/6.0)-dt*t343*t374*t459*(1.0/6.0)-dt*t343*t373*t461*(1.0/6.0);
00189         double t466 = t433+t442-t443;
00190         double t467 = ew1*t362*t388;
00191         double t468 = ew1*t362*t381;
00192         double t469 = t467+t468;
00193         double t470 = t343*t469*(1.0/2.0);
00194         double t471 = t384*t432;
00195         double t472 = ew2*t381;
00196         double t479 = ew3*t388;
00197         double t473 = t472-t479;
00198         double t474 = -t433+t448+t449;
00199         double t475 = dt*t343*t346*t366*t398*(1.0/3.0);
00200         double t476 = dt*t346*t347*t366;
00201         double t477 = ew2*ew3*t381;
00202         double t492 = t388*t435;
00203         double t478 = t471+t477-t492;
00204         double t480 = t472-t479;
00205         double t481 = ew1*ew3*t381;
00206         double t482 = ew1*ew2*t388;
00207         double t483 = t471+t481+t482;
00208         double t484 = ew2*ew3*t388;
00209         double t486 = t381*t441;
00210         double t485 = t471+t484-t486;
00211         double t487 = t394*t441;
00212         double t488 = ew2*ew3*t397;
00213         double t489 = t444+t487+t488;
00214         double t490 = t397*t435;
00215         double t491 = ew2*ew3*t394;
00216         double t493 = ew1*t381*t397;
00217         double t541 = ew1*t388*t394;
00218         double t494 = t493-t541;
00219         double t495 = t343*t494*(1.0/2.0);
00220         double t496 = ew1*ew2*t397;
00221         double t527 = ew1*ew3*t394;
00222         double t497 = t444+t496-t527;
00223         double t498 = ew2*ew3*t381*(1.0/2.0);
00224         double t499 = ew1*t343*t381*(1.0/2.0);
00225         double t500 = t384*t432*(1.0/2.0);
00226         double t501 = ew2*ew3*t388*(1.0/2.0);
00227         double t502 = n_bw1*n_bw1;
00228         double t503 = n_bw3*n_bw3;
00229         double t504 = t343*t347*t413*(1.0/2.0);
00230         double t505 = t343*t354*t414*(1.0/2.0);
00231         double t506 = t397*t397;
00232         double t507 = t390*t390;
00233         double t508 = t343*t355*t415*(1.0/2.0);
00234         double t509 = t394*t394;
00235         double t510 = t504+t505+t508-dt*t343*t373*t506*(1.0/6.0)-dt*t343*t371*t509*(1.0/6.0)-dt*t343*t374*t507*(1.0/6.0);
00236         double t512 = -t444+t490+t491;
00237         double t513 = t397*t437*(1.0/2.0);
00238         double t514 = t362*t394*t429;
00239         double t515 = dt*t362*t397;
00240         double t516 = t362*t489*(1.0/2.0);
00241         double t517 = t394*t466*(1.0/2.0);
00242         double t518 = t362*t397*t429;
00243         double t519 = t516+t517+t518;
00244         double t520 = dt*t362*t394;
00245         double t521 = t440+t520-dt*t343*t519*(1.0/3.0);
00246         double t522 = t371*t521;
00247         double t523 = t362*t447;
00248         double t524 = t390*t425;
00249         double t525 = t523+t524;
00250         double t526 = t343*t525*(1.0/2.0);
00251         double t528 = t425*t447;
00252         double t529 = t390*t474*(1.0/2.0);
00253         double t530 = t528+t529-t362*t497*(1.0/2.0);
00254         double t531 = dt*t343*t530*(1.0/3.0);
00255         double t532 = dt*t362*t390;
00256         double t533 = t526+t531+t532;
00257         double t534 = t374*t533;
00258         double t535 = dt*t343*t345*t370*t400*(1.0/3.0);
00259         double t536 = dt*t345*t355*t370;
00260         double t537 = t381*t489*(1.0/2.0);
00261         double t538 = t388*t397*t429;
00262         double t539 = t537+t538-t394*t485*(1.0/2.0);
00263         double t540 = dt*t343*t539*(1.0/3.0);
00264         double t542 = t495+t540-dt*t381*t394;
00265         double t543 = t388*t512*(1.0/2.0);
00266         double t544 = t381*t394*t429;
00267         double t545 = t543+t544-t397*t478*(1.0/2.0);
00268         double t546 = dt*t343*t545*(1.0/3.0);
00269         double t547 = t495+t546-dt*t388*t397;
00270         double t548 = t373*t547;
00271         double t549 = t384*t447;
00272         double t550 = t549-t390*t473;
00273         double t551 = t343*t550*(1.0/2.0);
00274         double t552 = t384*t497*(1.0/2.0);
00275         double t553 = t390*t483*(1.0/2.0);
00276         double t554 = t447*t473;
00277         double t555 = t552+t553+t554;
00278         double t556 = dt*t384*t390;
00279         double t557 = t551+t556-dt*t343*t555*(1.0/3.0);
00280         double t558 = dt*t343*t367*t372*t399*(1.0/3.0);
00281         double t559 = dt*t354*t367*t372;
00282         double t560 = t548+t558+t559-t371*t542-t374*t557-dt*t347*t366*t369-dt*t355*t370*t376-dt*t343*t366*t369*t398*(1.0/3.0)-dt*t343*t370*t376*t400*(1.0/3.0);
00283         double t561 = ew1*t343*t394*t397;
00284         double t562 = ew1*t343*t397*(1.0/2.0);
00285         double t563 = n_bw2*n_bw2;
00286         double t564 = dt*t343*t362*t374*(1.0/6.0);
00287         double t565 = dt*t343*t374*t390*(1.0/6.0);
00288         double t566 = ew1*ew2*t362*(1.0/2.0);
00289         double t567 = -t433+t450+t566;
00290         double t568 = dt*t343*t567*(1.0/3.0);
00291         double t569 = t343*t425*(1.0/2.0);
00292         double t570 = t451+t568+t569;
00293         double t571 = dt*t343*t362*t373*t432*(1.0/6.0);
00294         double t572 = dt*t343*t362*t371*t432*(1.0/6.0);
00295         double t573 = t571+t572-t374*t570;
00296         double t574 = ew1*ew2*t397*(1.0/2.0);
00297         double t575 = t444+t574-ew1*ew3*t394*(1.0/2.0);
00298         double t576 = t343*t447*(1.0/2.0);
00299         double t577 = dt*t390;
00300         double t578 = t576+t577-dt*t343*t575*(1.0/3.0);
00301         double t579 = dt*t343*t371*t394*t432*(1.0/6.0);
00302         double t580 = t579-t374*t578-dt*t343*t373*t397*t432*(1.0/6.0);
00303         double t581 = dt*t343*t373*t388*(1.0/6.0);
00304         double t582 = t362*t432*(1.0/2.0);
00305         double t583 = ew2*ew3*t362*(1.0/2.0);
00306         double t584 = t362*t429;
00307         double t585 = t583+t584;
00308         double t586 = ew3*t473;
00309         double t587 = ew1*ew2*t384*(1.0/2.0);
00310         double t588 = t586+t587;
00311         double t589 = dt*t343*t588*(1.0/3.0);
00312         double t590 = t374*(t589-ew3*t343*t384*(1.0/2.0));
00313         double t591 = t388*t429;
00314         double t592 = t498+t591;
00315         double t593 = dt*t343*t592*(1.0/3.0);
00316         double t594 = t499+t593;
00317         double t595 = -t492+t498+t500;
00318         double t596 = dt*t343*t595*(1.0/3.0);
00319         double t597 = dt*t388;
00320         double t598 = -t499+t596+t597;
00321         double t599 = t590-t371*t594-t373*t598;
00322         double t600 = t397*t429;
00323         double t601 = ew2*ew3*t394*(1.0/2.0);
00324         double t602 = ew1*t343*t394*(1.0/2.0);
00325         double t603 = ew3*t447;
00326         double t604 = t603-ew1*ew2*t390*(1.0/2.0);
00327         double t605 = dt*t343*t604*(1.0/3.0);
00328         double t606 = ew3*t343*t390*(1.0/2.0);
00329         double t607 = t605+t606;
00330         double t608 = t374*t607;
00331         double t609 = t390*t432*(1.0/2.0);
00332         double t610 = dt*t397;
00333         double t611 = t430*(1.0/2.0);
00334         double t612 = t431*(1.0/2.0);
00335         double t613 = t611+t612;
00336         double t614 = ew1*t343*(1.0/2.0);
00337         double t615 = dt*t343*t362*t371*(1.0/6.0);
00338         double t616 = dt*t343*t371*t381*(1.0/6.0);
00339         double t617 = dt*t343*t371*t394*(1.0/6.0);
00340         double t618 = ew2*t425;
00341         double t619 = t450+t618;
00342         double t620 = dt*t343*t619*(1.0/3.0);
00343         double t621 = ew2*t343*t362*(1.0/2.0);
00344         double t622 = t620+t621;
00345         double t623 = dt*t343*t585*(1.0/3.0);
00346         double t624 = t381*t429;
00347         double t625 = t501+t624;
00348         double t626 = dt*t343*t625*(1.0/3.0);
00349         double t627 = ew1*t343*t388*(1.0/2.0);
00350         double t628 = ew2*t473;
00351         double t629 = t628-ew1*ew3*t384*(1.0/2.0);
00352         double t630 = dt*t343*t629*(1.0/3.0);
00353         double t631 = t630-ew2*t343*t384*(1.0/2.0);
00354         double t632 = -t486+t500+t501;
00355         double t633 = dt*t343*t632*(1.0/3.0);
00356         double t634 = dt*t381;
00357         double t635 = t627+t633+t634;
00358         double t636 = ew2*t447;
00359         double t637 = ew1*ew3*t390*(1.0/2.0);
00360         double t638 = t636+t637;
00361         double t639 = dt*t343*t638*(1.0/3.0);
00362         double t640 = ew2*t343*t390*(1.0/2.0);
00363         double t641 = t639+t640;
00364         double t642 = t394*t429;
00365         double t643 = ew2*ew3*t397*(1.0/2.0);
00366         double t644 = t487+t609+t643;
00367         double t645 = dt*t343*t644*(1.0/3.0);
00368         double t646 = t562+t645-dt*t394;
00369         double t647 = t371*t646;
00370         double t648 = ew2*t343*(1.0/2.0);
00371         double t649 = dt*ew1*ew3*t343*(1.0/6.0);
00372         double t650 = t648+t649;
00373         double t651 = t374*t650;
00374         double t652 = t651-dt*t343*t371*t613*(1.0/3.0);
00375         double t653 = dt*ew2*ew3*t343*(1.0/6.0);
00376         double t654 = t614+t653;
00377         double t655 = t371*t654;
00378         double t656 = dt*t343*t397*t563*(1.0/6.0);
00379         double t657 = dt*ew1*t343*t563*(1.0/6.0);
00380         double t658 = dt*t343*t369*t398*(1.0/6.0);
00381         double t659 = t343*t369*t398*(1.0/2.0);
00382         double t660 = dt*t343*t344*t399*(1.0/6.0);
00383         double t661 = t343*t344*t399*(1.0/2.0);
00384         double t662 = dt*t343*t376*t400*(1.0/6.0);
00385         double t663 = t343*t376*t400*(1.0/2.0);
00386         Qd(0,0) = dt*t343*t347*t361*(1.0/3.0)+dt*t343*t354*t359*(1.0/3.0)+dt*t343*t355*t360*(1.0/3.0);
00387         Qd(0,1) = t375-dt*t343*t345*t355*(t358-q3*q4*2.0)*(1.0/3.0)-dt*t343*t344*t354*t367*(1.0/3.0);
00388         Qd(0,2) = t402;
00389         Qd(0,3) = t419;
00390         Qd(0,4) = t420;
00391         Qd(0,5) = t408;
00392         Qd(0,6) = t564;
00393         Qd(0,8) = t615;
00394         Qd(0,12) = dt*t343*t346*t398*(-1.0/6.0);
00395         Qd(0,13) = t660;
00396         Qd(0,14) = dt*t343*t345*t400*(-1.0/6.0);
00397         Qd(1,0) = t375-dt*t343*t344*t354*t367*(1.0/3.0)-dt*t343*t345*t355*t376*(1.0/3.0);
00398         Qd(1,1) = dt*t343*t347*t378*(1.0/3.0)+dt*t343*t355*t379*(1.0/3.0)+dt*t343*t354*t385*(1.0/3.0);
00399         Qd(1,2) = t404;
00400         Qd(1,3) = t377+t455-t343*t344*t354*t367*(1.0/2.0)-t343*t345*t355*t376*(1.0/2.0)-dt*t343*t362*t371*t381*(1.0/6.0)-dt*t343*t362*t373*t388*(1.0/6.0);
00401         Qd(1,4) = t462;
00402         Qd(1,5) = t412;
00403         Qd(1,6) = dt*t343*t374*t384*(-1.0/6.0);
00404         Qd(1,7) = t581;
00405         Qd(1,8) = t616;
00406         Qd(1,12) = dt*t343*t366*t398*(-1.0/6.0);
00407         Qd(1,13) = dt*t343*t367*t399*(-1.0/6.0);
00408         Qd(1,14) = t662;
00409         Qd(2,0) = t402;
00410         Qd(2,1) = t404;
00411         Qd(2,2) = dt*t343*t347*t413*(1.0/3.0)+dt*t343*t354*t414*(1.0/3.0)+dt*t343*t355*t415*(1.0/3.0);
00412         Qd(2,3) = t408;
00413         Qd(2,4) = t412;
00414         Qd(2,5) = t510;
00415         Qd(2,6) = t565;
00416         Qd(2,7) = dt*t343*t373*t397*(-1.0/6.0);
00417         Qd(2,8) = t617;
00418         Qd(2,12) = t658;
00419         Qd(2,13) = dt*t343*t372*t399*(-1.0/6.0);
00420         Qd(2,14) = dt*t343*t370*t400*(-1.0/6.0);
00421         Qd(3,0) = t419;
00422         Qd(3,1) = t420;
00423         Qd(3,2) = t408;
00424         Qd(3,3) = t374*(t428+t343*t362*t425+dt*t343*(t362*(t448+t449-t362*t432)+t425*t425)*(1.0/3.0))+t373*(t428-t434+dt*t343*(t365*t429-t362*t437)*(1.0/3.0))+t371*(t428+t434+dt*t343*(t365*t429-t362*(t433+t442-ew2*ew3*t362))*(1.0/3.0))+dt*t347*t361+dt*t354*t359+dt*t355*t360+dt*t343*t359*t399*(1.0/3.0)+dt*t343*t361*t398*(1.0/3.0)+dt*t343*t360*t400*(1.0/3.0);
00425         Qd(3,4) = t475+t476-dt*t344*t354*t367-dt*t345*t355*t376-dt*t343*t344*t367*t399*(1.0/3.0)-dt*t343*t345*t376*t400*(1.0/3.0);
00426         Qd(3,5) = t522+t534+t535+t536-t373*(t440+t515-dt*t343*(t513+t514+t362*(t490+t491-t390*t432)*(1.0/2.0))*(1.0/3.0))-dt*t346*t347*t369-dt*t344*t354*t372-dt*t343*t346*t369*t398*(1.0/3.0)-dt*t343*t344*t372*t399*(1.0/3.0);
00427         Qd(3,6) = t573;
00428         Qd(3,8) = -t371*(t451+t452-dt*t343*(t442+t582-ew2*ew3*t362*(1.0/2.0))*(1.0/3.0))-t374*t622+t373*(t452-dt*t343*t585*(1.0/3.0));
00429         Qd(3,9) = dt*t343*t362*t502*(-1.0/6.0);
00430         Qd(3,11) = dt*t343*t362*t503*(-1.0/6.0);
00431         Qd(3,12) = t343*t346*t398*(-1.0/2.0);
00432         Qd(3,13) = t661;
00433         Qd(3,14) = t343*t345*t400*(-1.0/2.0);
00434         Qd(4,0) = t377-t453-t454+t455-dt*t343*t362*t371*t381*(1.0/6.0)-dt*t343*t362*t373*t388*(1.0/6.0);
00435         Qd(4,1) = t462;
00436         Qd(4,2) = t412;
00437         Qd(4,3) = t475+t476-t374*(t343*(t384*t425-t362*t473)*(1.0/2.0)-dt*t343*(t362*t483*(1.0/2.0)-t384*t474*(1.0/2.0)+t425*t473)*(1.0/3.0)+dt*t362*t384)+t371*(t470+dt*t362*t381+dt*t343*(t362*t485*(1.0/2.0)-t381*t466*(1.0/2.0)+t362*t388*t429)*(1.0/3.0))+t373*(-t470+dt*t362*t388+dt*t343*(t388*t437*(-1.0/2.0)+t362*t478*(1.0/2.0)+t362*t381*t429)*(1.0/3.0))-dt*t344*t354*t367-dt*t345*t355*t376-dt*t343*t344*t367*t399*(1.0/3.0)-dt*t343*t345*t376*t400*(1.0/3.0);
00438         Qd(4,4) = -t374*(-dt*t459+dt*t343*(t384*t483-t480*t480)*(1.0/3.0)+t343*t384*t473)+t373*(dt*t461+dt*t343*(t388*t478+t429*t458)*(1.0/3.0)-ew1*t343*t381*t388)+t371*(dt*t458+dt*t343*(t381*t485+t429*t461)*(1.0/3.0)+ew1*t343*t381*t388)+dt*t347*t378+dt*t355*t379+dt*t354*t385+dt*t343*t378*t398*(1.0/3.0)+dt*t343*t385*t399*(1.0/3.0)+dt*t343*t379*t400*(1.0/3.0);
00439         Qd(4,5) = t560;
00440         Qd(4,6) = -t374*(-dt*t384+t343*t473*(1.0/2.0)+dt*t343*(t471+ew1*ew2*t388*(1.0/2.0)+ew1*ew3*t381*(1.0/2.0))*(1.0/3.0))+dt*t343*t371*t381*t432*(1.0/6.0)+dt*t343*t373*t388*t432*(1.0/6.0);
00441         Qd(4,7) = t599;
00442         Qd(4,8) = -t374*t631-t371*t635-t373*(t626-ew1*t343*t388*(1.0/2.0));
00443         Qd(4,9) = dt*t343*t384*t502*(1.0/6.0);
00444         Qd(4,10) = dt*t343*t388*t563*(-1.0/6.0);
00445         Qd(4,11) = dt*t343*t381*t503*(-1.0/6.0);
00446         Qd(4,12) = t343*t366*t398*(-1.0/2.0);
00447         Qd(4,13) = t343*t367*t399*(-1.0/2.0);
00448         Qd(4,14) = t663;
00449         Qd(5,0) = t408;
00450         Qd(5,1) = t412;
00451         Qd(5,2) = t510;
00452         Qd(5,3) = t522+t534+t535+t536-t373*(t440+t515-dt*t343*(t513+t514+t362*t512*(1.0/2.0))*(1.0/3.0))-dt*t346*t347*t369-dt*t344*t354*t372-dt*t343*t346*t369*t398*(1.0/3.0)-dt*t343*t344*t372*t399*(1.0/3.0);
00453         Qd(5,4) = t560;
00454         Qd(5,5) = -t371*(t561-dt*t509+dt*t343*(t394*t489-t429*t506)*(1.0/3.0))+t373*(t561+dt*t506-dt*t343*(t397*t512-t429*t509)*(1.0/3.0))+t374*(dt*t507-dt*t343*(t390*t497-t447*t447)*(1.0/3.0)+t343*t390*t447)+dt*t347*t413+dt*t354*t414+dt*t355*t415+dt*t343*t398*t413*(1.0/3.0)+dt*t343*t399*t414*(1.0/3.0)+dt*t343*t400*t415*(1.0/3.0);
00455         Qd(5,6) = t580;
00456         Qd(5,7) = t608+t371*(dt*t343*(t600-ew2*ew3*t394*(1.0/2.0))*(1.0/3.0)-ew1*t343*t394*(1.0/2.0))+t373*(t602+t610-dt*t343*(t490+t601-t390*t432*(1.0/2.0))*(1.0/3.0));
00457         Qd(5,8) = t647-t374*t641-t373*(t562+dt*t343*(t642-ew2*ew3*t397*(1.0/2.0))*(1.0/3.0));
00458         Qd(5,9) = dt*t343*t390*t502*(-1.0/6.0);
00459         Qd(5,10) = t656;
00460         Qd(5,11) = dt*t343*t394*t503*(-1.0/6.0);
00461         Qd(5,12) = t659;
00462         Qd(5,13) = t343*t372*t399*(-1.0/2.0);
00463         Qd(5,14) = t343*t370*t400*(-1.0/2.0);
00464         Qd(6,0) = t564;
00465         Qd(6,2) = t565;
00466         Qd(6,3) = t573;
00467         Qd(6,5) = t580;
00468         Qd(6,6) = t374*(dt-dt*t343*t432*(1.0/3.0))+dt*t343*t502*(1.0/3.0);
00469         Qd(6,8) = t652;
00470         Qd(6,9) = t343*t502*(-1.0/2.0);
00471         Qd(7,0) = dt*t343*t362*t373*(1.0/6.0);
00472         Qd(7,1) = t581;
00473         Qd(7,2) = dt*t343*t373*t397*(-1.0/6.0);
00474         Qd(7,3) = -t371*(t452+t623)-t374*(dt*t343*(t566-ew3*t425)*(1.0/3.0)-ew3*t343*t362*(1.0/2.0))+t373*(-t451+t452+dt*t343*(t436+t582-t583)*(1.0/3.0));
00475         Qd(7,4) = t599;
00476         Qd(7,5) = t608+t373*(t602+t610-dt*t343*(t490+t601-t609)*(1.0/3.0))-t371*(t602-dt*t343*(t600-t601)*(1.0/3.0));
00477         Qd(7,6) = -t374*(ew3*t343*(1.0/2.0)-dt*ew1*ew2*t343*(1.0/6.0))-dt*t343*t373*t613*(1.0/3.0);
00478         Qd(7,7) = t373*(dt-dt*t343*t435*(1.0/3.0))+dt*t343*t563*(1.0/3.0)+dt*t343*t371*t429*(1.0/3.0)+dt*t343*t374*t431*(1.0/3.0);
00479         Qd(7,8) = t655-t373*(t614-dt*ew2*ew3*t343*(1.0/6.0))-dt*ew2*ew3*t343*t374*(1.0/3.0);
00480         Qd(7,9) = dt*ew3*t343*t502*(1.0/6.0);
00481         Qd(7,10) = t343*t563*(-1.0/2.0);
00482         Qd(7,11) = dt*ew1*t343*t503*(-1.0/6.0);
00483         Qd(8,0) = t615;
00484         Qd(8,1) = t616;
00485         Qd(8,2) = t617;
00486         Qd(8,3) = -t374*t622-t371*(t451+t452-dt*t343*(t442+t582-t583)*(1.0/3.0))+t373*(t452-t623);
00487         Qd(8,4) = -t374*t631-t371*t635-t373*(t626-t627);
00488         Qd(8,5) = t647-t374*t641-t373*(t562+dt*t343*(t642-t643)*(1.0/3.0));
00489         Qd(8,6) = t652;
00490         Qd(8,7) = t655-t373*(t614-t653)-dt*ew2*ew3*t343*t374*(1.0/3.0);
00491         Qd(8,8) = t371*(dt-dt*t343*t441*(1.0/3.0))+dt*t343*t503*(1.0/3.0)+dt*t343*t373*t429*(1.0/3.0)+dt*t343*t374*t430*(1.0/3.0);
00492         Qd(8,9) = dt*ew2*t343*t502*(-1.0/6.0);
00493         Qd(8,10) = t657;
00494         Qd(8,11) = t343*t503*(-1.0/2.0);
00495         Qd(9,3) = dt*t343*t362*t502*(-1.0/6.0);
00496         Qd(9,5) = dt*t343*t390*t502*(-1.0/6.0);
00497         Qd(9,6) = t343*t502*(-1.0/2.0);
00498         Qd(9,8) = dt*ew2*t343*t502*(-1.0/6.0);
00499         Qd(9,9) = dt*t502;
00500         Qd(10,3) = dt*t343*t362*t563*(-1.0/6.0);
00501         Qd(10,4) = dt*t343*t388*t563*(-1.0/6.0);
00502         Qd(10,5) = t656;
00503         Qd(10,7) = t343*t563*(-1.0/2.0);
00504         Qd(10,8) = t657;
00505         Qd(10,10) = dt*t563;
00506         Qd(11,3) = dt*t343*t362*t503*(-1.0/6.0);
00507         Qd(11,4) = dt*t343*t381*t503*(-1.0/6.0);
00508         Qd(11,5) = dt*t343*t394*t503*(-1.0/6.0);
00509         Qd(11,7) = dt*ew1*t343*t503*(-1.0/6.0);
00510         Qd(11,8) = t343*t503*(-1.0/2.0);
00511         Qd(11,11) = dt*t503;
00512         Qd(12,0) = dt*t343*t346*t398*(-1.0/6.0);
00513         Qd(12,1) = dt*t343*t366*t398*(-1.0/6.0);
00514         Qd(12,2) = t658;
00515         Qd(12,3) = t343*t346*t398*(-1.0/2.0);
00516         Qd(12,4) = t343*t366*t398*(-1.0/2.0);
00517         Qd(12,5) = t659;
00518         Qd(12,12) = dt*t398;
00519         Qd(13,0) = t660;
00520         Qd(13,1) = dt*t343*t367*t399*(-1.0/6.0);
00521         Qd(13,2) = dt*t343*t372*t399*(-1.0/6.0);
00522         Qd(13,3) = t661;
00523         Qd(13,4) = t343*t367*t399*(-1.0/2.0);
00524         Qd(13,5) = t343*t372*t399*(-1.0/2.0);
00525         Qd(13,13) = dt*t399;
00526         Qd(14,0) = dt*t343*t345*t400*(-1.0/6.0);
00527         Qd(14,1) = t662;
00528         Qd(14,2) = dt*t343*t370*t400*(-1.0/6.0);
00529         Qd(14,3) = t343*t345*t400*(-1.0/2.0);
00530         Qd(14,4) = t663;
00531         Qd(14,5) = t343*t370*t400*(-1.0/2.0);
00532         Qd(14,14) = dt*t400;
00533         Qd(15,15) = dt*(n_L*n_L);
00534         Qd(16,16) = dt*(n_qvw1*n_qvw1);
00535         Qd(17,17) = dt*(n_qvw2*n_qvw2);
00536         Qd(18,18) = dt*(n_qvw3*n_qvw3);
00537         Qd(19,19) = dt*(n_qci[0]*n_qci[0]);
00538         Qd(20,20) = dt*(n_qci[1]*n_qci[1]);
00539         Qd(21,21) = dt*(n_qci[2]*n_qci[2]);
00540         Qd(22,22) = dt*(n_pic[0]*n_pic[0]);
00541         Qd(23,23) = dt*(n_pic[1]*n_pic[1]);
00542         Qd(24,24) = dt*(n_pic[2]*n_pic[2]);
00543 };
00544 
00545 #endif /* CALCQ_H_ */

ssf_core
Author(s): Stephan Weiss, Markus Achtelik
autogenerated on Fri Jan 3 2014 11:21:29