From 22593183ef1b9a787ca236c3986d27be49368d9b Mon Sep 17 00:00:00 2001 From: jbmag Date: Thu, 17 Jun 2021 15:04:47 +0200 Subject: [PATCH] added previous state in backward augmented state backward augmented state now also calculates previous speed, previous angular speed and previous orientation. It's used in the model of the plane to take into account simulated data. --- .../AugmentedState_PlaneDynamicModel_Sim.nb | 2385 +++++++++++ ...State_PlaneDynamicModel_calculated_prev.nb | 2505 +++++++++++ ...mentedState_PlaneDynamicModel_corrected.nb | 2636 ++++++++++++ ...wardAugmentedStateEstimator_Splitted_v7.nb | 3785 ++++++++++------- ...ntedStateEstimator_v7_JAugQprevSO.cppready | 0 ...ntedStateEstimator_v7_JAugQprevX0.cppready | 0 ...ntedStateEstimator_v7_JAugQprevX1.cppready | 44 + ...ntedStateEstimator_v7_JAugQprevX2.cppready | 0 ...tedStateEstimator_v7_JAugQprevqOS.cppready | 44 + ...ntedStateEstimator_v7_JAugVprevSO.cppready | 85 + ...ntedStateEstimator_v7_JAugVprevX0.cppready | 67 + ...ntedStateEstimator_v7_JAugVprevX1.cppready | 320 ++ ...ntedStateEstimator_v7_JAugVprevX2.cppready | 139 + ...tedStateEstimator_v7_JAugVprevqOS.cppready | 161 + ...ntedStateEstimator_v7_JAugWprevSO.cppready | 0 ...ntedStateEstimator_v7_JAugWprevX0.cppready | 165 + ...ntedStateEstimator_v7_JAugWprevX1.cppready | 228 + ...ntedStateEstimator_v7_JAugWprevX2.cppready | 0 ...tedStateEstimator_v7_JAugWprevqOS.cppready | 164 + ...dAugmentedStateEstimator_v7_Vprev.cppready | 114 + ...dAugmentedStateEstimator_v7_Wprev.cppready | 67 + .../ROAMestimation/AugmentedStateCalculator.h | 16 + .../AugmentedStateJacobianCalculator.h | 90 + .../include/ROAMestimation/Enums.h | 5 +- .../include/ROAMestimation/GenericEdge.h | 2 +- .../ROAMestimation/QuaternionGenericEdge.h | 8 +- .../generated/PlaneDynamicModel_Err.cppready | 475 ++- .../PlaneDynamicModel_JErrCBar.cppready | 231 +- .../PlaneDynamicModel_JErrCp.cppready | 527 ++- .../PlaneDynamicModel_JErrFDrag.cppready | 248 +- .../PlaneDynamicModel_JErrFLat.cppready | 131 +- .../PlaneDynamicModel_JErrFLift.cppready | 195 +- .../PlaneDynamicModel_JErrFThrust.cppready | 174 +- .../PlaneDynamicModel_JErrIbd.cppready | 347 +- .../PlaneDynamicModel_JErrIbod.cppready | 379 +- .../PlaneDynamicModel_JErrMPitch.cppready | 235 +- .../PlaneDynamicModel_JErrMRoll.cppready | 236 +- .../PlaneDynamicModel_JErrMYaw.cppready | 230 +- .../PlaneDynamicModel_JErrQ.cppready | 770 ++-- .../PlaneDynamicModel_JErrQprev.cppready | 776 ++++ .../PlaneDynamicModel_JErrV.cppready | 318 +- .../PlaneDynamicModel_JErrVprev.cppready | 442 ++ .../PlaneDynamicModel_JErrW.cppready | 201 +- .../PlaneDynamicModel_JErrWind.cppready | 875 ++-- .../PlaneDynamicModel_JErrWprev.cppready | 187 + .../PlaneDynamicModel_debug.cppready | 89 +- .../ROAMfunctions/PlaneDynamicModelM.h | 21 + roamfree/ROAMfunctions/src/AbsolutePoseM.cpp | 2 +- .../ROAMfunctions/src/AbsolutePositionM.cpp | 2 +- .../src/AccelerationInEarthFrameM.cpp | 2 +- roamfree/ROAMfunctions/src/AccelerationM.cpp | 2 +- .../ROAMfunctions/src/AckermannKinematicM.cpp | 2 +- .../src/AckermannKinematicNoInputsM.cpp | 2 +- .../src/AnchoredRectangularObjectFirstM.cpp | 2 +- .../src/AnchoredRectangularObjectM.cpp | 2 +- .../src/AngularVelocityInEarthFrameM.cpp | 2 +- .../ROAMfunctions/src/AngularVelocityM.cpp | 2 +- .../src/DifferentialDriveKinematicM.cpp | 2 +- roamfree/ROAMfunctions/src/DisplacementM.cpp | 2 +- .../ROAMfunctions/src/FixedFeaturePoseM.cpp | 2 +- .../src/FixedFeaturePositionM.cpp | 2 +- .../src/FramedHomogeneousPointM.cpp | 2 +- .../ROAMfunctions/src/GenericOdometerM.cpp | 2 +- .../ROAMfunctions/src/IMUIntegralDeltaPM.cpp | 2 +- .../ROAMfunctions/src/IMUIntegralDeltaQM.cpp | 2 +- .../src/ImagePlaneProjectionM.cpp | 2 +- .../ROAMfunctions/src/LinearVelocityM.cpp | 2 +- .../ROAMfunctions/src/PlaneDynamicModelM.cpp | 2 +- .../ROAMfunctions/src/QuadDynamicModelM.cpp | 2 +- .../ROAMfunctions/src/RectangularObjectM.cpp | 2 +- .../ROAMfunctions/src/TriskarKinematicM.cpp | 2 +- roamfree/ROAMfunctions/src/VectorFieldM.cpp | 2 +- 72 files changed, 15996 insertions(+), 4171 deletions(-) create mode 100644 _development/Mathematica/models/AugmentedState_PlaneDynamicModel_Sim.nb create mode 100644 _development/Mathematica/models/AugmentedState_PlaneDynamicModel_calculated_prev.nb create mode 100644 _development/Mathematica/models/AugmentedState_PlaneDynamicModel_corrected.nb create mode 100644 roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_JAugQprevSO.cppready create mode 100644 roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_JAugQprevX0.cppready create mode 100644 roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_JAugQprevX1.cppready create mode 100644 roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_JAugQprevX2.cppready create mode 100644 roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_JAugQprevqOS.cppready create mode 100644 roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_JAugVprevSO.cppready create mode 100644 roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_JAugVprevX0.cppready create mode 100644 roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_JAugVprevX1.cppready create mode 100644 roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_JAugVprevX2.cppready create mode 100644 roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_JAugVprevqOS.cppready create mode 100644 roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_JAugWprevSO.cppready create mode 100644 roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_JAugWprevX0.cppready create mode 100644 roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_JAugWprevX1.cppready create mode 100644 roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_JAugWprevX2.cppready create mode 100644 roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_JAugWprevqOS.cppready create mode 100644 roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_Vprev.cppready create mode 100644 roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_Wprev.cppready create mode 100644 roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrQprev.cppready create mode 100644 roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrVprev.cppready create mode 100644 roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrWprev.cppready diff --git a/_development/Mathematica/models/AugmentedState_PlaneDynamicModel_Sim.nb b/_development/Mathematica/models/AugmentedState_PlaneDynamicModel_Sim.nb new file mode 100644 index 0000000..266ea0d --- /dev/null +++ b/_development/Mathematica/models/AugmentedState_PlaneDynamicModel_Sim.nb @@ -0,0 +1,2385 @@ +(* Content-type: application/vnd.wolfram.mathematica *) + +(*** Wolfram Notebook File ***) +(* http://www.wolfram.com/nb *) + +(* CreatedBy='Mathematica 10.0' *) + +(*CacheID: 234*) +(* Internal cache information: +NotebookFileLineBreakTest +NotebookFileLineBreakTest +NotebookDataPosition[ 158, 7] +NotebookDataLength[ 93537, 2377] +NotebookOptionsPosition[ 87057, 2286] +NotebookOutlinePosition[ 87604, 2306] +CellTagsIndexPosition[ 87561, 2303] +WindowFrame->Normal*) + +(* Beginning of Notebook Content *) +Notebook[{ +Cell[BoxData[ + RowBox[{"Clear", "[", "\"\\"", "]"}]], "Input", + CellChangeTimes->{{3.5951624068865957`*^9, 3.595162434106801*^9}}, + CellLabel->"In[11]:=",ExpressionUUID->"28aebd44-7a23-4603-b370-38149a649cc1"], + +Cell[BoxData[{ + RowBox[{"Import", "[", + RowBox[{ + RowBox[{"NotebookDirectory", "[", "]"}], "<>", + "\"\\""}], "]"}], "\[IndentingNewLine]", + RowBox[{"Import", "[", + RowBox[{ + RowBox[{"NotebookDirectory", "[", "]"}], "<>", + "\"\\""}], "]"}]}], "Input", + CellChangeTimes->{{3.595163228510494*^9, 3.595163230854044*^9}, { + 3.6175174204357758`*^9, 3.617517424343709*^9}, {3.632487617988913*^9, + 3.632487633951275*^9}}, + CellLabel->"In[12]:=",ExpressionUUID->"d5e95da8-5675-4df2-8ff6-af7b8fb219a1"], + +Cell[CellGroupData[{ + +Cell["Quaternion Functions", "Subtitle", + InitializationCell->True, + CellChangeTimes->{{3.563606408899285*^9, 3.563606413402224*^9}, { + 3.572951250149337*^9, + 3.572951255936389*^9}},ExpressionUUID->"addb9730-72b2-4859-8e4b-\ +0e776f073328"], + +Cell[BoxData[ + RowBox[{ + RowBox[{"VectProdMatrix", "[", "v_", "]"}], ":=", + RowBox[{"{", + RowBox[{ + RowBox[{"{", + RowBox[{"0", ",", + RowBox[{"-", + RowBox[{"v", "[", + RowBox[{"[", "3", "]"}], "]"}]}], ",", + RowBox[{"v", "[", + RowBox[{"[", "2", "]"}], "]"}]}], "}"}], ",", + RowBox[{"{", + RowBox[{ + RowBox[{"v", "[", + RowBox[{"[", "3", "]"}], "]"}], ",", "0", ",", + RowBox[{"-", + RowBox[{"v", "[", + RowBox[{"[", "1", "]"}], "]"}]}]}], "}"}], ",", + RowBox[{"{", + RowBox[{ + RowBox[{"-", + RowBox[{"v", "[", + RowBox[{"[", "2", "]"}], "]"}]}], ",", + RowBox[{"v", "[", + RowBox[{"[", "1", "]"}], "]"}], ",", "0"}], "}"}]}], "}"}]}]], "Input",\ + + InitializationCell->True, + CellChangeTimes->{{3.563607037606062*^9, 3.563607124416689*^9}, + 3.563608231083735*^9}, + CellLabel->"In[14]:=",ExpressionUUID->"78e5840d-381c-47a9-b9dd-90c3c4dacb03"], + +Cell[BoxData[ + RowBox[{ + RowBox[{"PertStateSubs", "[", + RowBox[{"X_", ",", "dX_"}], "]"}], ":=", + RowBox[{"Flatten", "[", + RowBox[{"{", + RowBox[{ + RowBox[{"Table", "[", + RowBox[{ + RowBox[{ + RowBox[{"X", "[", + RowBox[{"[", "i", "]"}], "]"}], "\[Rule]", + RowBox[{ + RowBox[{"X", "[", + RowBox[{"[", "i", "]"}], "]"}], "+", + RowBox[{"dX", "[", + RowBox[{"[", "i", "]"}], "]"}]}]}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", "3"}], "}"}]}], "]"}], ",", + RowBox[{"Table", "[", + RowBox[{ + RowBox[{ + RowBox[{"X", "[", + RowBox[{"[", + RowBox[{"i", "+", "3"}], "]"}], "]"}], "\[Rule]", + RowBox[{ + RowBox[{"(", + RowBox[{"QuatInc", "[", + RowBox[{ + RowBox[{"X", "[", + RowBox[{"[", + RowBox[{"{", + RowBox[{"4", ",", "5", ",", "6", ",", "7"}], "}"}], "]"}], + "]"}], ",", + RowBox[{"dX", "[", + RowBox[{"[", + RowBox[{"{", + RowBox[{"4", ",", "5", ",", "6"}], "}"}], "]"}], "]"}]}], + "]"}], ")"}], "[", + RowBox[{"[", "i", "]"}], "]"}]}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", "4"}], "}"}]}], "]"}]}], "}"}], + "]"}]}]], "Input", + InitializationCell->True, + CellChangeTimes->{{3.571130663264105*^9, 3.571130765329321*^9}, { + 3.57113111793888*^9, 3.57113116341093*^9}, {3.571131195958424*^9, + 3.571131254626845*^9}, 3.571133049520565*^9}, + CellLabel->"In[15]:=",ExpressionUUID->"66ac8851-0848-4866-8069-42c37f27b5de"], + +Cell[BoxData[{ + RowBox[{ + RowBox[{ + RowBox[{"L2", "[", "x_", "]"}], ":=", + RowBox[{"Sqrt", "[", + RowBox[{"Sum", "[", + RowBox[{ + RowBox[{ + RowBox[{"x", "[", + RowBox[{"[", "j", "]"}], "]"}], "^", "2"}], ",", + RowBox[{"{", + RowBox[{"j", ",", "1", ",", + RowBox[{"Length", "[", "x", "]"}]}], "}"}]}], "]"}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"VecNorm", "[", "x_", "]"}], ":=", + RowBox[{"Table", "[", + RowBox[{ + RowBox[{ + RowBox[{"x", "[", + RowBox[{"[", "i", "]"}], "]"}], "/", + RowBox[{"L2", "[", "x", "]"}]}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", + RowBox[{"Length", "[", "x", "]"}]}], "}"}]}], "]"}]}], ";"}]}], "Input",\ + + InitializationCell->True, + CellChangeTimes->{{3.568453075302482*^9, 3.568453143924776*^9}}, + CellLabel->"In[16]:=",ExpressionUUID->"a40aa98c-a7a2-4814-b63b-e7c90c9932a0"], + +Cell[BoxData[ + RowBox[{ + RowBox[{ + RowBox[{"SkewMatrix", "[", "v_", "]"}], ":=", + RowBox[{ + RowBox[{"{", + RowBox[{ + RowBox[{"{", + RowBox[{"0", ",", + RowBox[{"-", + RowBox[{"v", "[", + RowBox[{"[", "1", "]"}], "]"}]}], ",", + RowBox[{"-", + RowBox[{"v", "[", + RowBox[{"[", "2", "]"}], "]"}]}], ",", + RowBox[{"-", + RowBox[{"v", "[", + RowBox[{"[", "3", "]"}], "]"}]}]}], "}"}], ",", + RowBox[{"{", + RowBox[{ + RowBox[{"v", "[", + RowBox[{"[", "1", "]"}], "]"}], ",", "0", ",", + RowBox[{"v", "[", + RowBox[{"[", "3", "]"}], "]"}], ",", + RowBox[{"-", + RowBox[{"v", "[", + RowBox[{"[", "2", "]"}], "]"}]}]}], "}"}], ",", + RowBox[{"{", + RowBox[{ + RowBox[{"v", "[", + RowBox[{"[", "2", "]"}], "]"}], ",", + RowBox[{"-", + RowBox[{"v", "[", + RowBox[{"[", "3", "]"}], "]"}]}], ",", "0", ",", + RowBox[{"v", "[", + RowBox[{"[", "1", "]"}], "]"}]}], "}"}], ",", + RowBox[{"{", + RowBox[{ + RowBox[{"v", "[", + RowBox[{"[", "3", "]"}], "]"}], ",", + RowBox[{"v", "[", + RowBox[{"[", "2", "]"}], "]"}], ",", + RowBox[{"-", + RowBox[{"v", "[", + RowBox[{"[", "1", "]"}], "]"}]}], ",", "0"}], "}"}]}], "}"}], "/", + "2"}]}], ";"}]], "Input", + InitializationCell->True, + CellChangeTimes->{{3.568443187261707*^9, 3.568443273520089*^9}, { + 3.568458568630984*^9, 3.568458568967643*^9}}, + CellLabel->"In[18]:=",ExpressionUUID->"ee9449fb-cd56-4179-b90c-f5e3bb0fdcdb"], + +Cell[BoxData[ + RowBox[{ + RowBox[{ + RowBox[{"ConstantAngularSpeeDtMatrix", "[", + RowBox[{"w_", ",", "Dt_"}], "]"}], ":=", + RowBox[{ + RowBox[{ + RowBox[{"Cos", "[", + RowBox[{ + RowBox[{"L2", "[", "w", "]"}], + RowBox[{"Dt", "/", "2"}]}], "]"}], " ", + RowBox[{"IdentityMatrix", "[", "4", "]"}]}], "+", + RowBox[{"2", + RowBox[{ + RowBox[{"Sin", "[", + RowBox[{ + RowBox[{"L2", "[", "w", "]"}], + RowBox[{"Dt", "/", "2"}]}], "]"}], "/", + RowBox[{"L2", "[", "w", "]"}]}], " ", + RowBox[{"SkewMatrix", "[", "w", "]"}]}]}]}], ";"}]], "Input", + InitializationCell->True, + CellChangeTimes->{{3.5684429732904*^9, 3.56844300516892*^9}, { + 3.568443103374373*^9, 3.568443183239631*^9}, {3.56844338461282*^9, + 3.568443409373211*^9}, {3.568453008511107*^9, 3.568453014783046*^9}, { + 3.60835655838758*^9, 3.608356558395439*^9}}, + CellLabel->"In[19]:=",ExpressionUUID->"b69052f0-f728-4bd6-9bf7-bae15aa76095"], + +Cell[BoxData[{ + RowBox[{ + RowBox[{ + RowBox[{"SubVec", "[", + RowBox[{"exp_", ",", "v1_", ",", "v2_"}], "]"}], ":=", + RowBox[{"exp", "/.", + RowBox[{"Table", "[", + RowBox[{ + RowBox[{ + RowBox[{"v1", "[", + RowBox[{"[", "i", "]"}], "]"}], "\[Rule]", + RowBox[{"v2", "[", + RowBox[{"[", "i", "]"}], "]"}]}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", + RowBox[{ + RowBox[{"Dimensions", "[", + RowBox[{"v1", ",", "1"}], "]"}], "[", + RowBox[{"[", "1", "]"}], "]"}]}], "}"}]}], "]"}]}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"VecToZero", "[", + RowBox[{"exp_", ",", "v_"}], "]"}], ":=", + RowBox[{"exp", "/.", + RowBox[{"Table", "[", + RowBox[{ + RowBox[{ + RowBox[{"v", "[", + RowBox[{"[", "i", "]"}], "]"}], "\[Rule]", "0"}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", + RowBox[{ + RowBox[{"Dimensions", "[", + RowBox[{"v", ",", "1"}], "]"}], "[", + RowBox[{"[", "1", "]"}], "]"}]}], "}"}]}], "]"}]}]}], + ";"}]}], "Input", + InitializationCell->True, + CellChangeTimes->{{3.568444740747602*^9, 3.568444776506934*^9}, { + 3.568444838483142*^9, 3.568444844345901*^9}}, + CellLabel->"In[20]:=",ExpressionUUID->"905c9ad7-8258-45b1-b64f-2d576f63cf20"], + +Cell[BoxData[{ + RowBox[{ + RowBox[{"ToGoodC", "[", "exp_", "]"}], ":=", + RowBox[{"Module", "[", + RowBox[{ + RowBox[{"{", "oexp", "}"}], ",", " ", + RowBox[{ + RowBox[{"oexp", "=", + RowBox[{"Experimental`OptimizeExpression", "[", "exp", "]"}]}], ";", + "\[IndentingNewLine]", + RowBox[{"If", "[", + RowBox[{ + RowBox[{ + RowBox[{ + RowBox[{"Dimensions", " ", "[", + RowBox[{ + RowBox[{"StringPosition", "[", + RowBox[{ + RowBox[{"ToString", "[", + RowBox[{"InputForm", "[", "oexp", "]"}], "]"}], ",", + "\"\\""}], "]"}], ",", "1"}], "]"}], "[", + RowBox[{"[", "1", "]"}], "]"}], ">", "0"}], ",", + RowBox[{"Block", "[", + RowBox[{ + RowBox[{"{", " ", + RowBox[{"locals", ",", " ", "code"}], "}"}], ",", + RowBox[{ + RowBox[{ + RowBox[{"{", + RowBox[{"locals", ",", "code"}], "}"}], "=", + RowBox[{"ReleaseHold", "[", + RowBox[{ + RowBox[{"(", + RowBox[{"Hold", "@@", "oexp"}], ")"}], "/.", + RowBox[{ + RowBox[{ + RowBox[{"Verbatim", "[", "Block", "]"}], "[", + RowBox[{"vars_", ",", "seq_"}], "]"}], "\[RuleDelayed]", + RowBox[{"{", + RowBox[{"vars", ",", + RowBox[{"Hold", "[", "seq", "]"}]}], "}"}]}]}], "]"}]}], ";", + + RowBox[{"ToString", "[", + RowBox[{"CForm", "[", "code", "]"}], "]"}]}]}], "]"}], + "\[IndentingNewLine]", ",", " ", + RowBox[{"ToString", "[", + RowBox[{"CForm", "[", "exp", "]"}], "]"}]}], "]"}]}]}], + "]"}]}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{"str_", ",", "file_"}], "]"}], ":=", + RowBox[{"Module", "[", + RowBox[{ + RowBox[{"{", "stream", "}"}], ",", + RowBox[{ + RowBox[{"stream", "=", + RowBox[{"OpenWrite", "[", "file", "]"}]}], ";", + RowBox[{"WriteString", "[", + RowBox[{"stream", ",", "str"}], "]"}], ";", + RowBox[{"Close", "[", "stream", "]"}], ";"}]}], "]"}]}]}], "Input", + InitializationCell->True, + CellChangeTimes->{ + 3.568440350374321*^9, {3.56844481385726*^9, 3.56844481884285*^9}, { + 3.568446816971497*^9, 3.568446831371454*^9}, {3.56844689667688*^9, + 3.568446897039928*^9}, {3.571133081827365*^9, 3.571133085326789*^9}}, + CellLabel->"In[22]:=",ExpressionUUID->"a7b9df83-8379-4a26-b658-cd16a2c680e3"] +}, Open ]], + +Cell["Beginning of stuff", "Title", + CellChangeTimes->{{3.5951624112710238`*^9, 3.5951624314967413`*^9}, + 3.595309863589487*^9},ExpressionUUID->"243eeb2a-7eb6-46c5-afb4-\ +9166e290b8da"], + +Cell["State", "Subsubtitle", + CellChangeTimes->{{3.5732982476551723`*^9, + 3.573298257964068*^9}},ExpressionUUID->"934e8056-77ed-401a-af0c-\ +38f7a1beea69"], + +Cell[BoxData[ + RowBox[{ + RowBox[{"X", "=", + RowBox[{"Table", "[", + RowBox[{ + RowBox[{"x", "[", "i", "]"}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", "39"}], "}"}]}], "]"}]}], ";"}]], "Input", + CellChangeTimes->{{3.5732982634483347`*^9, 3.573298275094433*^9}, { + 3.606108793708241*^9, 3.6061087941202383`*^9}, {3.832299724443674*^9, + 3.8322997253433037`*^9}, {3.8325883987514553`*^9, 3.832588399422798*^9}, + 3.83265271945756*^9, {3.832652778217605*^9, 3.832652780536776*^9}}, + CellLabel->"In[24]:=",ExpressionUUID->"89e76b07-ca4f-4879-a9de-3406cd562327"], + +Cell[BoxData[{ + RowBox[{ + RowBox[{"SW", "=", + RowBox[{"X", "[", + RowBox[{"[", + RowBox[{"1", ";;", "3"}], "]"}], "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"QWS", "=", + RowBox[{"X", "[", + RowBox[{"[", + RowBox[{"4", ";;", "7"}], "]"}], "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"VS", "=", + RowBox[{"X", "[", + RowBox[{"[", + RowBox[{"8", ";;", "10"}], "]"}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"WS", "=", + RowBox[{"X", "[", + RowBox[{"[", + RowBox[{"11", ";;", "13"}], "]"}], "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"AS", "=", + RowBox[{"X", "[", + RowBox[{"[", + RowBox[{"14", ";;", "16"}], "]"}], "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"ALPHAS", "=", + RowBox[{"X", "[", + RowBox[{"[", + RowBox[{"17", ";;", "19"}], "]"}], "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"QWSprev", " ", "=", " ", + RowBox[{"X", "[", + RowBox[{"[", + RowBox[{"30", ";;", "33"}], "]"}], "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"VSprev", " ", "=", " ", + RowBox[{"X", "[", + RowBox[{"[", + RowBox[{"34", ";;", "36"}], "]"}], "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"WSprev", " ", "=", " ", + RowBox[{"X", "[", + RowBox[{"[", + RowBox[{"37", ";;", "39"}], "]"}], "]"}]}], ";"}]}], "Input", + CellChangeTimes->{{3.632463823621646*^9, 3.632463843454915*^9}, { + 3.632463896430626*^9, 3.632463897928619*^9}, {3.632464021491871*^9, + 3.632464039150372*^9}, {3.6324668182980833`*^9, 3.6324668268371*^9}, { + 3.63248479427204*^9, 3.632484802957326*^9}, {3.6324860670961533`*^9, + 3.6324860672576303`*^9}, 3.6324863191229258`*^9, {3.632487946501395*^9, + 3.6324879678038177`*^9}, {3.632488008725754*^9, 3.632488019377554*^9}, { + 3.832298839476634*^9, 3.832298840668848*^9}, {3.832299719300475*^9, + 3.832299722434368*^9}, {3.832651983226047*^9, 3.832652008265932*^9}, { + 3.83265207622701*^9, 3.832652128771888*^9}}, + CellLabel->"In[25]:=",ExpressionUUID->"0f88cdc1-a39e-4d13-b528-8472e029b05f"], + +Cell["Parameters", "Subsubtitle", + CellChangeTimes->{{3.576920320535687*^9, + 3.576920325728017*^9}},ExpressionUUID->"a1d3915d-f613-4425-8a04-\ +fe27c5187298"], + +Cell[BoxData[{ + RowBox[{ + RowBox[{"h0", " ", "=", + RowBox[{"airDensity", "[", "1", "]"}]}], ";", + RowBox[{"(*", " ", + RowBox[{ + "base", " ", "point", " ", "altitude", " ", "above", " ", "sea", " ", + "level"}], " ", "*)"}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"T0", "=", + RowBox[{"airDensity", "[", "2", "]"}]}], ";", " ", + RowBox[{"(*", " ", + RowBox[{ + "ambient", " ", "temperature", " ", "at", " ", "mean", " ", "sea", " ", + "level"}], " ", "*)"}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"p0", "=", + RowBox[{"airDensity", "[", "3", "]"}]}], ";", " ", + RowBox[{"(*", " ", + RowBox[{"pressure", " ", "at", " ", "sea", " ", "level"}], " ", "*)"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"a", " ", "=", " ", + RowBox[{ + RowBox[{"-", "6.5"}], " ", + RowBox[{"10", "^", + RowBox[{"-", "3"}]}]}]}], ";", " ", + RowBox[{"(*", " ", + RowBox[{"temperature", " ", "gradient"}], " ", "*)"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"Ra", " ", "=", " ", "287.30"}], ";", " ", + RowBox[{"(*", " ", + RowBox[{"gas", " ", "constant", " ", "for", " ", "air"}], " ", "*)"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"g", "=", " ", + RowBox[{"gravity", "[", "1", "]"}]}], ";", " ", + RowBox[{"(*", " ", "gravity", " ", "*)"}], ";"}], " ", + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"CFT1", "=", + RowBox[{"fThrust", "[", "1", "]"}]}], ";", " ", + RowBox[{"(*", " ", + RowBox[{"thrust", " ", "force", " ", "coefficients"}], "*)"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"CFT2", "=", + RowBox[{"fThrust", "[", "2", "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"CFT3", "=", + RowBox[{"fThrust", "[", "3", "]"}]}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"CFX1", "=", + RowBox[{"fDrag", "[", "1", "]"}]}], ";", " ", + RowBox[{"(*", " ", + RowBox[{"drag", " ", "force", " ", "coefficients"}], " ", "*)"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"CFXalpha1", " ", "=", " ", + RowBox[{"fDrag", "[", "2", "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"CFXalpha2", " ", "=", " ", + RowBox[{"fDrag", "[", "3", "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"CFXbeta2", " ", "=", " ", + RowBox[{"fDrag", "[", "4", "]"}]}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"CFY1", "=", + RowBox[{"fLat", "[", "1", "]"}]}], ";", " ", + RowBox[{"(*", " ", + RowBox[{"lateral", " ", "force", " ", "coefficient"}], " ", "*)"}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"CFZ1", "=", + RowBox[{"fLift", "[", "1", "]"}]}], ";", " ", + RowBox[{"(*", " ", + RowBox[{"lift", " ", "force", " ", "coefficients"}], " ", "*)"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"CFZalpha", "=", + RowBox[{"fLift", "[", "2", "]"}]}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"CMXa", "=", + RowBox[{"mRoll", "[", "1", "]"}]}], ";", + RowBox[{"(*", " ", + RowBox[{"roll", " ", "moment", " ", "coefficients"}], " ", "*)"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"CMXbeta", "=", + RowBox[{"mRoll", "[", "2", "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"CMX\[Omega]tildex", "=", + RowBox[{"mRoll", "[", "3", "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"CMX\[Omega]tildez", "=", + RowBox[{"mRoll", "[", "4", "]"}]}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"CMY1", "=", + RowBox[{"mPitch", "[", "1", "]"}]}], ";", + RowBox[{"(*", " ", + RowBox[{"pitch", " ", "moment", " ", "coefficients"}], " ", "*)"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"CMYe", "=", + RowBox[{"mPitch", "[", "2", "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"CMYalpha", "=", + RowBox[{"mPitch", "[", "3", "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"CMY\[Omega]tildey", "=", + RowBox[{"mPitch", "[", "4", "]"}]}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"CMZr", "=", + RowBox[{"mYaw", "[", "1", "]"}]}], ";"}], " ", + RowBox[{"(*", " ", + RowBox[{"yaw", " ", "moment", " ", "coefficients"}], " ", "*)"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"CMZbeta", "=", + RowBox[{"mYaw", "[", "2", "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"CMZ\[Omega]tildez", "=", + RowBox[{"mYaw", "[", "3", "]"}]}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"cbar", " ", "=", " ", + RowBox[{"cBar", "[", "1", "]"}]}], ";", " ", + RowBox[{"(*", " ", + RowBox[{"mean", " ", "aereodynamic", " ", "chord"}], " ", "*)"}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"PD", "=", + RowBox[{"cp", "[", "1", "]"}]}], ";", " ", + RowBox[{"(*", " ", + RowBox[{"propeller", " ", "diameter"}], " ", "*)"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"S", "=", + RowBox[{"cp", "[", "2", "]"}]}], ";", " ", + RowBox[{"(*", " ", + RowBox[{"wing", " ", "surface"}], " ", "*)"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"b", "=", + RowBox[{"cp", "[", "3", "]"}]}], ";", " ", + RowBox[{"(*", " ", + RowBox[{"wind", " ", "span"}], " ", "*)"}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"m", " ", "=", " ", + RowBox[{"cp", "[", "4", "]"}]}], ";", " ", + RowBox[{"(*", " ", "mass", " ", "*)"}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"Ixx", "=", + RowBox[{"ibd", "[", "1", "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"Iyy", "=", + RowBox[{"ibd", "[", "2", "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"Izz", " ", "=", + RowBox[{"ibd", "[", "3", "]"}]}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"Ixz", "=", + RowBox[{"ibod", "[", "1", "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"Ixy", "=", + RowBox[{"ibod", "[", "2", "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"Iyz", "=", + RowBox[{"ibod", "[", "3", "]"}]}], ";"}]}], "Input", + CellChangeTimes->{{3.630156966488817*^9, 3.630156997327407*^9}, { + 3.630240787595443*^9, 3.630240871119405*^9}, {3.632462563480411*^9, + 3.6324626574790983`*^9}, {3.6324627822557497`*^9, + 3.6324627840911913`*^9}, {3.6324628429667397`*^9, 3.632462845395872*^9}, { + 3.632463452013672*^9, 3.632463468595188*^9}, {3.6324635424969807`*^9, + 3.632463578847823*^9}, {3.6324638570723257`*^9, 3.632463892398457*^9}, + 3.6324640836270533`*^9, {3.632465074140349*^9, 3.632465180642975*^9}, { + 3.632465229204035*^9, 3.632465255301177*^9}, {3.632465722100479*^9, + 3.632465736568845*^9}, {3.6324658501384373`*^9, 3.632465874972086*^9}, { + 3.632466231247014*^9, 3.6324662436985607`*^9}, {3.632466293804319*^9, + 3.63246666807226*^9}, {3.632466702556988*^9, 3.632466739506076*^9}, { + 3.6324669714561977`*^9, 3.632466989419321*^9}, {3.6324671749054317`*^9, + 3.632467191927196*^9}, {3.632467242245677*^9, 3.632467340635708*^9}, { + 3.632467371717039*^9, 3.632467375747436*^9}, {3.6324674914282084`*^9, + 3.632467492058816*^9}, {3.6324735043656588`*^9, 3.632473540164185*^9}, + 3.632473659229198*^9, {3.6324739348426247`*^9, 3.632473970395046*^9}, { + 3.632474373588602*^9, 3.63247449276618*^9}, {3.632484917459989*^9, + 3.6324849486938457`*^9}, {3.632485035091144*^9, 3.632485038949534*^9}, { + 3.6324859054962873`*^9, 3.632485963402033*^9}, {3.632486128812416*^9, + 3.632486192068241*^9}, {3.6325479968094463`*^9, 3.632548014105666*^9}, { + 3.6325480606534348`*^9, 3.632548077440715*^9}, {3.632563365761787*^9, + 3.6325633685352783`*^9}, {3.634301394539613*^9, 3.6343014011226387`*^9}, { + 3.6343016879675493`*^9, 3.634301688364636*^9}, {3.63430226729615*^9, + 3.634302271560999*^9}, 3.634302333067884*^9, 3.634303000998124*^9, { + 3.634303882252087*^9, 3.634303882568714*^9}, 3.634303913676345*^9, { + 3.7715699472059917`*^9, 3.771569949298133*^9}, {3.772359310486154*^9, + 3.772359365392565*^9}, {3.7723594227641973`*^9, 3.772359613795624*^9}, { + 3.772360215586792*^9, 3.77236021597759*^9}, {3.77314250558466*^9, + 3.773142505699256*^9}, {3.774589936393152*^9, 3.774589945751444*^9}, { + 3.777109991948604*^9, 3.777109997353167*^9}, {3.820073719683056*^9, + 3.820073720934869*^9}}, + CellLabel->"In[39]:=",ExpressionUUID->"33f47236-ad3c-4a61-8f93-8222aa6cc9fa"], + +Cell[BoxData[{ + RowBox[{ + RowBox[{ + RowBox[{ + RowBox[{"IB", "=", + RowBox[{"{", + RowBox[{ + RowBox[{"{", + RowBox[{"Ixx", ",", "Ixy", ",", "Ixz"}], "}"}], ",", + RowBox[{"{", + RowBox[{"Ixy", ",", "Iyy", ",", "Iyz"}], "}"}], ",", + RowBox[{"{", + RowBox[{"Ixz", ",", "Iyz", ",", "Izz"}], "}"}]}], "}"}]}], ";"}], " ", + RowBox[{"(*", " ", + RowBox[{"Inertia", " ", "matrix"}], " ", "*)"}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"AIRDENSITY", " ", "=", " ", + RowBox[{"Table", "[", + RowBox[{ + RowBox[{"airDensity", "[", "i", "]"}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", "3"}], "}"}]}], "]"}]}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"FTHRUST", " ", "=", " ", + RowBox[{"Table", "[", + RowBox[{ + RowBox[{"fThrust", "[", "i", "]"}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", "3"}], "}"}]}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"FDRAG", " ", "=", " ", + RowBox[{"Table", "[", + RowBox[{ + RowBox[{"fDrag", "[", "i", "]"}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", "4"}], "}"}]}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"FLAT", " ", "=", " ", + RowBox[{"Table", "[", + RowBox[{ + RowBox[{"fLat", "[", "i", "]"}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", "1"}], "}"}]}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"FLIFT", "=", " ", + RowBox[{"Table", "[", + RowBox[{ + RowBox[{"fLift", "[", "i", "]"}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", "2"}], "}"}]}], "]"}]}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MROLL", " ", "=", " ", + RowBox[{"Table", "[", + RowBox[{ + RowBox[{"mRoll", "[", "i", "]"}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", "4"}], "}"}]}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MPITCH", " ", "=", " ", + RowBox[{"Table", "[", + RowBox[{ + RowBox[{"mPitch", "[", "i", "]"}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", "4"}], "}"}]}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"MYAW", " ", "=", " ", + RowBox[{"Table", "[", + RowBox[{ + RowBox[{"mYaw", "[", "i", "]"}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", "3"}], "}"}]}], "]"}]}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"CBAR", " ", "=", " ", + RowBox[{"Table", "[", + RowBox[{ + RowBox[{"cBar", "[", "i", "]"}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", "1"}], "}"}]}], "]"}]}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"CP", " ", "=", " ", + RowBox[{"Table", "[", + RowBox[{ + RowBox[{"cp", "[", "i", "]"}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", "4"}], "}"}]}], "]"}]}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"IBD", " ", "=", " ", + RowBox[{"Table", "[", + RowBox[{ + RowBox[{"ibd", "[", "i", "]"}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", "3"}], "}"}]}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"IBOD", " ", "=", " ", + RowBox[{"Table", "[", + RowBox[{ + RowBox[{"ibod", "[", "i", "]"}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", "3"}], "}"}]}], "]"}]}], + ";"}], "\[IndentingNewLine]"}], "Input", + CellChangeTimes->{{3.6324667198820887`*^9, 3.632466771730083*^9}, { + 3.6324745054453506`*^9, 3.632474645927088*^9}, {3.6324746840634813`*^9, + 3.6324746893289757`*^9}, {3.632484902639776*^9, 3.6324849084840813`*^9}, { + 3.6324849547663603`*^9, 3.632485026020185*^9}, {3.632485970613168*^9, + 3.6324859838780127`*^9}, {3.632486046079789*^9, 3.632486046409316*^9}, { + 3.632486196852049*^9, 3.632486214881503*^9}, 3.6325480929677277`*^9, { + 3.6325633642283154`*^9, 3.632563371518127*^9}, {3.772359621486253*^9, + 3.772359671724037*^9}, {3.772359705820054*^9, 3.772359711259782*^9}, { + 3.772359937605813*^9, 3.772360048550315*^9}, {3.7723600966964912`*^9, + 3.772360359842062*^9}, {3.775206708566532*^9, 3.775206780566957*^9}}, + CellLabel->"In[77]:=",ExpressionUUID->"53a40cfc-f0e8-4e44-9f25-b052baa99c29"], + +Cell[BoxData[ + RowBox[{ + RowBox[{ + RowBox[{"WINDW", "=", + RowBox[{"Table", "[", + RowBox[{ + RowBox[{"wind", "[", "i", "]"}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", "3"}], "}"}]}], "]"}]}], ";"}], + RowBox[{"(*", " ", + RowBox[{"wind", " ", "speed", " ", "in", " ", "world", " ", "frame"}], " ", + "*)"}]}]], "Input", + CellChangeTimes->{3.8325884344470167`*^9}, + CellLabel->"In[90]:=",ExpressionUUID->"230fd034-8fee-4db7-94bf-9c0c77325dce"], + +Cell[BoxData[{ + RowBox[{ + RowBox[{ + RowBox[{"n", " ", "=", " ", + RowBox[{"z", "[", "1", "]"}]}], ";"}], " ", + RowBox[{"(*", " ", + RowBox[{"propeller", " ", "speed"}], " ", "*)"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"\[Delta]a", "=", + RowBox[{"z", "[", "2", "]"}]}], ";", + RowBox[{"(*", " ", + RowBox[{"aileron", " ", "deflection"}], " ", "*)"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"\[Delta]e", "=", + RowBox[{"z", "[", "3", "]"}]}], ";", + RowBox[{"(*", " ", + RowBox[{"elevator", " ", "deflection"}], " ", "*)"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{ + RowBox[{"\[Delta]r", "=", + RowBox[{"z", "[", "4", "]"}]}], ";"}], + RowBox[{"(*", " ", + RowBox[{"rudder", " ", "deflection"}], " ", "*)"}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"Z", "=", + RowBox[{"Table", "[", + RowBox[{ + RowBox[{"z", "[", "i", "]"}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", "4"}], "}"}]}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"NOISES", "=", + RowBox[{"Table", "[", + RowBox[{ + RowBox[{"eta", "[", "i", "]"}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", "6"}], "}"}]}], "]"}]}], ";"}]}], "Input", + CellChangeTimes->{{3.632466994423296*^9, 3.6324670023447437`*^9}, { + 3.6324673568630743`*^9, 3.632467391423369*^9}, {3.632473596617989*^9, + 3.632473628189311*^9}, {3.6324860167895203`*^9, 3.6324860253571453`*^9}, { + 3.6324862298118343`*^9, 3.632486253278077*^9}, {3.632488200362911*^9, + 3.632488217048265*^9}}, + CellLabel->"In[91]:=",ExpressionUUID->"3db40cdd-aae8-4aed-bbcf-7b6c917acc34"], + +Cell["Equations", "Subsubtitle", + CellChangeTimes->{{3.573298514193714*^9, 3.5732985158016453`*^9}, { + 3.5814133864550056`*^9, 3.581413389613989*^9}, 3.5814150402587423`*^9, { + 3.600064344573599*^9, + 3.600064346732829*^9}},ExpressionUUID->"3a22e419-d65c-4c9e-84c6-\ +863e409e519a"], + +Cell["Air density model", "Text", + CellChangeTimes->{{3.632464865348349*^9, 3.632464865838887*^9}, { + 3.632464907969676*^9, + 3.632464942989243*^9}},ExpressionUUID->"9ac0d911-0e84-4579-bcb9-\ +2110f48ca902"], + +Cell[BoxData[ + RowBox[{ + RowBox[{"(*", + RowBox[{ + RowBox[{"T", "=", + RowBox[{"T0", + RowBox[{"(", + RowBox[{"1", "+", + RowBox[{"a", " ", + RowBox[{ + RowBox[{"(", + RowBox[{"h0", "+", + RowBox[{"SW", "[", + RowBox[{"[", "3", "]"}], "]"}]}], ")"}], "/", "T0"}]}]}], + ")"}]}]}], ";", "\[IndentingNewLine]", + RowBox[{"\[Rho]", "=", + RowBox[{ + RowBox[{"p0", + RowBox[{ + RowBox[{ + RowBox[{"(", + RowBox[{"1", "+", + RowBox[{"a", + RowBox[{ + RowBox[{"(", + RowBox[{"h0", "+", + RowBox[{"SW", "[", + RowBox[{"[", "3", "]"}], "]"}]}], ")"}], "/", "T0"}]}]}], + ")"}], "^", "5.2561"}], "/", + RowBox[{"(", + RowBox[{"Ra", " ", "T"}], ")"}]}]}], "//", "FullSimplify"}]}], ";"}], + "*)"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"\[Rho]", " ", "=", " ", "1.268"}], ";"}]}]], "Input", + CellChangeTimes->{{3.632462276976709*^9, 3.632462293513674*^9}, { + 3.63246255070463*^9, 3.63246255752766*^9}, {3.632462666581998*^9, + 3.632462767916204*^9}, 3.632462848468005*^9, {3.632462884866085*^9, + 3.632463087325437*^9}, {3.632464836860113*^9, 3.632464838151527*^9}, { + 3.6324649483473377`*^9, 3.632464955096529*^9}, {3.632473673346718*^9, + 3.6324736736899147`*^9}, {3.632484816779133*^9, 3.632484818831092*^9}, { + 3.6324860751887093`*^9, 3.6324860763421392`*^9}, {3.6326351738117943`*^9, + 3.632635175495201*^9}, {3.634305313267096*^9, 3.634305315999455*^9}, + 3.771231605060816*^9, {3.7713227938461018`*^9, 3.771322795852179*^9}, { + 3.772946102534224*^9, 3.772946104786611*^9}, 3.773142614758792*^9, { + 3.781532267573668*^9, 3.781532269711938*^9}, {3.831543702800745*^9, + 3.831543703679405*^9}, {3.8315534192530937`*^9, 3.831553422523533*^9}, + 3.831553568121788*^9, {3.831699456786346*^9, 3.83169945836695*^9}, { + 3.8325862000061197`*^9, 3.83258620771362*^9}, 3.832644757539814*^9}, + CellLabel->"In[97]:=",ExpressionUUID->"9f81cfa6-a749-4450-bfb3-736066b50bce"], + +Cell["Airspeed", "Text", + CellChangeTimes->{{3.6324649632440243`*^9, + 3.6324649676453876`*^9}},ExpressionUUID->"656d798c-a5c9-4cc6-a1dd-\ +06c8dc6c6565"], + +Cell[BoxData[{ + RowBox[{ + RowBox[{"AIRS", "=", + RowBox[{"VSprev", "-", + RowBox[{ + RowBox[{"QuatToRot", "[", + RowBox[{"QuatInv", "[", "QWSprev", "]"}], "]"}], ".", "WINDW"}]}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"\[Alpha]", "=", + RowBox[{"ArcTan", "[", + RowBox[{ + RowBox[{"AIRS", "[", + RowBox[{"[", "1", "]"}], "]"}], ",", + RowBox[{"AIRS", "[", + RowBox[{"[", "3", "]"}], "]"}]}], "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"\[Beta]", "=", + RowBox[{"ArcSin", "[", + RowBox[{ + RowBox[{"AIRS", "[", + RowBox[{"[", "2", "]"}], "]"}], "/", + RowBox[{"L2", "[", "AIRS", "]"}]}], "]"}]}], ";"}], " ", + "\[IndentingNewLine]", + RowBox[{"(*", " ", + RowBox[{"dynamic", " ", "pressure"}], " ", "*)"}]}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"qbar", "=", + RowBox[{"\[Rho]", " ", + RowBox[{ + RowBox[{ + RowBox[{"L2", "[", "AIRS", "]"}], "^", "2"}], "/", "2"}]}]}], + ";"}], "\[IndentingNewLine]"}], "Input", + CellChangeTimes->{{3.632463641043902*^9, 3.632463700014979*^9}, { + 3.632463760723637*^9, 3.6324638026841*^9}, {3.6324638481892347`*^9, + 3.6324638485511303`*^9}, {3.632464012285355*^9, 3.6324640922581472`*^9}, { + 3.63246427497224*^9, 3.6324645819649153`*^9}, {3.632464802514184*^9, + 3.632464847852816*^9}, {3.632464958182373*^9, 3.632464975561612*^9}, { + 3.632548707477417*^9, 3.632548715250105*^9}, {3.774611994153266*^9, + 3.774611997890765*^9}, 3.781531472311548*^9, {3.782109505828165*^9, + 3.782109507995604*^9}, 3.831543728856717*^9, {3.831548449935013*^9, + 3.831548459109014*^9}, {3.831550697106958*^9, 3.831550699078291*^9}, { + 3.832247268983202*^9, 3.832247269898589*^9}, {3.8322473017502937`*^9, + 3.832247302418412*^9}, 3.83224897502195*^9, {3.832249189474696*^9, + 3.8322491900790663`*^9}, {3.832299949819049*^9, 3.832299967291203*^9}, { + 3.832300630206356*^9, 3.8323006337974033`*^9}, {3.8323006971741323`*^9, + 3.832300698717581*^9}, {3.832304078072732*^9, 3.832304081647558*^9}, { + 3.8323052074555264`*^9, 3.832305221126236*^9}, {3.83230937627665*^9, + 3.8323094006135273`*^9}, {3.832309438863019*^9, 3.832309447080554*^9}, { + 3.832309501309993*^9, 3.832309611275866*^9}, {3.832309752797491*^9, + 3.8323098428394012`*^9}, 3.832484877821838*^9, {3.832484914498892*^9, + 3.832484945826915*^9}, {3.832588820959147*^9, 3.83258884035524*^9}}, + CellLabel->"In[98]:=",ExpressionUUID->"29b01d2f-83ef-4d5a-8a25-69d70c2e9207"], + +Cell["Wind frame ", "Text", + CellChangeTimes->{{3.6324649823613243`*^9, + 3.6324649864149113`*^9}},ExpressionUUID->"7aa4501a-e764-4f85-8410-\ +73dc94387049"], + +Cell[BoxData[ + RowBox[{ + RowBox[{"(*", + RowBox[{ + RowBox[{"RWINDS", "=", + RowBox[{ + RowBox[{"Transpose", "[", + RowBox[{"RotationMatrix", "[", + RowBox[{"\[Alpha]", ",", + RowBox[{"{", + RowBox[{"0", ",", "1", ",", "0"}], "}"}]}], "]"}], "]"}], ".", + RowBox[{"RotationMatrix", "[", + RowBox[{"\[Beta]", ",", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "1"}], "}"}]}], "]"}]}]}], ";"}], "*)"}], + "\[IndentingNewLine]", + RowBox[{ + RowBox[{"RWINDS", " ", "=", " ", + RowBox[{ + RowBox[{"{", + RowBox[{ + RowBox[{"{", + RowBox[{ + RowBox[{"Cos", "[", "\[Alpha]", "]"}], ",", "0", ",", + RowBox[{"-", + RowBox[{"Sin", "[", "\[Alpha]", "]"}]}]}], "}"}], ",", + RowBox[{"{", + RowBox[{"0", ",", "1", ",", "0"}], "}"}], ",", + RowBox[{"{", + RowBox[{ + RowBox[{"Sin", "[", "\[Alpha]", "]"}], ",", "0", ",", + RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], "}"}]}], "}"}], ".", + RowBox[{"{", + RowBox[{ + RowBox[{"{", + RowBox[{ + RowBox[{"Cos", "[", "\[Beta]", "]"}], ",", + RowBox[{"-", + RowBox[{"Sin", "[", "\[Beta]", "]"}]}], ",", "0"}], "}"}], ",", + RowBox[{"{", + RowBox[{ + RowBox[{"Sin", "[", "\[Beta]", "]"}], ",", + RowBox[{"Cos", "[", "\[Beta]", "]"}], ",", "0"}], "}"}], ",", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "1"}], "}"}]}], "}"}]}]}], + ";"}]}]], "Input", + CellChangeTimes->{{3.6324646247401857`*^9, 3.632464632600142*^9}, { + 3.632464680101325*^9, 3.632464784467514*^9}, {3.6324649794605837`*^9, + 3.6324649904896584`*^9}, {3.632474117326888*^9, 3.632474118720315*^9}, { + 3.632474151294368*^9, 3.6324741531255283`*^9}, {3.63257138686383*^9, + 3.632571433828497*^9}, {3.632571484241777*^9, 3.632571500711404*^9}, { + 3.634300370544889*^9, 3.634300375506771*^9}, {3.774266318707065*^9, + 3.774266321501566*^9}, {3.774612005261743*^9, 3.774612007778376*^9}, { + 3.774691121917021*^9, 3.7746911255822783`*^9}, 3.7815314791176033`*^9, + 3.831544148398527*^9, 3.832299971483536*^9, 3.832300701453065*^9, + 3.832309961580757*^9, {3.832309996548251*^9, 3.832310121436122*^9}, { + 3.832310268266717*^9, 3.83231027077208*^9}, 3.83248499849096*^9, + 3.83248513698739*^9, 3.832587709752873*^9, {3.832591612964304*^9, + 3.8325916292963552`*^9}}, + CellLabel-> + "In[102]:=",ExpressionUUID->"dbea6cef-0336-433e-92e3-cc59b4a69219"], + +Cell["Thrust force", "Text", + CellChangeTimes->{{3.632464997924268*^9, 3.63246500193495*^9}, + 3.7723597660999403`*^9},ExpressionUUID->"15e73123-e84e-4360-b55b-\ +334ef918a821"], + +Cell[BoxData[ + RowBox[{ + RowBox[{"(*", + RowBox[{ + "this", " ", "can", " ", "cause", " ", "division", " ", "by", " ", + "zero"}], "*)"}], "\[IndentingNewLine]", + RowBox[{"(*", + RowBox[{ + RowBox[{ + RowBox[{"CFT", "[", "J_", "]"}], ":=", + RowBox[{"CFT1", "+", + RowBox[{"CFT2", " ", "J"}], "+", " ", + RowBox[{"CFT3", " ", + RowBox[{"J", "^", "2"}]}]}]}], ";", "\[IndentingNewLine]", + RowBox[{"FT", "=", + RowBox[{"\[Rho]", " ", + RowBox[{"n", "^", "2"}], " ", + RowBox[{"PD", "^", "4"}], " ", + RowBox[{"CFT", "[", + RowBox[{ + RowBox[{"L2", "[", "AIRS", "]"}], "/", + RowBox[{"(", + RowBox[{"PD", " ", "\[Pi]", " ", "n"}], ")"}]}], "]"}]}]}], ";"}], + "*)"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"FT", "=", + RowBox[{"\[Rho]", " ", + RowBox[{"PD", "^", "2"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"CFT1", " ", + RowBox[{"PD", "^", "2"}], " ", + RowBox[{"n", "^", "2"}]}], " ", "+", " ", + RowBox[{"CFT2", " ", "PD", " ", "n", " ", + RowBox[{ + RowBox[{"L2", "[", "AIRS", "]"}], "/", "\[Pi]"}]}], "+", + RowBox[{"CFT3", " ", + RowBox[{ + RowBox[{ + RowBox[{"L2", "[", "AIRS", "]"}], "^", "2"}], "/", + RowBox[{"\[Pi]", "^", "2"}]}]}]}], ")"}]}]}], ";"}]}]], "Input", + CellChangeTimes->{{3.6324630957984457`*^9, 3.6324631592841997`*^9}, { + 3.63246362407451*^9, 3.632463645243698*^9}, {3.632464992403228*^9, + 3.632465054737678*^9}, {3.632465263450095*^9, 3.6324652660503607`*^9}, + 3.632465322251411*^9, 3.632548717997613*^9, 3.634980291260788*^9, { + 3.634980577412641*^9, 3.6349807142179937`*^9}, 3.6349807576397457`*^9, { + 3.6349808735654984`*^9, 3.634980887899046*^9}, {3.634982313791176*^9, + 3.634982334481447*^9}, {3.634988395067772*^9, 3.634988403877116*^9}, { + 3.8315480801115217`*^9, 3.831548105174776*^9}, {3.831552329329358*^9, + 3.831552340183215*^9}}, + CellLabel-> + "In[103]:=",ExpressionUUID->"176642b9-ef21-428e-bda6-88b3064f0fe2"], + +Cell["Drag force", "Text", + CellChangeTimes->{{3.632465883892436*^9, + 3.632465884995759*^9}},ExpressionUUID->"ac9144ba-14f4-4cb2-9d7c-\ +1b84aa717d7b"], + +Cell[BoxData[{ + RowBox[{ + RowBox[{"CFX", "=", + RowBox[{"CFX1", "-", + RowBox[{"CFXalpha1", " ", "\[Alpha]"}], "+", + RowBox[{"CFXalpha2", " ", + RowBox[{"\[Alpha]", "^", "2"}]}], "+", + RowBox[{"CFXbeta2", " ", + RowBox[{"\[Beta]", "^", "2"}]}]}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"FWX", "=", + RowBox[{"qbar", " ", "S", " ", "CFX"}]}], ";"}]}], "Input", + CellChangeTimes->{{3.632465900598139*^9, 3.63246595313962*^9}, { + 3.632466061225836*^9, 3.632466070531445*^9}, 3.632474180051531*^9, { + 3.632486333812204*^9, 3.632486338262673*^9}, {3.831544195582663*^9, + 3.831544196885738*^9}}, + CellLabel-> + "In[104]:=",ExpressionUUID->"f4931c36-9cb3-41d5-9636-f3fbcef428c9"], + +Cell["Lateral force", "Text", + CellChangeTimes->{{3.63246580061057*^9, + 3.632465802148518*^9}},ExpressionUUID->"57ce017d-559b-4d58-ba18-\ +1e7631c436d9"], + +Cell[BoxData[{ + RowBox[{ + RowBox[{"CFY", "=", + RowBox[{"CFY1", " ", "\[Beta]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"FWY", "=", + RowBox[{"qbar", " ", "S", " ", "CFY"}]}], ";"}]}], "Input", + CellChangeTimes->{{3.632465807062015*^9, 3.63246584035797*^9}, + 3.774612027001218*^9, 3.7815314926688843`*^9}, + CellLabel-> + "In[106]:=",ExpressionUUID->"63203ee0-9601-47b1-9606-b67b10ce9a46"], + +Cell["Lift force", "Text", + CellChangeTimes->{{3.6324653247972507`*^9, + 3.6324653267561407`*^9}},ExpressionUUID->"4432b235-bb2b-4e9d-87f8-\ +55bf93c648df"], + +Cell[BoxData[{ + RowBox[{ + RowBox[{"CFZ", "=", + RowBox[{ + RowBox[{"-", "CFZ1"}], "+", + RowBox[{"CFZalpha", " ", "\[Alpha]"}]}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"FWZ", "=", + RowBox[{"qbar", " ", "S", " ", "CFZ"}]}], ";"}]}], "Input", + CellChangeTimes->{{3.6324653325318327`*^9, 3.632465404457531*^9}, { + 3.632465682669859*^9, 3.6324656933429127`*^9}, {3.632465792314137*^9, + 3.632465798108323*^9}, 3.774612028899675*^9, {3.831544264680245*^9, + 3.8315442668236523`*^9}}, + CellLabel-> + "In[108]:=",ExpressionUUID->"c73ee33c-6f07-4ea9-82e2-0f37fad73c8e"], + +Cell["Roll moment", "Text", + CellChangeTimes->{{3.632466790216868*^9, + 3.632466796296864*^9}},ExpressionUUID->"3b78e61b-a31c-43c6-949c-\ +8ab46a558347"], + +Cell[BoxData[{ + RowBox[{ + RowBox[{"\[Omega]tildex", "=", " ", + RowBox[{"b", " ", + RowBox[{ + RowBox[{"WSprev", "[", + RowBox[{"[", "1", "]"}], "]"}], "/", + RowBox[{"(", + RowBox[{"2", " ", + RowBox[{"L2", "[", "AIRS", "]"}]}], ")"}]}]}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"\[Omega]tildez", "=", + RowBox[{"b", " ", + RowBox[{ + RowBox[{"WSprev", "[", + RowBox[{"[", "3", "]"}], "]"}], "/", + RowBox[{"(", + RowBox[{"2", " ", + RowBox[{"L2", "[", "AIRS", "]"}]}], ")"}]}]}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"CMX", "=", + RowBox[{ + RowBox[{"CMXa", " ", "\[Delta]a"}], " ", "-", + RowBox[{"CMXbeta", " ", "\[Beta]"}], "+", + RowBox[{"CMX\[Omega]tildex", " ", "\[Omega]tildex"}], " ", "-", + RowBox[{"CMX\[Omega]tildez", " ", "\[Omega]tildez"}]}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MBX", "=", + RowBox[{"qbar", " ", "S", " ", "b", " ", "CMX"}]}], ";"}]}], "Input", + CellChangeTimes->{{3.632466801116767*^9, 3.6324669518785152`*^9}, { + 3.632467018819668*^9, 3.632467037920843*^9}, {3.632467073135228*^9, + 3.632467123591279*^9}, {3.6324674621580877`*^9, 3.632467462850863*^9}, { + 3.6324862875540953`*^9, 3.6324862888104773`*^9}, {3.6325487193918743`*^9, + 3.632548719873026*^9}, 3.634292400473446*^9, {3.774612038472706*^9, + 3.774612054579494*^9}, {3.7815315014383574`*^9, 3.781531501625888*^9}, { + 3.8315442788487*^9, 3.831544307311618*^9}, {3.832247284671228*^9, + 3.832247288169239*^9}, {3.832304124264847*^9, 3.832304125991396*^9}, { + 3.8323052366918087`*^9, 3.832305240596637*^9}}, + CellLabel-> + "In[110]:=",ExpressionUUID->"7be4a0a1-b2c0-4d7b-b564-9f92dabf375f"], + +Cell["Pitch moment", "Text", + CellChangeTimes->{{3.632467155741108*^9, + 3.632467157541481*^9}},ExpressionUUID->"78b600c8-17c9-43b2-8561-\ +39fdff7b92e4"], + +Cell[BoxData[{ + RowBox[{ + RowBox[{"\[Omega]tildey", "=", + RowBox[{"cbar", " ", + RowBox[{ + RowBox[{"WSprev", "[", + RowBox[{"[", "2", "]"}], "]"}], "/", + RowBox[{"(", + RowBox[{"2", " ", + RowBox[{"L2", "[", "AIRS", "]"}]}], ")"}]}]}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"CMY", "=", + RowBox[{ + RowBox[{"-", "CMY1"}], "-", + RowBox[{"CMYe", " ", "\[Delta]e"}], " ", "+", + RowBox[{"CMY\[Omega]tildey", " ", "\[Omega]tildey"}], "+", + RowBox[{"CMYalpha", " ", "\[Alpha]"}]}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MBY", "=", + RowBox[{"qbar", " ", "S", " ", "cbar", " ", "CMY"}]}], ";"}]}], "Input", + CellChangeTimes->{{3.632467408804394*^9, 3.632467485452004*^9}, { + 3.6324734635941877`*^9, 3.6324734804954777`*^9}, {3.632486355640251*^9, + 3.632486356968248*^9}, 3.6325487214032393`*^9, {3.774612041318342*^9, + 3.774612074499466*^9}, 3.831544287152123*^9, {3.831544336079039*^9, + 3.8315443421869287`*^9}, {3.832247292103303*^9, 3.832247292550302*^9}, + 3.832304119896791*^9, {3.832305243123885*^9, 3.8323052436925697`*^9}}, + CellLabel-> + "In[114]:=",ExpressionUUID->"4ce3e11d-b2d8-42df-94f3-f44671feb140"], + +Cell["Yaw moment", "Text", + CellChangeTimes->{{3.6324733890787363`*^9, + 3.632473392049634*^9}},ExpressionUUID->"c302290e-b2df-4299-8d3e-\ +f59d1fc850b6"], + +Cell[BoxData[{ + RowBox[{ + RowBox[{"CMZ", " ", "=", " ", + RowBox[{ + RowBox[{ + RowBox[{"-", "CMZr"}], " ", "\[Delta]r"}], " ", "+", " ", + RowBox[{"CMZ\[Omega]tildez", " ", "\[Omega]tildez"}], " ", "+", + RowBox[{"CMZbeta", " ", "\[Beta]"}]}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MBZ", "=", + RowBox[{"qbar", " ", "S", " ", "b", " ", "CMZ"}]}], ";"}]}], "Input", + CellChangeTimes->{{3.6324733978818483`*^9, 3.632473492292446*^9}, + 3.632473773078868*^9, 3.632486362438322*^9, {3.632734158387792*^9, + 3.632734159678158*^9}, 3.774612081502236*^9, {3.781531506233139*^9, + 3.781531506482781*^9}, 3.831544367175006*^9}, + CellLabel-> + "In[117]:=",ExpressionUUID->"1a28b43d-e9a2-4ebf-8251-f3ab2ddb7786"], + +Cell["Error equations", "Text", + CellChangeTimes->{{3.6324737754609947`*^9, 3.632473779006583*^9}, { + 3.832248443615851*^9, + 3.832248454604686*^9}},ExpressionUUID->"4ba37bc9-3ca4-4415-be72-\ +e388f0dad47a"], + +Cell[BoxData[{ + RowBox[{ + RowBox[{"forceBfPrev", " ", "=", " ", + RowBox[{ + RowBox[{"{", + RowBox[{"FT", ",", "0", ",", "0"}], "}"}], "+", + RowBox[{"RWINDS", ".", + RowBox[{"{", + RowBox[{"FWX", ",", "FWY", ",", "FWZ"}], "}"}]}]}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"forceBf", " ", "=", " ", + RowBox[{ + RowBox[{"QuatToRot", "[", + RowBox[{"QuatInv", "[", "QWS", "]"}], "]"}], ".", + RowBox[{"QuatToRot", "[", "QWSprev", "]"}], ".", "forceBfPrev"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"momentBfPrev", " ", "=", " ", + RowBox[{"{", + RowBox[{"MBX", ",", "MBY", ",", "MBZ"}], "}"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"momentBf", " ", "=", " ", + RowBox[{ + RowBox[{"QuatToRot", "[", + RowBox[{"QuatInv", "[", "QWS", "]"}], "]"}], ".", + RowBox[{"QuatToRot", "[", "QWSprev", "]"}], " ", ".", "momentBfPrev"}]}], + ";"}], "\[IndentingNewLine]"}], "Input", + CellChangeTimes->{{3.8322484594733133`*^9, 3.8322485749169703`*^9}, { + 3.832248623124647*^9, 3.8322486546207333`*^9}, {3.8322491968290453`*^9, + 3.832249198979947*^9}, {3.832249579349374*^9, 3.832249584091609*^9}, { + 3.832299977117188*^9, 3.8322999859622297`*^9}, 3.832301691372081*^9, { + 3.8323037953164454`*^9, 3.832303798567361*^9}, 3.832304405712686*^9, { + 3.832603447873975*^9, 3.8326034516114483`*^9}, {3.8326043193782263`*^9, + 3.8326043198498*^9}, {3.8326051206906157`*^9, 3.8326051208899183`*^9}}, + CellLabel-> + "In[119]:=",ExpressionUUID->"a2375489-1864-4b87-9537-7325ce23183f"], + +Cell[BoxData[{ + RowBox[{ + RowBox[{"Ahatlong", "=", + RowBox[{ + RowBox[{ + RowBox[{"QuatToRot", "[", + RowBox[{"QuatInv", "[", "QWS", "]"}], "]"}], ".", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", + RowBox[{"-", "g"}]}], "}"}]}], " ", "+", " ", + RowBox[{ + RowBox[{"1", "/", "m"}], " ", + RowBox[{"(", "forceBf", ")"}]}]}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"AlphaHatlong", "=", " ", + RowBox[{ + RowBox[{"Inverse", "[", "IB", "]"}], ".", + RowBox[{"(", + RowBox[{"momentBf", "-", + RowBox[{"WS", "\[Cross]", + RowBox[{"(", + RowBox[{"IB", ".", "WS"}], ")"}]}]}], ")"}]}]}], ";"}]}], "Input", + CellChangeTimes->{{3.632473782667057*^9, 3.632473783948543*^9}, { + 3.632473980553233*^9, 3.632474122781205*^9}, {3.632474156463627*^9, + 3.6324741654080887`*^9}, {3.632474242482204*^9, 3.632474271983945*^9}, { + 3.6324746531850758`*^9, 3.632474665303821*^9}, {3.632474712022402*^9, + 3.632474752296493*^9}, {3.6324863716442823`*^9, 3.632486373451024*^9}, { + 3.6324864511201982`*^9, 3.632486454813035*^9}, {3.6324869920050364`*^9, + 3.632486995777545*^9}, 3.632572048567833*^9, {3.632820407426373*^9, + 3.632820415425437*^9}, {3.6330662719267473`*^9, 3.633066275128694*^9}, + 3.6330672246184607`*^9, {3.633067323386444*^9, 3.633067323446558*^9}, { + 3.633668520364181*^9, 3.6336685404354343`*^9}, {3.6336692322822113`*^9, + 3.633669235904211*^9}, {3.6342782924356194`*^9, 3.634278296987405*^9}, { + 3.6342783329840317`*^9, 3.6342783528443823`*^9}, {3.6344523148400803`*^9, + 3.634452317130845*^9}, {3.7746911481775713`*^9, 3.774691149760127*^9}, { + 3.8319972965581017`*^9, 3.831997300997398*^9}, {3.831997646944586*^9, + 3.831997653276815*^9}, {3.832040513254773*^9, 3.8320405446977453`*^9}, { + 3.832247316431518*^9, 3.832247316918489*^9}, {3.832247398198761*^9, + 3.832247398934546*^9}, {3.8322486164054317`*^9, 3.832248619091113*^9}, + 3.832248663164793*^9, 3.832248733164867*^9, {3.832253775199555*^9, + 3.832253778582693*^9}, {3.832304657623603*^9, 3.832304668959508*^9}, { + 3.83230474180126*^9, 3.832304819047428*^9}, {3.832304866856081*^9, + 3.832304871719405*^9}, {3.832304908031575*^9, 3.832304910295294*^9}, { + 3.832304997408471*^9, 3.832305003871789*^9}, 3.832305054682229*^9, + 3.832305095560555*^9, {3.832305129953635*^9, 3.8323051386755466`*^9}, { + 3.832489603897416*^9, 3.832489625021543*^9}, 3.8325886953739157`*^9, { + 3.832596155349049*^9, 3.832596156246426*^9}, 3.832597219252363*^9}, + CellLabel-> + "In[123]:=",ExpressionUUID->"fe419f0a-adf5-43af-9f08-7b8a7f19180b"], + +Cell[BoxData[{ + RowBox[{ + RowBox[{"Ahat", "=", "Ahatlong"}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"Alphahat", "=", "AlphaHatlong"}], + ";"}], "\[IndentingNewLine]"}], "Input", + CellChangeTimes->{{3.632486455549663*^9, 3.6324865144132833`*^9}, { + 3.632486628670555*^9, 3.632486630005704*^9}, {3.6324868147521963`*^9, + 3.632486817333385*^9}, 3.632486929763055*^9, {3.632487008855073*^9, + 3.632487019375031*^9}, {3.632487069748158*^9, 3.63248707741856*^9}, { + 3.632487112090027*^9, 3.632487150840695*^9}, {3.632487252771805*^9, + 3.6324872660424747`*^9}, {3.632488139926024*^9, 3.632488140072042*^9}, { + 3.632734121716978*^9, 3.632734200327458*^9}, {3.633067298879485*^9, + 3.63306731942362*^9}, {3.634278298237008*^9, 3.63427829964246*^9}, { + 3.63427833587599*^9, 3.634278345660905*^9}, {3.634298584045291*^9, + 3.634298585468253*^9}, {3.7723598046089363`*^9, 3.772359805228352*^9}}, + CellLabel-> + "In[125]:=",ExpressionUUID->"ff5f7d73-add6-4e24-9e55-7b15786660c6"], + +Cell[BoxData[{ + RowBox[{ + RowBox[{"DBG", "=", + RowBox[{"{", + RowBox[{ + "FWX", ",", "FWY", ",", "FWZ", ",", "FT", ",", "MBX", ",", "MBY", ",", + "MBZ"}], "}"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"AhatlongDBG", "=", + RowBox[{ + RowBox[{ + RowBox[{"QuatToRot", "[", + RowBox[{"QuatInv", "[", "QWS", "]"}], "]"}], ".", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", + RowBox[{"-", "g"}]}], "}"}]}], "+", + RowBox[{ + RowBox[{"1", "/", "m"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"{", + RowBox[{ + RowBox[{"f", "[", "4", "]"}], ",", "0", ",", "0"}], "}"}], "+", + RowBox[{ + RowBox[{"Transpose", "[", "RWINDS", "]"}], ".", + RowBox[{"{", + RowBox[{ + RowBox[{"f", "[", "1", "]"}], ",", + RowBox[{"f", "[", "2", "]"}], ",", + RowBox[{"f", "[", "3", "]"}]}], "}"}]}]}], ")"}]}], "-", + RowBox[{"WS", "\[Cross]", "VS"}]}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"AlphahatlongDBG", "=", " ", + RowBox[{ + RowBox[{"Inverse", "[", "IB", "]"}], ".", + RowBox[{"(", + RowBox[{ + RowBox[{"{", + RowBox[{ + RowBox[{"mm", "[", "1", "]"}], ",", + RowBox[{"mm", "[", "2", "]"}], ",", + RowBox[{"mm", "[", "3", "]"}]}], "}"}], "-", + RowBox[{"WS", "\[Cross]", + RowBox[{"(", + RowBox[{"IB", ".", "WS"}], ")"}]}]}], ")"}]}]}], ";"}]}], "Input", + CellChangeTimes->{{3.633234965893896*^9, 3.633234975029994*^9}, { + 3.6332350425322247`*^9, 3.633235051548617*^9}, 3.633244923580419*^9, { + 3.633327208678884*^9, 3.633327209833385*^9}, {3.634298362377328*^9, + 3.6342983969550533`*^9}, {3.634298517168047*^9, 3.63429851785793*^9}, { + 3.634298591096792*^9, 3.634298593067531*^9}, {3.634299639194295*^9, + 3.634299645586197*^9}, {3.634299681394882*^9, 3.6342996858226843`*^9}, + 3.634299852392684*^9, 3.634300111654772*^9, {3.634300178359811*^9, + 3.634300196975257*^9}, 3.8315541730600033`*^9, {3.832036484673256*^9, + 3.832036488336998*^9}, {3.8320404846183357`*^9, 3.832040508374733*^9}}, + CellLabel-> + "In[127]:=",ExpressionUUID->"d17df8c1-9260-4a0c-8495-b5ef6f820848"], + +Cell[BoxData[ + RowBox[{ + RowBox[{ + RowBox[{"ERR", "=", + RowBox[{ + RowBox[{"Flatten", "[", + RowBox[{"{", + RowBox[{ + RowBox[{"AS", "-", "Ahat"}], ",", + RowBox[{"ALPHAS", "-", "Alphahat"}]}], "}"}], "]"}], "+", + "NOISES"}]}], ";"}], "\[IndentingNewLine]"}]], "Input", + CellChangeTimes->{{3.632487913384449*^9, 3.632487921874243*^9}, { + 3.632488026778446*^9, 3.632488059172721*^9}, {3.632488578258338*^9, + 3.632488606817566*^9}, {3.784538832900227*^9, 3.784538833715313*^9}, { + 3.832303486557356*^9, 3.8323034980789423`*^9}, {3.832303665519973*^9, + 3.832303715578887*^9}, {3.832303758180533*^9, 3.832303816907099*^9}, { + 3.832303999935471*^9, 3.832304012743093*^9}, {3.832485226223405*^9, + 3.8324852294423723`*^9}}, + CellLabel-> + "In[130]:=",ExpressionUUID->"8c91f72d-3f47-4547-a792-5e0813bcc352"], + +Cell["Predictor", "Subsubtitle", + CellChangeTimes->{{3.573298851623851*^9, 3.57329885458322*^9}, { + 3.6326367603131237`*^9, + 3.6326367612299*^9}},ExpressionUUID->"b92b1bb0-abe4-4170-8cfc-1e9dd8bc5679"], + +Cell[BoxData[ + RowBox[{ + RowBox[{"(*", " ", + RowBox[{ + "until", " ", "we", " ", "fix", " ", "the", " ", "predictor", " ", + "interface", " ", "we", " ", "just", " ", "predict", " ", "that", " ", + "we", " ", "move", " ", "forward", " ", "at", " ", "1", " ", + RowBox[{"m", "/", "s"}]}], " ", "*)"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"SWhat", "=", + RowBox[{"SW", "+", + RowBox[{ + RowBox[{"QuatToRot", "[", "QWS", "]"}], ".", + RowBox[{"(", + RowBox[{"{", + RowBox[{ + RowBox[{"1", "*", "dt"}], ",", " ", "0", ",", "0"}], "}"}], + ")"}]}]}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"QWShat", "=", "QWS"}], ";"}], "\[IndentingNewLine]"}]}]], "Input",\ + + CellChangeTimes->{{3.632636764848427*^9, 3.632636870351482*^9}, + 3.633327075601856*^9, 3.634538652426509*^9}, + CellLabel-> + "In[131]:=",ExpressionUUID->"dea35a84-3a0f-4600-a2f4-2ab045608d87"], + +Cell["Output", "Subsubtitle", + CellChangeTimes->{{3.573298851623851*^9, + 3.57329885458322*^9}},ExpressionUUID->"bf1afef8-2e19-4e8e-9c26-\ +8b9e596cd10c"], + +Cell[BoxData[ + RowBox[{ + RowBox[{"SetDirectory", "[", + RowBox[{"NotebookDirectory", "[", "]"}], "]"}], ";"}]], "Input", + CellLabel-> + "In[133]:=",ExpressionUUID->"c2f98c75-5faf-44c8-a958-147c2a660678"], + +Cell[CellGroupData[{ + +Cell[BoxData[{ + RowBox[{ + RowBox[{"Err", " ", "=", " ", + RowBox[{"VecToZero", "[", + RowBox[{"ERR", ",", "NOISES"}], "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JErrNoises", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "NOISES", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JErrPose", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "SW", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JErrQ", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "QWS", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JErrV", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "VS", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JErrW", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "WS", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JErrA", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "AS", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JErrAlpha", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "ALPHAS", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JErrQprev", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "QWSprev", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JErrVprev", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "VSprev", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JErrWprev", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "WSprev", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{"Dimensions", "[", "ERR", "]"}]}], "Input", + CellChangeTimes->{{3.573299033702265*^9, 3.5732990981024723`*^9}, { + 3.573299164795454*^9, 3.5732991692508307`*^9}, {3.573456001706543*^9, + 3.5734560048187523`*^9}, {3.574152235462706*^9, 3.574152238223208*^9}, { + 3.5798489548623657`*^9, 3.579848955103243*^9}, {3.581401976643291*^9, + 3.5814019885894117`*^9}, {3.58141377998179*^9, 3.581413814574294*^9}, { + 3.581414129291024*^9, 3.581414154679249*^9}, {3.581765163109974*^9, + 3.581765173746241*^9}, {3.581765277500546*^9, 3.581765284545003*^9}, { + 3.6000644741616707`*^9, 3.600064476157126*^9}, {3.600064636065899*^9, + 3.60006464126534*^9}, {3.60610941052632*^9, 3.606109449315563*^9}, { + 3.617517818502956*^9, 3.617517831777896*^9}, 3.617518017239346*^9, + 3.6175224718126917`*^9, {3.617535776012012*^9, 3.6175357977310543`*^9}, { + 3.617691303889946*^9, 3.617691325047617*^9}, {3.6191537025170603`*^9, + 3.619153721782481*^9}, {3.6248780977624187`*^9, 3.6248781199132423`*^9}, { + 3.624878461572874*^9, 3.624878529180564*^9}, {3.626608128585112*^9, + 3.626608147165112*^9}, 3.626608290164939*^9, {3.627970217824462*^9, + 3.6279702738585*^9}, {3.627970352003558*^9, 3.62797036945547*^9}, { + 3.62971028921635*^9, 3.6297105068213043`*^9}, {3.629806116962702*^9, + 3.629806117816551*^9}, {3.630321579757883*^9, 3.630321583197545*^9}, { + 3.6303216981362867`*^9, 3.630321706346642*^9}, {3.630321757205647*^9, + 3.63032178972649*^9}, {3.6303219034786263`*^9, 3.630321908393339*^9}, { + 3.630322038330865*^9, 3.630322043133882*^9}, {3.630322095647093*^9, + 3.630322097242449*^9}, {3.630322176329072*^9, 3.630322192772039*^9}, + 3.630322231534956*^9, {3.6303231357931433`*^9, 3.630323150170701*^9}, { + 3.632488075940673*^9, 3.632488120413509*^9}, {3.6324882328081903`*^9, + 3.632488236596348*^9}, {3.632488545113055*^9, 3.632488557103654*^9}, { + 3.832304244631546*^9, 3.8323042449118233`*^9}, {3.83267621120508*^9, + 3.8326762815793943`*^9}}, + CellLabel-> + "In[134]:=",ExpressionUUID->"716ee195-4767-4067-92df-43ed8da3bd15"], + +Cell[BoxData[ + RowBox[{"{", "6", "}"}]], "Output", + CellChangeTimes->{ + 3.832304252829917*^9, 3.832304414353018*^9, 3.8323044766343803`*^9, + 3.832304675181992*^9, 3.832304758656847*^9, 3.832304788848263*^9, + 3.832304823704341*^9, 3.832304919215414*^9, 3.832304957528509*^9, + 3.8323050100942993`*^9, 3.832305060814383*^9, 3.8323051041767406`*^9, + 3.832305150684971*^9, 3.8323052670345993`*^9, 3.832307399835104*^9, + 3.832308045512988*^9, 3.832308268930606*^9, 3.832308569017376*^9, { + 3.83230979560109*^9, 3.832309806289016*^9}, 3.8323102818921347`*^9, + 3.832310366544312*^9, 3.8323809824018517`*^9, 3.832492002479721*^9, + 3.832494705811585*^9, 3.8325916783555603`*^9, 3.832594972029954*^9, + 3.832596182134193*^9, 3.832597315921483*^9, 3.832599921098196*^9, + 3.8326016449186573`*^9, 3.832601735197879*^9, 3.8326041973641443`*^9, + 3.83260434364052*^9, 3.8326045115586023`*^9, 3.832604580015305*^9, + 3.832605150749875*^9, 3.8326439041718693`*^9, 3.8326440463130608`*^9, + 3.832644286295485*^9, 3.8326444028166027`*^9, 3.832644572749074*^9, + 3.8326447963960342`*^9, 3.832647147297798*^9, 3.832648788806467*^9, + 3.832652787809978*^9, 3.832676412540778*^9, 3.832676649979397*^9, + 3.832676733612543*^9}, + CellLabel-> + "Out[145]=",ExpressionUUID->"1bcc3553-7aaf-4cd1-b8d1-178a830567d9"] +}, Open ]], + +Cell[BoxData[{ + RowBox[{ + RowBox[{ + RowBox[{"JErrAirDensity", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "AIRDENSITY", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], + ";"}], "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JErrFThrust", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "FTHRUST", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JErrFDrag", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "FDRAG", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JErrFLat", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "FLAT", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"JErrFLift", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "FLIFT", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JErrMRoll", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "MROLL", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JErrMPitch", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "MPITCH", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"JErrMYaw", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "MYAW", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"JErrCBar", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "CBAR", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JErrCp", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "CP", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JErrIbd", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "IBD", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"JErrIbod", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "IBOD", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JErrWind", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "WINDW", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], + ";"}], "\[IndentingNewLine]"}], "Input", + CellChangeTimes->{{3.632488242861087*^9, 3.63248826986901*^9}, { + 3.632488302807823*^9, 3.632488310708211*^9}, {3.632549179684801*^9, + 3.6325491830062933`*^9}, 3.632549233454729*^9, {3.772359825953252*^9, + 3.772359833140972*^9}, {3.7723603795875998`*^9, 3.7723605440122347`*^9}, { + 3.772364327412218*^9, 3.772364391371952*^9}, {3.774583805536461*^9, + 3.774583830109449*^9}, {3.775206417689166*^9, 3.77520651396764*^9}, { + 3.77520681773381*^9, 3.7752068235363483`*^9}}, + CellLabel-> + "In[146]:=",ExpressionUUID->"cdb0dbd5-e787-49a0-bb76-c4468b401cbc"], + +Cell[BoxData[ + RowBox[{ + RowBox[{"Run", "[", "\"\\"", "]"}], ";"}]], "Input", + CellChangeTimes->{{3.605001578968874*^9, 3.605001586300552*^9}}, + CellLabel-> + "In[159]:=",ExpressionUUID->"41b82835-d8e1-446a-bec9-5cabfad38221"], + +Cell[BoxData[{ + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "Err", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrNoises", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrPose", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrQ", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrV", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrW", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrA", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrAlpha", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrQprev", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrVprev", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrWprev", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrAirDensity", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrFThrust", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrFDrag", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrFLat", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrFLift", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrMRoll", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrMPitch", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrMYaw", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrCBar", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrCp", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrIbd", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrIbod", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrWind", "]"}], ",", + "\"\\""}], "]"}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", + RowBox[{"Flatten", "[", + RowBox[{"{", + RowBox[{"SWhat", ",", "QWShat"}], "}"}], "]"}], "]"}], ",", + "\"\\""}], "]"}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "DBG", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", + RowBox[{"Flatten", "[", + RowBox[{"{", + RowBox[{"AhatlongDBG", ",", "AlphahatlongDBG"}], "}"}], "]"}], "]"}], + ",", "\"\\""}], "]"}], + ";"}]}], "Input", + CellChangeTimes->{{3.57329895973281*^9, 3.5732990549001513`*^9}, { + 3.57329915658729*^9, 3.5732991792126017`*^9}, {3.57329950483379*^9, + 3.573299506552189*^9}, {3.573456025938487*^9, 3.573456050810623*^9}, { + 3.574152257417694*^9, 3.574152270678878*^9}, {3.579848972820836*^9, + 3.579848980632938*^9}, {3.5814019928034163`*^9, 3.58140200617373*^9}, { + 3.581402211401102*^9, 3.581402211596108*^9}, {3.581413827590354*^9, + 3.5814138412416353`*^9}, {3.581414159871148*^9, 3.5814141678166847`*^9}, { + 3.5814153831753387`*^9, 3.581415395342497*^9}, 3.5817651746256*^9, { + 3.581765299311152*^9, 3.581765299456869*^9}, {3.583128034440399*^9, + 3.583128047560751*^9}, 3.600064481693171*^9, {3.6000645447460413`*^9, + 3.600064544768753*^9}, 3.605001560621763*^9, {3.606108718930592*^9, + 3.606108718954677*^9}, {3.606109458423176*^9, 3.6061094764764013`*^9}, { + 3.606109797740724*^9, 3.6061098090662613`*^9}, {3.6061099568596067`*^9, + 3.6061099585045424`*^9}, {3.617517858078869*^9, 3.6175178592248383`*^9}, { + 3.6175179081434183`*^9, 3.617517920863043*^9}, {3.617535815115308*^9, + 3.6175358335236387`*^9}, {3.6176913283286*^9, 3.617691368379365*^9}, { + 3.61915374051689*^9, 3.6191538003658953`*^9}, {3.624878788568387*^9, + 3.6248788166517553`*^9}, {3.6248826307742863`*^9, 3.624882631277665*^9}, { + 3.626606948234263*^9, 3.626606949108878*^9}, {3.626608154489799*^9, + 3.6266082045678787`*^9}, {3.626608392816053*^9, 3.62660839330686*^9}, { + 3.6297095365887547`*^9, 3.629709536615233*^9}, {3.629710549109893*^9, + 3.629710565390929*^9}, {3.630321740435486*^9, 3.630321740476594*^9}, { + 3.63032317635601*^9, 3.630323195570113*^9}, {3.632488330236774*^9, + 3.6324884066409388`*^9}, {3.632549186227305*^9, 3.6325491909421*^9}, { + 3.632636908066628*^9, 3.632636924909555*^9}, {3.633235060777438*^9, + 3.6332350814039917`*^9}, {3.634287350134149*^9, 3.63428738680274*^9}, { + 3.6342984069072943`*^9, 3.634298419729596*^9}, {3.63429849221801*^9, + 3.6342985327140083`*^9}, {3.772364411990938*^9, 3.7723645797179956`*^9}, + 3.831547472610442*^9, 3.8315475368389463`*^9, {3.8326762952689457`*^9, + 3.832676342389372*^9}}, + CellLabel-> + "In[160]:=",ExpressionUUID->"cbb713d0-b847-42c0-9c5d-8772380d8dba"], + +Cell[BoxData[{ + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]"}], "Input", + CellChangeTimes->CompressedData[" +1:eJwdzX8slHEcB/AjDFkUnUXrmHK5U5tLfv1RfSumW9nt2pqfc6Ety7HObCod +3bhLdMadstZUG+dHnNI1JpNjixMSt0bt7nRx2fM8lHOxRfR8vn+89/rjvb3f +wVkFwmvODAYjig7Y+EM4J39GoJWqN1hemcsOqCb2MRS014f+HQZj6q1s0Lhj +4IPp8gns540F1QwtU+qlBkdFO5XsF/TOqvUBaC93rgF7ZjywcSMNIa4d9J/v +JvaRpbVwU0ug4djgIlCZ4hbr1EWg1ApDHEi1CLPB88OfsD7+g1ugXGXEOnQx +pxK6CRTQXI+tXkjcVk0TSFxajOW883FR05p0TKw21xIfaCaQPmse2zvRpgE5 +/NIW8EjQ7BT45KJxGswLkXPMawSaPKnAurMcN+dpZ1U8Cbi+sELaaO8nxSyD +TGammecgUHlRhgUUGnXsE7SCxzYO6CjwIrfWCZQ8JsZ6NlXZQVEp/w/YH5qW +suREIgHXPQOkyNbwSFcSeee/x+qN/R/B0QbhOGhvHv8OtjcGWcHWA34HWbtJ +1O29H6u2KDSgxJTXAmr0NR58LxJFnKvDimWXB8H4L2l68PilC6s3fEhUPMnH +5oRGbIFTRzu3wWNDmsxKWu1LTxGYmJDdBpaUtGPNXEOZjnZNoroHZnGttbN7 +STRwdqMONLK6rn71JdHTbVMO2Pft1oiFtrc22gBKf334GRVIoZLOJTu4GCYZ +7zhEob+Cnknw95ifbIxWaojGpgeam8Bkrg2r7cutkrAoZJ14jVVeSb0r5lHI +eVcFdkCmlYGK5bfY+pyAuXza6YRCrP2OwATmn36O3dPRsAi+mr9tw30Yk139 +kEKxUf5YlZSjYSopFBkUjq1LKnA+Q6vcELmB/wEdT8R4 + "], + CellLabel-> + "In[187]:=",ExpressionUUID->"1a2cbc4e-fbe6-4f7c-8741-b70e886cb2db"], + +Cell[CellGroupData[{ + +Cell[BoxData[ + RowBox[{"Run", "[", + "\"\\"", + "]"}]], "Input", + CellChangeTimes->{{3.6050014532183523`*^9, 3.6050015701885967`*^9}, { + 3.605001619532137*^9, 3.6050016309343433`*^9}, {3.626084473225601*^9, + 3.626084492513814*^9}, {3.6260847134763527`*^9, 3.6260847142374268`*^9}}, + CellLabel-> + "In[221]:=",ExpressionUUID->"92a89388-7fc0-4900-84db-870a39602d88"], + +Cell[BoxData["0"], "Output", + CellChangeTimes->{ + 3.626084595698902*^9, {3.626084692883334*^9, 3.6260847202544928`*^9}, + 3.626085055644226*^9, {3.6266082860420322`*^9, 3.626608294517626*^9}, + 3.6266083422477207`*^9, 3.626608408034442*^9, 3.6279705231130753`*^9, + 3.627974194334207*^9, 3.627976022316907*^9, 3.627982078483562*^9, + 3.627983299927937*^9, 3.627992180265356*^9, 3.627992659732891*^9, + 3.62799287386307*^9, 3.6282290101624613`*^9, 3.6297105708954773`*^9, + 3.6297106574368258`*^9, 3.6298061279971113`*^9, 3.629807720408*^9, + 3.63032321558445*^9, 3.6324884540770473`*^9, 3.632488624588769*^9, + 3.632548341659026*^9, 3.632548980662763*^9, 3.632549207713517*^9, + 3.6325492403682756`*^9, 3.632571746113782*^9, 3.632635561755374*^9, + 3.632636954213489*^9, 3.6328206639342012`*^9, 3.633066575735487*^9, + 3.633067332266951*^9, 3.633235102950844*^9, 3.633327218459009*^9, + 3.633668547850634*^9, 3.633669240128911*^9, 3.6342783048055973`*^9, + 3.634278361480673*^9, 3.634287411095068*^9, 3.6342924066725883`*^9, + 3.634298448119008*^9, {3.634298529980817*^9, 3.6342985373529863`*^9}, + 3.634298597389635*^9, 3.634299652755254*^9, 3.634299691384879*^9, + 3.634299857136918*^9, 3.634299889682819*^9, 3.634299958854151*^9, + 3.63430011568889*^9, {3.634300182757251*^9, 3.634300204063946*^9}, + 3.634300381734619*^9, 3.634301695788665*^9, 3.634302340168003*^9, + 3.634303008624929*^9, 3.6343039186453753`*^9, 3.634452382385035*^9, + 3.634452775895094*^9, 3.634980290608531*^9, 3.634980867087916*^9, { + 3.634982332576049*^9, 3.6349823386672897`*^9}, 3.771327259597065*^9, + 3.771569957428474*^9, 3.7723647052058573`*^9, 3.7724326260823793`*^9, + 3.772432982619478*^9, 3.772946110952771*^9, 3.773142622983836*^9, { + 3.7741789490039988`*^9, 3.774178957149096*^9}, 3.774239768380513*^9, + 3.774266326673469*^9, 3.77458390783916*^9, 3.774612092999992*^9, + 3.774691158272262*^9, 3.7746926287703743`*^9, {3.775206430535994*^9, + 3.775206518604731*^9}, 3.7752066826065903`*^9, 3.775206790200211*^9, + 3.775206827144771*^9, 3.781531524359655*^9, {3.781532274516631*^9, + 3.781532281751*^9}, {3.782109514640354*^9, 3.782109522477334*^9}, + 3.784535070682267*^9, 3.784537809638708*^9, 3.784537843760861*^9, + 3.784537914040141*^9, 3.820073727931135*^9, 3.831544438892476*^9, + 3.831548150530548*^9, 3.831548464699*^9, 3.831550705400441*^9, + 3.831552346161853*^9, 3.831997320121353*^9, 3.8320364983368073`*^9, + 3.8320405504472303`*^9, 3.8320481854565268`*^9, 3.832120936612749*^9, + 3.832248835365497*^9, 3.832248994613755*^9, 3.8322493453949966`*^9, + 3.832249630129558*^9, 3.8322496829962463`*^9, 3.832251733072578*^9, + 3.8322518897639847`*^9, 3.832254017905024*^9, 3.832255272371408*^9, + 3.832294229867085*^9, 3.8322989730148478`*^9, 3.832299579509521*^9, + 3.832299852685203*^9, 3.832303925882906*^9, 3.8323040960864477`*^9, + 3.832304137826005*^9, 3.832304257502684*^9, 3.8323044191014023`*^9, + 3.832304481446743*^9, 3.832304675951449*^9, {3.832304763545319*^9, + 3.832304789622552*^9}, 3.832304824929778*^9, 3.832304923988615*^9, + 3.832304962537294*^9, 3.832305012211985*^9, 3.832305062931774*^9, + 3.8323051054980927`*^9, 3.8323051522702417`*^9, 3.832305279164638*^9, + 3.8323074136604967`*^9, 3.832308059399588*^9, 3.832308280940714*^9, + 3.8323085747629337`*^9, {3.8323098008411922`*^9, 3.832309811466663*^9}, + 3.8323102877592707`*^9, 3.832310372335705*^9, 3.832380987827281*^9, + 3.832492015402467*^9, 3.832494711222521*^9, 3.8325916840551567`*^9, + 3.832594977839239*^9, 3.832596188432527*^9, 3.832597321933096*^9, + 3.832599927189682*^9, 3.8326016650358753`*^9, 3.8326017555851994`*^9, + 3.832604197894318*^9, 3.832604354903805*^9, 3.832604522899815*^9, + 3.832604590922518*^9, 3.832605156804879*^9, 3.832643925746409*^9, + 3.8326440676357603`*^9, 3.832644306421803*^9, 3.832644423549642*^9, + 3.8326445938251762`*^9, 3.8326448171647577`*^9, 3.8326471670070143`*^9, + 3.832648809027214*^9, 3.832652788969205*^9, 3.832676414066802*^9, + 3.832676651369659*^9, 3.832676697888241*^9, 3.8326767356388683`*^9}, + CellLabel-> + "Out[221]=",ExpressionUUID->"72745ff6-29f5-42ab-98e8-dd16886e2d48"] +}, Open ]], + +Cell[BoxData[{ + RowBox[{ + RowBox[{"Run", "[", "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"Run", "[", "\"\\"", "]"}], ";"}]}], "Input", + CellChangeTimes->{{3.8322994472360992`*^9, 3.832299451746531*^9}, { + 3.832300807121028*^9, 3.832300808616116*^9}, {3.83230446185632*^9, + 3.832304469358658*^9}, {3.832304948135626*^9, 3.832304949742977*^9}}, + CellLabel-> + "In[222]:=",ExpressionUUID->"aaff244c-3ad5-4faf-aeb3-7cdbaa9f50f5"], + +Cell[BoxData[" "], "Input", + CellChangeTimes->{3.627992683770735*^9}, + CellLabel-> + "In[224]:=",ExpressionUUID->"8714cb77-63af-49bd-8987-722049f040d7"] +}, +CellGrouping->Manual, +WindowSize->{1386., 782.25}, +WindowMargins->{{Automatic, -1440}, {0, Automatic}}, +TaggingRules->{ + "WelcomeScreenSettings" -> {"FEStarting" -> False}, "TryRealOnly" -> False}, +Magnification:>0.9 Inherited, +FrontEndVersion->"12.2 for Linux x86 (64-bit) (December 12, 2020)", +StyleDefinitions->"Default.nb", +ExpressionUUID->"1d696320-bd25-4c6c-bc1e-4ec757dce194" +] +(* End of Notebook Content *) + +(* Internal cache information *) +(*CellTagsOutline +CellTagsIndex->{} +*) +(*CellTagsIndex +CellTagsIndex->{} +*) +(*NotebookFileOutline +Notebook[{ +Cell[558, 20, 220, 3, 28, "Input",ExpressionUUID->"28aebd44-7a23-4603-b370-38149a649cc1"], +Cell[781, 25, 554, 12, 48, "Input",ExpressionUUID->"d5e95da8-5675-4df2-8ff6-af7b8fb219a1"], +Cell[CellGroupData[{ +Cell[1360, 41, 242, 5, 46, "Subtitle",ExpressionUUID->"addb9730-72b2-4859-8e4b-0e776f073328", + InitializationCell->True], +Cell[1605, 48, 964, 30, 41, "Input",ExpressionUUID->"78e5840d-381c-47a9-b9dd-90c3c4dacb03", + InitializationCell->True], +Cell[2572, 80, 1654, 47, 43, "Input",ExpressionUUID->"66ac8851-0848-4866-8069-42c37f27b5de", + InitializationCell->True], +Cell[4229, 129, 944, 29, 62, "Input",ExpressionUUID->"a40aa98c-a7a2-4814-b63b-e7c90c9932a0", + InitializationCell->True], +Cell[5176, 160, 1652, 49, 41, "Input",ExpressionUUID->"ee9449fb-cd56-4179-b90c-f5e3bb0fdcdb", + InitializationCell->True], +Cell[6831, 211, 973, 25, 42, "Input",ExpressionUUID->"b69052f0-f728-4bd6-9bf7-bae15aa76095", + InitializationCell->True], +Cell[7807, 238, 1345, 40, 62, "Input",ExpressionUUID->"905c9ad7-8258-45b1-b64f-2d576f63cf20", + InitializationCell->True], +Cell[9155, 280, 2510, 65, 181, "Input",ExpressionUUID->"a7b9df83-8379-4a26-b658-cd16a2c680e3", + InitializationCell->True] +}, Open ]], +Cell[11680, 348, 188, 3, 89, "Title",ExpressionUUID->"243eeb2a-7eb6-46c5-afb4-9166e290b8da"], +Cell[11871, 353, 156, 3, 29, "Subsubtitle",ExpressionUUID->"934e8056-77ed-401a-af0c-38f7a1beea69"], +Cell[12030, 358, 590, 12, 28, "Input",ExpressionUUID->"89e76b07-ca4f-4879-a9de-3406cd562327"], +Cell[12623, 372, 2126, 56, 177, "Input",ExpressionUUID->"0f88cdc1-a39e-4d13-b528-8472e029b05f"], +Cell[14752, 430, 159, 3, 29, "Subsubtitle",ExpressionUUID->"a1d3915d-f613-4425-8a04-fe27c5187298"], +Cell[14914, 435, 8700, 220, 927, "Input",ExpressionUUID->"33f47236-ad3c-4a61-8f93-8222aa6cc9fa"], +Cell[23617, 657, 4436, 127, 397, "Input",ExpressionUUID->"53a40cfc-f0e8-4e44-9f25-b052baa99c29"], +Cell[28056, 786, 482, 13, 28, "Input",ExpressionUUID->"230fd034-8fee-4db7-94bf-9c0c77325dce"], +Cell[28541, 801, 1691, 48, 142, "Input",ExpressionUUID->"3db40cdd-aae8-4aed-bbcf-7b6c917acc34"], +Cell[30235, 851, 287, 5, 29, "Subsubtitle",ExpressionUUID->"3a22e419-d65c-4c9e-84c6-863e409e519a"], +Cell[30525, 858, 208, 4, 31, "Text",ExpressionUUID->"9ac0d911-0e84-4579-bcb9-2110f48ca902"], +Cell[30736, 864, 2112, 48, 65, "Input",ExpressionUUID->"9f81cfa6-a749-4450-bfb3-736066b50bce"], +Cell[32851, 914, 154, 3, 31, "Text",ExpressionUUID->"656d798c-a5c9-4cc6-a1dd-06c8dc6c6565"], +Cell[33008, 919, 2512, 53, 121, "Input",ExpressionUUID->"29b01d2f-83ef-4d5a-8a25-69d70c2e9207"], +Cell[35523, 974, 157, 3, 31, "Text",ExpressionUUID->"7aa4501a-e764-4f85-8410-73dc94387049"], +Cell[35683, 979, 2506, 60, 48, "Input",ExpressionUUID->"dbea6cef-0336-433e-92e3-cc59b4a69219"], +Cell[38192, 1041, 178, 3, 31, "Text",ExpressionUUID->"15e73123-e84e-4360-b55b-334ef918a821"], +Cell[38373, 1046, 2059, 51, 83, "Input",ExpressionUUID->"176642b9-ef21-428e-bda6-88b3064f0fe2"], +Cell[40435, 1099, 152, 3, 31, "Text",ExpressionUUID->"ac9144ba-14f4-4cb2-9d7c-1b84aa717d7b"], +Cell[40590, 1104, 717, 17, 46, "Input",ExpressionUUID->"f4931c36-9cb3-41d5-9636-f3fbcef428c9"], +Cell[41310, 1123, 154, 3, 31, "Text",ExpressionUUID->"57ce017d-559b-4d58-ba18-1e7631c436d9"], +Cell[41467, 1128, 411, 10, 46, "Input",ExpressionUUID->"63203ee0-9601-47b1-9606-b67b10ce9a46"], +Cell[41881, 1140, 156, 3, 31, "Text",ExpressionUUID->"4432b235-bb2b-4e9d-87f8-55bf93c648df"], +Cell[42040, 1145, 593, 14, 46, "Input",ExpressionUUID->"c73ee33c-6f07-4ea9-82e2-0f37fad73c8e"], +Cell[42636, 1161, 153, 3, 31, "Text",ExpressionUUID->"3b78e61b-a31c-43c6-949c-8ab46a558347"], +Cell[42792, 1166, 1726, 42, 83, "Input",ExpressionUUID->"7be4a0a1-b2c0-4d7b-b564-9f92dabf375f"], +Cell[44521, 1210, 154, 3, 31, "Text",ExpressionUUID->"78b600c8-17c9-43b2-8561-39fdff7b92e4"], +Cell[44678, 1215, 1204, 28, 65, "Input",ExpressionUUID->"4ce3e11d-b2d8-42df-94f3-f44671feb140"], +Cell[45885, 1245, 154, 3, 31, "Text",ExpressionUUID->"c302290e-b2df-4299-8d3e-f59d1fc850b6"], +Cell[46042, 1250, 740, 16, 46, "Input",ExpressionUUID->"1a28b43d-e9a2-4ebf-8251-f3ab2ddb7786"], +Cell[46785, 1268, 208, 4, 31, "Text",ExpressionUUID->"4ba37bc9-3ca4-4415-be72-e388f0dad47a"], +Cell[46996, 1274, 1571, 37, 102, "Input",ExpressionUUID->"a2375489-1864-4b87-9537-7325ce23183f"], +Cell[48570, 1313, 2610, 48, 46, "Input",ExpressionUUID->"fe419f0a-adf5-43af-9f08-7b8a7f19180b"], +Cell[51183, 1363, 1000, 17, 65, "Input",ExpressionUUID->"ff5f7d73-add6-4e24-9e55-7b15786660c6"], +Cell[52186, 1382, 2189, 55, 65, "Input",ExpressionUUID->"d17df8c1-9260-4a0c-8495-b5ef6f820848"], +Cell[54378, 1439, 847, 19, 46, "Input",ExpressionUUID->"8c91f72d-3f47-4547-a792-5e0813bcc352"], +Cell[55228, 1460, 204, 3, 29, "Subsubtitle",ExpressionUUID->"b92b1bb0-abe4-4170-8cfc-1e9dd8bc5679"], +Cell[55435, 1465, 956, 25, 83, "Input",ExpressionUUID->"dea35a84-3a0f-4600-a2f4-2ab045608d87"], +Cell[56394, 1492, 154, 3, 29, "Subsubtitle",ExpressionUUID->"bf1afef8-2e19-4e8e-9c26-8b9e596cd10c"], +Cell[56551, 1497, 206, 5, 28, "Input",ExpressionUUID->"c2f98c75-5faf-44c8-a958-147c2a660678"], +Cell[CellGroupData[{ +Cell[56782, 1506, 4596, 114, 233, "Input",ExpressionUUID->"716ee195-4767-4067-92df-43ed8da3bd15"], +Cell[61381, 1622, 1326, 21, 30, "Output",ExpressionUUID->"1bcc3553-7aaf-4cd1-b8d1-178a830567d9"] +}, Open ]], +Cell[62722, 1646, 3894, 118, 365, "Input",ExpressionUUID->"cdb0dbd5-e787-49a0-bb76-c4468b401cbc"], +Cell[66619, 1766, 243, 5, 26, "Input",ExpressionUUID->"41b82835-d8e1-446a-bec9-5cabfad38221"], +Cell[66865, 1773, 7935, 202, 583, "Input",ExpressionUUID->"cbb713d0-b847-42c0-9c5d-8772380d8dba"], +Cell[74803, 1977, 6922, 215, 760, "Input",ExpressionUUID->"1a2cbc4e-fbe6-4f7c-8741-b70e886cb2db"], +Cell[CellGroupData[{ +Cell[81750, 2196, 427, 8, 28, "Input",ExpressionUUID->"92a89388-7fc0-4900-84db-870a39602d88"], +Cell[82180, 2206, 4217, 60, 30, "Output",ExpressionUUID->"72745ff6-29f5-42ab-98e8-dd16886e2d48"] +}, Open ]], +Cell[86412, 2269, 486, 10, 46, "Input",ExpressionUUID->"aaff244c-3ad5-4faf-aeb3-7cdbaa9f50f5"], +Cell[86901, 2281, 152, 3, 57, "Input",ExpressionUUID->"8714cb77-63af-49bd-8987-722049f040d7"] +} +] +*) + diff --git a/_development/Mathematica/models/AugmentedState_PlaneDynamicModel_calculated_prev.nb b/_development/Mathematica/models/AugmentedState_PlaneDynamicModel_calculated_prev.nb new file mode 100644 index 0000000..fc3eb93 --- /dev/null +++ b/_development/Mathematica/models/AugmentedState_PlaneDynamicModel_calculated_prev.nb @@ -0,0 +1,2505 @@ +(* Content-type: application/vnd.wolfram.mathematica *) + +(*** Wolfram Notebook File ***) +(* http://www.wolfram.com/nb *) + +(* CreatedBy='Mathematica 10.0' *) + +(*CacheID: 234*) +(* Internal cache information: +NotebookFileLineBreakTest +NotebookFileLineBreakTest +NotebookDataPosition[ 158, 7] +NotebookDataLength[ 99908, 2497] +NotebookOptionsPosition[ 92593, 2394] +NotebookOutlinePosition[ 93139, 2414] +CellTagsIndexPosition[ 93096, 2411] +WindowFrame->Normal*) + +(* Beginning of Notebook Content *) +Notebook[{ +Cell[BoxData[ + RowBox[{"Clear", "[", "\"\\"", "]"}]], "Input", + CellChangeTimes->{{3.5951624068865957`*^9, 3.595162434106801*^9}}, + CellLabel->"In[11]:=",ExpressionUUID->"28aebd44-7a23-4603-b370-38149a649cc1"], + +Cell[BoxData[{ + RowBox[{"Import", "[", + RowBox[{ + RowBox[{"NotebookDirectory", "[", "]"}], "<>", + "\"\\""}], "]"}], "\[IndentingNewLine]", + RowBox[{"Import", "[", + RowBox[{ + RowBox[{"NotebookDirectory", "[", "]"}], "<>", + "\"\\""}], "]"}]}], "Input", + CellChangeTimes->{{3.595163228510494*^9, 3.595163230854044*^9}, { + 3.6175174204357758`*^9, 3.617517424343709*^9}, {3.632487617988913*^9, + 3.632487633951275*^9}}, + CellLabel->"In[12]:=",ExpressionUUID->"d5e95da8-5675-4df2-8ff6-af7b8fb219a1"], + +Cell[CellGroupData[{ + +Cell["Quaternion Functions", "Subtitle", + InitializationCell->True, + CellChangeTimes->{{3.563606408899285*^9, 3.563606413402224*^9}, { + 3.572951250149337*^9, + 3.572951255936389*^9}},ExpressionUUID->"addb9730-72b2-4859-8e4b-\ +0e776f073328"], + +Cell[BoxData[ + RowBox[{ + RowBox[{"VectProdMatrix", "[", "v_", "]"}], ":=", + RowBox[{"{", + RowBox[{ + RowBox[{"{", + RowBox[{"0", ",", + RowBox[{"-", + RowBox[{"v", "[", + RowBox[{"[", "3", "]"}], "]"}]}], ",", + RowBox[{"v", "[", + RowBox[{"[", "2", "]"}], "]"}]}], "}"}], ",", + RowBox[{"{", + RowBox[{ + RowBox[{"v", "[", + RowBox[{"[", "3", "]"}], "]"}], ",", "0", ",", + RowBox[{"-", + RowBox[{"v", "[", + RowBox[{"[", "1", "]"}], "]"}]}]}], "}"}], ",", + RowBox[{"{", + RowBox[{ + RowBox[{"-", + RowBox[{"v", "[", + RowBox[{"[", "2", "]"}], "]"}]}], ",", + RowBox[{"v", "[", + RowBox[{"[", "1", "]"}], "]"}], ",", "0"}], "}"}]}], "}"}]}]], "Input",\ + + InitializationCell->True, + CellChangeTimes->{{3.563607037606062*^9, 3.563607124416689*^9}, + 3.563608231083735*^9}, + CellLabel->"In[14]:=",ExpressionUUID->"78e5840d-381c-47a9-b9dd-90c3c4dacb03"], + +Cell[BoxData[ + RowBox[{ + RowBox[{"PertStateSubs", "[", + RowBox[{"X_", ",", "dX_"}], "]"}], ":=", + RowBox[{"Flatten", "[", + RowBox[{"{", + RowBox[{ + RowBox[{"Table", "[", + RowBox[{ + RowBox[{ + RowBox[{"X", "[", + RowBox[{"[", "i", "]"}], "]"}], "\[Rule]", + RowBox[{ + RowBox[{"X", "[", + RowBox[{"[", "i", "]"}], "]"}], "+", + RowBox[{"dX", "[", + RowBox[{"[", "i", "]"}], "]"}]}]}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", "3"}], "}"}]}], "]"}], ",", + RowBox[{"Table", "[", + RowBox[{ + RowBox[{ + RowBox[{"X", "[", + RowBox[{"[", + RowBox[{"i", "+", "3"}], "]"}], "]"}], "\[Rule]", + RowBox[{ + RowBox[{"(", + RowBox[{"QuatInc", "[", + RowBox[{ + RowBox[{"X", "[", + RowBox[{"[", + RowBox[{"{", + RowBox[{"4", ",", "5", ",", "6", ",", "7"}], "}"}], "]"}], + "]"}], ",", + RowBox[{"dX", "[", + RowBox[{"[", + RowBox[{"{", + RowBox[{"4", ",", "5", ",", "6"}], "}"}], "]"}], "]"}]}], + "]"}], ")"}], "[", + RowBox[{"[", "i", "]"}], "]"}]}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", "4"}], "}"}]}], "]"}]}], "}"}], + "]"}]}]], "Input", + InitializationCell->True, + CellChangeTimes->{{3.571130663264105*^9, 3.571130765329321*^9}, { + 3.57113111793888*^9, 3.57113116341093*^9}, {3.571131195958424*^9, + 3.571131254626845*^9}, 3.571133049520565*^9}, + CellLabel->"In[15]:=",ExpressionUUID->"66ac8851-0848-4866-8069-42c37f27b5de"], + +Cell[BoxData[{ + RowBox[{ + RowBox[{ + RowBox[{"L2", "[", "x_", "]"}], ":=", + RowBox[{"Sqrt", "[", + RowBox[{"Sum", "[", + RowBox[{ + RowBox[{ + RowBox[{"x", "[", + RowBox[{"[", "j", "]"}], "]"}], "^", "2"}], ",", + RowBox[{"{", + RowBox[{"j", ",", "1", ",", + RowBox[{"Length", "[", "x", "]"}]}], "}"}]}], "]"}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"VecNorm", "[", "x_", "]"}], ":=", + RowBox[{"Table", "[", + RowBox[{ + RowBox[{ + RowBox[{"x", "[", + RowBox[{"[", "i", "]"}], "]"}], "/", + RowBox[{"L2", "[", "x", "]"}]}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", + RowBox[{"Length", "[", "x", "]"}]}], "}"}]}], "]"}]}], ";"}]}], "Input",\ + + InitializationCell->True, + CellChangeTimes->{{3.568453075302482*^9, 3.568453143924776*^9}}, + CellLabel->"In[16]:=",ExpressionUUID->"a40aa98c-a7a2-4814-b63b-e7c90c9932a0"], + +Cell[BoxData[ + RowBox[{ + RowBox[{ + RowBox[{"SkewMatrix", "[", "v_", "]"}], ":=", + RowBox[{ + RowBox[{"{", + RowBox[{ + RowBox[{"{", + RowBox[{"0", ",", + RowBox[{"-", + RowBox[{"v", "[", + RowBox[{"[", "1", "]"}], "]"}]}], ",", + RowBox[{"-", + RowBox[{"v", "[", + RowBox[{"[", "2", "]"}], "]"}]}], ",", + RowBox[{"-", + RowBox[{"v", "[", + RowBox[{"[", "3", "]"}], "]"}]}]}], "}"}], ",", + RowBox[{"{", + RowBox[{ + RowBox[{"v", "[", + RowBox[{"[", "1", "]"}], "]"}], ",", "0", ",", + RowBox[{"v", "[", + RowBox[{"[", "3", "]"}], "]"}], ",", + RowBox[{"-", + RowBox[{"v", "[", + RowBox[{"[", "2", "]"}], "]"}]}]}], "}"}], ",", + RowBox[{"{", + RowBox[{ + RowBox[{"v", "[", + RowBox[{"[", "2", "]"}], "]"}], ",", + RowBox[{"-", + RowBox[{"v", "[", + RowBox[{"[", "3", "]"}], "]"}]}], ",", "0", ",", + RowBox[{"v", "[", + RowBox[{"[", "1", "]"}], "]"}]}], "}"}], ",", + RowBox[{"{", + RowBox[{ + RowBox[{"v", "[", + RowBox[{"[", "3", "]"}], "]"}], ",", + RowBox[{"v", "[", + RowBox[{"[", "2", "]"}], "]"}], ",", + RowBox[{"-", + RowBox[{"v", "[", + RowBox[{"[", "1", "]"}], "]"}]}], ",", "0"}], "}"}]}], "}"}], "/", + "2"}]}], ";"}]], "Input", + InitializationCell->True, + CellChangeTimes->{{3.568443187261707*^9, 3.568443273520089*^9}, { + 3.568458568630984*^9, 3.568458568967643*^9}}, + CellLabel->"In[18]:=",ExpressionUUID->"ee9449fb-cd56-4179-b90c-f5e3bb0fdcdb"], + +Cell[BoxData[ + RowBox[{ + RowBox[{ + RowBox[{"ConstantAngularSpeeDtMatrix", "[", + RowBox[{"w_", ",", "Dt_"}], "]"}], ":=", + RowBox[{ + RowBox[{ + RowBox[{"Cos", "[", + RowBox[{ + RowBox[{"L2", "[", "w", "]"}], + RowBox[{"Dt", "/", "2"}]}], "]"}], " ", + RowBox[{"IdentityMatrix", "[", "4", "]"}]}], "+", + RowBox[{"2", + RowBox[{ + RowBox[{"Sin", "[", + RowBox[{ + RowBox[{"L2", "[", "w", "]"}], + RowBox[{"Dt", "/", "2"}]}], "]"}], "/", + RowBox[{"L2", "[", "w", "]"}]}], " ", + RowBox[{"SkewMatrix", "[", "w", "]"}]}]}]}], ";"}]], "Input", + InitializationCell->True, + CellChangeTimes->{{3.5684429732904*^9, 3.56844300516892*^9}, { + 3.568443103374373*^9, 3.568443183239631*^9}, {3.56844338461282*^9, + 3.568443409373211*^9}, {3.568453008511107*^9, 3.568453014783046*^9}, { + 3.60835655838758*^9, 3.608356558395439*^9}}, + CellLabel->"In[19]:=",ExpressionUUID->"b69052f0-f728-4bd6-9bf7-bae15aa76095"], + +Cell[BoxData[{ + RowBox[{ + RowBox[{ + RowBox[{"SubVec", "[", + RowBox[{"exp_", ",", "v1_", ",", "v2_"}], "]"}], ":=", + RowBox[{"exp", "/.", + RowBox[{"Table", "[", + RowBox[{ + RowBox[{ + RowBox[{"v1", "[", + RowBox[{"[", "i", "]"}], "]"}], "\[Rule]", + RowBox[{"v2", "[", + RowBox[{"[", "i", "]"}], "]"}]}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", + RowBox[{ + RowBox[{"Dimensions", "[", + RowBox[{"v1", ",", "1"}], "]"}], "[", + RowBox[{"[", "1", "]"}], "]"}]}], "}"}]}], "]"}]}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"VecToZero", "[", + RowBox[{"exp_", ",", "v_"}], "]"}], ":=", + RowBox[{"exp", "/.", + RowBox[{"Table", "[", + RowBox[{ + RowBox[{ + RowBox[{"v", "[", + RowBox[{"[", "i", "]"}], "]"}], "\[Rule]", "0"}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", + RowBox[{ + RowBox[{"Dimensions", "[", + RowBox[{"v", ",", "1"}], "]"}], "[", + RowBox[{"[", "1", "]"}], "]"}]}], "}"}]}], "]"}]}]}], + ";"}]}], "Input", + InitializationCell->True, + CellChangeTimes->{{3.568444740747602*^9, 3.568444776506934*^9}, { + 3.568444838483142*^9, 3.568444844345901*^9}}, + CellLabel->"In[20]:=",ExpressionUUID->"905c9ad7-8258-45b1-b64f-2d576f63cf20"], + +Cell[BoxData[{ + RowBox[{ + RowBox[{"ToGoodC", "[", "exp_", "]"}], ":=", + RowBox[{"Module", "[", + RowBox[{ + RowBox[{"{", "oexp", "}"}], ",", " ", + RowBox[{ + RowBox[{"oexp", "=", + RowBox[{"Experimental`OptimizeExpression", "[", "exp", "]"}]}], ";", + "\[IndentingNewLine]", + RowBox[{"If", "[", + RowBox[{ + RowBox[{ + RowBox[{ + RowBox[{"Dimensions", " ", "[", + RowBox[{ + RowBox[{"StringPosition", "[", + RowBox[{ + RowBox[{"ToString", "[", + RowBox[{"InputForm", "[", "oexp", "]"}], "]"}], ",", + "\"\\""}], "]"}], ",", "1"}], "]"}], "[", + RowBox[{"[", "1", "]"}], "]"}], ">", "0"}], ",", + RowBox[{"Block", "[", + RowBox[{ + RowBox[{"{", " ", + RowBox[{"locals", ",", " ", "code"}], "}"}], ",", + RowBox[{ + RowBox[{ + RowBox[{"{", + RowBox[{"locals", ",", "code"}], "}"}], "=", + RowBox[{"ReleaseHold", "[", + RowBox[{ + RowBox[{"(", + RowBox[{"Hold", "@@", "oexp"}], ")"}], "/.", + RowBox[{ + RowBox[{ + RowBox[{"Verbatim", "[", "Block", "]"}], "[", + RowBox[{"vars_", ",", "seq_"}], "]"}], "\[RuleDelayed]", + RowBox[{"{", + RowBox[{"vars", ",", + RowBox[{"Hold", "[", "seq", "]"}]}], "}"}]}]}], "]"}]}], ";", + + RowBox[{"ToString", "[", + RowBox[{"CForm", "[", "code", "]"}], "]"}]}]}], "]"}], + "\[IndentingNewLine]", ",", " ", + RowBox[{"ToString", "[", + RowBox[{"CForm", "[", "exp", "]"}], "]"}]}], "]"}]}]}], + "]"}]}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{"str_", ",", "file_"}], "]"}], ":=", + RowBox[{"Module", "[", + RowBox[{ + RowBox[{"{", "stream", "}"}], ",", + RowBox[{ + RowBox[{"stream", "=", + RowBox[{"OpenWrite", "[", "file", "]"}]}], ";", + RowBox[{"WriteString", "[", + RowBox[{"stream", ",", "str"}], "]"}], ";", + RowBox[{"Close", "[", "stream", "]"}], ";"}]}], "]"}]}]}], "Input", + InitializationCell->True, + CellChangeTimes->{ + 3.568440350374321*^9, {3.56844481385726*^9, 3.56844481884285*^9}, { + 3.568446816971497*^9, 3.568446831371454*^9}, {3.56844689667688*^9, + 3.568446897039928*^9}, {3.571133081827365*^9, 3.571133085326789*^9}}, + CellLabel->"In[22]:=",ExpressionUUID->"a7b9df83-8379-4a26-b658-cd16a2c680e3"] +}, Open ]], + +Cell["Beginning of stuff", "Title", + CellChangeTimes->{{3.5951624112710238`*^9, 3.5951624314967413`*^9}, + 3.595309863589487*^9},ExpressionUUID->"243eeb2a-7eb6-46c5-afb4-\ +9166e290b8da"], + +Cell["State", "Subsubtitle", + CellChangeTimes->{{3.5732982476551723`*^9, + 3.573298257964068*^9}},ExpressionUUID->"934e8056-77ed-401a-af0c-\ +38f7a1beea69"], + +Cell[BoxData[ + RowBox[{ + RowBox[{"X", "=", + RowBox[{"Table", "[", + RowBox[{ + RowBox[{"x", "[", "i", "]"}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", "26"}], "}"}]}], "]"}]}], ";"}]], "Input", + CellChangeTimes->{{3.5732982634483347`*^9, 3.573298275094433*^9}, { + 3.606108793708241*^9, 3.6061087941202383`*^9}, {3.832299724443674*^9, + 3.8322997253433037`*^9}, {3.8325883987514553`*^9, 3.832588399422798*^9}}, + CellLabel->"In[24]:=",ExpressionUUID->"89e76b07-ca4f-4879-a9de-3406cd562327"], + +Cell[BoxData[{ + RowBox[{ + RowBox[{"SW", "=", + RowBox[{"X", "[", + RowBox[{"[", + RowBox[{"1", ";;", "3"}], "]"}], "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"QWS", "=", + RowBox[{"X", "[", + RowBox[{"[", + RowBox[{"4", ";;", "7"}], "]"}], "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"VS", "=", + RowBox[{"X", "[", + RowBox[{"[", + RowBox[{"8", ";;", "10"}], "]"}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"WS", "=", + RowBox[{"X", "[", + RowBox[{"[", + RowBox[{"11", ";;", "13"}], "]"}], "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"AS", "=", + RowBox[{"X", "[", + RowBox[{"[", + RowBox[{"14", ";;", "16"}], "]"}], "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"ALPHAS", "=", + RowBox[{"X", "[", + RowBox[{"[", + RowBox[{"17", ";;", "19"}], "]"}], "]"}]}], ";"}]}], "Input", + CellChangeTimes->{{3.632463823621646*^9, 3.632463843454915*^9}, { + 3.632463896430626*^9, 3.632463897928619*^9}, {3.632464021491871*^9, + 3.632464039150372*^9}, {3.6324668182980833`*^9, 3.6324668268371*^9}, { + 3.63248479427204*^9, 3.632484802957326*^9}, {3.6324860670961533`*^9, + 3.6324860672576303`*^9}, 3.6324863191229258`*^9, {3.632487946501395*^9, + 3.6324879678038177`*^9}, {3.632488008725754*^9, 3.632488019377554*^9}, { + 3.832298839476634*^9, 3.832298840668848*^9}, {3.832299719300475*^9, + 3.832299722434368*^9}}, + CellLabel->"In[25]:=",ExpressionUUID->"0f88cdc1-a39e-4d13-b528-8472e029b05f"], + +Cell["Previous State", "Text", + CellChangeTimes->{{3.83224646016693*^9, 3.8322464648387337`*^9}, { + 3.832251510394771*^9, + 3.832251511907185*^9}},ExpressionUUID->"e1331a56-c19e-47fe-a1a5-\ +206a0bc812e0"], + +Cell[CellGroupData[{ + +Cell[BoxData[ + RowBox[{"dtVdm", "=", "0.001"}]], "Input", + CellChangeTimes->{{3.832251515979452*^9, 3.832251519915113*^9}, + 3.8322515709908667`*^9, {3.8322516846036997`*^9, 3.8322516882998943`*^9}, { + 3.832380832890884*^9, 3.832380832989848*^9}, {3.832587009657296*^9, + 3.832587010168441*^9}, 3.832588475589563*^9, 3.832588570963978*^9, { + 3.832594955033228*^9, 3.83259495518712*^9}, {3.8325972811811*^9, + 3.832597281436166*^9}, {3.8325998890218782`*^9, 3.832599889180036*^9}, { + 3.832647119782226*^9, 3.83264711988409*^9}, {3.8326501185036907`*^9, + 3.832650118736822*^9}},ExpressionUUID->"c3fc8240-75b9-4e9c-b9f8-\ +94b7f5b549ab"], + +Cell[BoxData["0.004`"], "Output", + CellChangeTimes->{ + 3.832251692729765*^9, 3.832251846613799*^9, 3.832253820396224*^9, + 3.832255137920088*^9, 3.832294101362227*^9, 3.832298850463367*^9, + 3.832299457348761*^9, 3.832299729912161*^9, 3.832299991063039*^9, + 3.83230083251895*^9, 3.832301799343338*^9, 3.8323038018864813`*^9, + 3.8323040860785723`*^9, 3.8323041288448067`*^9, 3.832304248817923*^9, + 3.832304410265428*^9, 3.832304472616316*^9, 3.832304674703714*^9, + 3.832304754831437*^9, 3.832304788370494*^9, 3.832304822974585*^9, + 3.832304915172968*^9, 3.8323049536373367`*^9, 3.832305008506526*^9, + 3.832305059225812*^9, 3.832305103475458*^9, 3.832305149714987*^9, + 3.832305254333*^9, 3.832307387185625*^9, 3.832308033096203*^9, + 3.83230824960973*^9, 3.832308547136591*^9, {3.832309790464469*^9, + 3.832309801009409*^9}, 3.83231027582181*^9, 3.832310360250066*^9, + 3.832380971880837*^9, 3.832491984053829*^9, 3.8324946994120617`*^9, + 3.8325916723754053`*^9, 3.832594965718528*^9, 3.832596175557684*^9, + 3.8325973027301893`*^9, 3.8325999067948217`*^9, 3.8326016187974997`*^9, + 3.832601708671194*^9, 3.832604189697432*^9, 3.83260432834092*^9, + 3.832604491451466*^9, 3.832604560059162*^9, 3.832605137965205*^9, + 3.83264387844909*^9, 3.832644020381259*^9, 3.832644260900868*^9, + 3.83264437675336*^9, 3.832644546186769*^9, 3.832644770054124*^9, + 3.8326471230014153`*^9, 3.832648764025972*^9}, + CellLabel->"Out[31]=",ExpressionUUID->"d349f066-e80e-4084-98af-ba9e3cf8e0e5"] +}, Open ]], + +Cell[BoxData[ + StyleBox[ + RowBox[{"(*", + RowBox[{ + RowBox[{"QWSprevLong", " ", "=", " ", + RowBox[{"QuatBoxPlusRight", "[", + RowBox[{"QWS", ",", " ", + RowBox[{ + RowBox[{"-", "WS"}], " ", "*", " ", "dtVdm"}]}], "]"}]}], ";", + "\[IndentingNewLine]", + RowBox[{"QWSprev", "=", + RowBox[{"SimplifyQLite", "[", + RowBox[{"QWSprevLong", ",", + RowBox[{"{", "QWS", "}"}], ",", + RowBox[{"Join", "[", + RowBox[{ + RowBox[{"AreReals", "[", + RowBox[{"{", + RowBox[{"WS", ",", "dtVdm"}], "}"}], "]"}], ",", + RowBox[{"{", + RowBox[{"dtVdm", ">", "0"}], "}"}]}], "]"}]}], "]"}]}], ";"}], + "*)"}], + FontSize->14]], "Input", + CellChangeTimes->{{3.832246541364873*^9, 3.8322466082067337`*^9}, { + 3.832246726707139*^9, 3.832246727622882*^9}, {3.83224693062007*^9, + 3.83224693388627*^9}, 3.832251569004861*^9, 3.832255123275659*^9, { + 3.8323084790763197`*^9, 3.832308487536894*^9}, 3.832308523299737*^9, { + 3.832491945936014*^9, 3.832491957348793*^9}, {3.832494657193437*^9, + 3.8324946650778847`*^9}, {3.832588479754148*^9, 3.832588480801413*^9}, { + 3.832588571599259*^9, 3.832588572560486*^9}, 3.832598732741156*^9, { + 3.832601571236545*^9, 3.83260159390937*^9}, 3.83260168867662*^9, { + 3.8326040667929993`*^9, 3.832604088336248*^9}, 3.832643855485198*^9}, + CellLabel->"In[32]:=",ExpressionUUID->"26067934-b88a-41ed-a094-38971131318d"], + +Cell[BoxData[ + RowBox[{"(*", + RowBox[{ + RowBox[{"VSprevLong", " ", "=", " ", + RowBox[{ + RowBox[{"QuatToRot", "[", + RowBox[{"QuatInv", "[", "QWSprev", "]"}], "]"}], ".", + RowBox[{"QuatToRot", "[", "QWS", "]"}], ".", + RowBox[{"(", + RowBox[{"VS", " ", "-", " ", + RowBox[{"AS", " ", "*", " ", "dtVdm"}]}], ")"}]}]}], ";", + "\[IndentingNewLine]", + RowBox[{"VSprev", "=", + RowBox[{"SimplifyQLite", "[", + RowBox[{"VSprevLong", ",", + RowBox[{"{", + RowBox[{"QWSprev", ",", "QWS"}], "}"}], ",", + RowBox[{"Join", "[", + RowBox[{ + RowBox[{"AreReals", "[", + RowBox[{"{", + RowBox[{"VS", ",", "AS", ",", "dtVdm"}], "}"}], "]"}], ",", + RowBox[{"{", + RowBox[{"dtVdm", ">", "0"}], "}"}]}], "]"}]}], "]"}]}], ";"}], + "*)"}]], "Input", + CellChangeTimes->{{3.8322466246992292`*^9, 3.832246660421042*^9}, { + 3.8322467069108057`*^9, 3.8322467600065536`*^9}, {3.832249149341036*^9, + 3.8322491531321793`*^9}, {3.832249439949196*^9, 3.832249470930667*^9}, + 3.8322495309657717`*^9, 3.832251571059222*^9, 3.8322551311312847`*^9, { + 3.832308500188497*^9, 3.832308525851651*^9}, {3.832491963874091*^9, + 3.8324919748619013`*^9}, {3.832494669855976*^9, 3.832494675590197*^9}, { + 3.832588481408012*^9, 3.832588482307536*^9}, {3.8325885730748043`*^9, + 3.832588573999551*^9}, {3.832601581092298*^9, 3.8326015971014233`*^9}, + 3.83260169149759*^9, {3.832604070216125*^9, 3.83260409324842*^9}, + 3.8326438591275873`*^9}, + CellLabel->"In[33]:=",ExpressionUUID->"7e8141a9-cc68-4d70-a634-12c34f72cfd4"], + +Cell[BoxData[ + RowBox[{"(*", + RowBox[{ + RowBox[{"WSprevLong", " ", "=", " ", + RowBox[{ + RowBox[{"QuatToRot", "[", + RowBox[{"QuatInv", "[", "QWSprev", "]"}], "]"}], ".", + RowBox[{"QuatToRot", "[", "QWS", "]"}], ".", + RowBox[{"(", + RowBox[{"WS", " ", "-", " ", + RowBox[{"ALPHAS", " ", "*", "dtVdm"}]}], ")"}]}]}], ";", + "\[IndentingNewLine]", + RowBox[{"WSprev", "=", + RowBox[{"SimplifyQLite", "[", + RowBox[{"WSprevLong", ",", + RowBox[{"{", + RowBox[{"QWSprev", ",", "QWS"}], "}"}], ",", + RowBox[{"Join", "[", + RowBox[{ + RowBox[{"AreReals", "[", + RowBox[{"{", + RowBox[{"WS", ",", "ALPHAS", ",", "dtVdm"}], "}"}], "]"}], ",", + RowBox[{"{", + RowBox[{"dtVdm", ">", "0"}], "}"}]}], "]"}]}], "]"}]}], ";"}], + "*)"}]], "Input", + CellChangeTimes->{{3.8322468363661127`*^9, 3.832246867390759*^9}, + 3.832249168853107*^9, 3.8322495535087442`*^9, 3.8322515710606003`*^9, + 3.832255132819573*^9, {3.832308511268517*^9, 3.832308519796136*^9}, { + 3.832491966882955*^9, 3.8324919795729094`*^9}, {3.832494681167602*^9, + 3.832494684548196*^9}, {3.832588482962741*^9, 3.832588484033202*^9}, { + 3.832588574522708*^9, 3.8325885755083323`*^9}, {3.8326015895891438`*^9, + 3.8326016022935*^9}, 3.832601694670414*^9, {3.832604074000885*^9, + 3.832604098856386*^9}, {3.832604444017585*^9, 3.832604473387764*^9}, + 3.832643862417542*^9}, + CellLabel->"In[34]:=",ExpressionUUID->"dba75c2f-02b5-4068-8352-432d2c9ea9a3"], + +Cell[CellGroupData[{ + +Cell[BoxData[{ + RowBox[{"$MinPrecision", " ", "=", " ", "20"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"QWSprev", " ", "=", " ", + RowBox[{"QuatBoxPlusRight", "[", + RowBox[{"QWS", ",", " ", + RowBox[{ + RowBox[{"-", "WS"}], " ", "*", " ", "dtVdm"}]}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"VSprev", " ", "=", " ", + RowBox[{ + RowBox[{"QuatToRot", "[", + RowBox[{"QuatInv", "[", "QWSprev", "]"}], "]"}], ".", + RowBox[{"QuatToRot", "[", "QWS", "]"}], ".", + RowBox[{"(", + RowBox[{"VS", " ", "-", " ", + RowBox[{"AS", " ", "*", " ", "dtVdm"}]}], ")"}]}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"WSprev", " ", "=", " ", + RowBox[{ + RowBox[{"QuatToRot", "[", + RowBox[{"QuatInv", "[", "QWSprev", "]"}], "]"}], ".", + RowBox[{"QuatToRot", "[", "QWS", "]"}], ".", + RowBox[{"(", + RowBox[{"WS", " ", "-", " ", + RowBox[{"ALPHAS", " ", "*", "dtVdm"}]}], ")"}]}]}], ";"}]}], "Input", + CellChangeTimes->{{3.832643822473324*^9, 3.832643834846509*^9}, { + 3.832648699683984*^9, 3.832648724079982*^9}}, + CellLabel->"In[35]:=",ExpressionUUID->"c309ce02-68a6-472c-a37b-6aa6dd325b26"], + +Cell[BoxData["20"], "Output", + CellChangeTimes->{3.832648764082135*^9}, + CellLabel->"Out[35]=",ExpressionUUID->"f8dcc6e0-aafc-4a19-b381-1011d0ac9033"] +}, Open ]], + +Cell["Parameters", "Subsubtitle", + CellChangeTimes->{{3.576920320535687*^9, + 3.576920325728017*^9}},ExpressionUUID->"a1d3915d-f613-4425-8a04-\ +fe27c5187298"], + +Cell[BoxData[{ + RowBox[{ + RowBox[{"h0", " ", "=", + RowBox[{"airDensity", "[", "1", "]"}]}], ";", + RowBox[{"(*", " ", + RowBox[{ + "base", " ", "point", " ", "altitude", " ", "above", " ", "sea", " ", + "level"}], " ", "*)"}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"T0", "=", + RowBox[{"airDensity", "[", "2", "]"}]}], ";", " ", + RowBox[{"(*", " ", + RowBox[{ + "ambient", " ", "temperature", " ", "at", " ", "mean", " ", "sea", " ", + "level"}], " ", "*)"}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"p0", "=", + RowBox[{"airDensity", "[", "3", "]"}]}], ";", " ", + RowBox[{"(*", " ", + RowBox[{"pressure", " ", "at", " ", "sea", " ", "level"}], " ", "*)"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"a", " ", "=", " ", + RowBox[{ + RowBox[{"-", "6.5"}], " ", + RowBox[{"10", "^", + RowBox[{"-", "3"}]}]}]}], ";", " ", + RowBox[{"(*", " ", + RowBox[{"temperature", " ", "gradient"}], " ", "*)"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"Ra", " ", "=", " ", "287.30"}], ";", " ", + RowBox[{"(*", " ", + RowBox[{"gas", " ", "constant", " ", "for", " ", "air"}], " ", "*)"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"g", "=", " ", + RowBox[{"gravity", "[", "1", "]"}]}], ";", " ", + RowBox[{"(*", " ", "gravity", " ", "*)"}], ";"}], " ", + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"CFT1", "=", + RowBox[{"fThrust", "[", "1", "]"}]}], ";", " ", + RowBox[{"(*", " ", + RowBox[{"thrust", " ", "force", " ", "coefficients"}], "*)"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"CFT2", "=", + RowBox[{"fThrust", "[", "2", "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"CFT3", "=", + RowBox[{"fThrust", "[", "3", "]"}]}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"CFX1", "=", + RowBox[{"fDrag", "[", "1", "]"}]}], ";", " ", + RowBox[{"(*", " ", + RowBox[{"drag", " ", "force", " ", "coefficients"}], " ", "*)"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"CFXalpha1", " ", "=", " ", + RowBox[{"fDrag", "[", "2", "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"CFXalpha2", " ", "=", " ", + RowBox[{"fDrag", "[", "3", "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"CFXbeta2", " ", "=", " ", + RowBox[{"fDrag", "[", "4", "]"}]}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"CFY1", "=", + RowBox[{"fLat", "[", "1", "]"}]}], ";", " ", + RowBox[{"(*", " ", + RowBox[{"lateral", " ", "force", " ", "coefficient"}], " ", "*)"}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"CFZ1", "=", + RowBox[{"fLift", "[", "1", "]"}]}], ";", " ", + RowBox[{"(*", " ", + RowBox[{"lift", " ", "force", " ", "coefficients"}], " ", "*)"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"CFZalpha", "=", + RowBox[{"fLift", "[", "2", "]"}]}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"CMXa", "=", + RowBox[{"mRoll", "[", "1", "]"}]}], ";", + RowBox[{"(*", " ", + RowBox[{"roll", " ", "moment", " ", "coefficients"}], " ", "*)"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"CMXbeta", "=", + RowBox[{"mRoll", "[", "2", "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"CMX\[Omega]tildex", "=", + RowBox[{"mRoll", "[", "3", "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"CMX\[Omega]tildez", "=", + RowBox[{"mRoll", "[", "4", "]"}]}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"CMY1", "=", + RowBox[{"mPitch", "[", "1", "]"}]}], ";", + RowBox[{"(*", " ", + RowBox[{"pitch", " ", "moment", " ", "coefficients"}], " ", "*)"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"CMYe", "=", + RowBox[{"mPitch", "[", "2", "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"CMYalpha", "=", + RowBox[{"mPitch", "[", "3", "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"CMY\[Omega]tildey", "=", + RowBox[{"mPitch", "[", "4", "]"}]}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"CMZr", "=", + RowBox[{"mYaw", "[", "1", "]"}]}], ";"}], " ", + RowBox[{"(*", " ", + RowBox[{"yaw", " ", "moment", " ", "coefficients"}], " ", "*)"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"CMZbeta", "=", + RowBox[{"mYaw", "[", "2", "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"CMZ\[Omega]tildez", "=", + RowBox[{"mYaw", "[", "3", "]"}]}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"cbar", " ", "=", " ", + RowBox[{"cBar", "[", "1", "]"}]}], ";", " ", + RowBox[{"(*", " ", + RowBox[{"mean", " ", "aereodynamic", " ", "chord"}], " ", "*)"}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"PD", "=", + RowBox[{"cp", "[", "1", "]"}]}], ";", " ", + RowBox[{"(*", " ", + RowBox[{"propeller", " ", "diameter"}], " ", "*)"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"S", "=", + RowBox[{"cp", "[", "2", "]"}]}], ";", " ", + RowBox[{"(*", " ", + RowBox[{"wing", " ", "surface"}], " ", "*)"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"b", "=", + RowBox[{"cp", "[", "3", "]"}]}], ";", " ", + RowBox[{"(*", " ", + RowBox[{"wind", " ", "span"}], " ", "*)"}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"m", " ", "=", " ", + RowBox[{"cp", "[", "4", "]"}]}], ";", " ", + RowBox[{"(*", " ", "mass", " ", "*)"}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"Ixx", "=", + RowBox[{"ibd", "[", "1", "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"Iyy", "=", + RowBox[{"ibd", "[", "2", "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"Izz", " ", "=", + RowBox[{"ibd", "[", "3", "]"}]}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"Ixz", "=", + RowBox[{"ibod", "[", "1", "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"Ixy", "=", + RowBox[{"ibod", "[", "2", "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"Iyz", "=", + RowBox[{"ibod", "[", "3", "]"}]}], ";"}]}], "Input", + CellChangeTimes->{{3.630156966488817*^9, 3.630156997327407*^9}, { + 3.630240787595443*^9, 3.630240871119405*^9}, {3.632462563480411*^9, + 3.6324626574790983`*^9}, {3.6324627822557497`*^9, + 3.6324627840911913`*^9}, {3.6324628429667397`*^9, 3.632462845395872*^9}, { + 3.632463452013672*^9, 3.632463468595188*^9}, {3.6324635424969807`*^9, + 3.632463578847823*^9}, {3.6324638570723257`*^9, 3.632463892398457*^9}, + 3.6324640836270533`*^9, {3.632465074140349*^9, 3.632465180642975*^9}, { + 3.632465229204035*^9, 3.632465255301177*^9}, {3.632465722100479*^9, + 3.632465736568845*^9}, {3.6324658501384373`*^9, 3.632465874972086*^9}, { + 3.632466231247014*^9, 3.6324662436985607`*^9}, {3.632466293804319*^9, + 3.63246666807226*^9}, {3.632466702556988*^9, 3.632466739506076*^9}, { + 3.6324669714561977`*^9, 3.632466989419321*^9}, {3.6324671749054317`*^9, + 3.632467191927196*^9}, {3.632467242245677*^9, 3.632467340635708*^9}, { + 3.632467371717039*^9, 3.632467375747436*^9}, {3.6324674914282084`*^9, + 3.632467492058816*^9}, {3.6324735043656588`*^9, 3.632473540164185*^9}, + 3.632473659229198*^9, {3.6324739348426247`*^9, 3.632473970395046*^9}, { + 3.632474373588602*^9, 3.63247449276618*^9}, {3.632484917459989*^9, + 3.6324849486938457`*^9}, {3.632485035091144*^9, 3.632485038949534*^9}, { + 3.6324859054962873`*^9, 3.632485963402033*^9}, {3.632486128812416*^9, + 3.632486192068241*^9}, {3.6325479968094463`*^9, 3.632548014105666*^9}, { + 3.6325480606534348`*^9, 3.632548077440715*^9}, {3.632563365761787*^9, + 3.6325633685352783`*^9}, {3.634301394539613*^9, 3.6343014011226387`*^9}, { + 3.6343016879675493`*^9, 3.634301688364636*^9}, {3.63430226729615*^9, + 3.634302271560999*^9}, 3.634302333067884*^9, 3.634303000998124*^9, { + 3.634303882252087*^9, 3.634303882568714*^9}, 3.634303913676345*^9, { + 3.7715699472059917`*^9, 3.771569949298133*^9}, {3.772359310486154*^9, + 3.772359365392565*^9}, {3.7723594227641973`*^9, 3.772359613795624*^9}, { + 3.772360215586792*^9, 3.77236021597759*^9}, {3.77314250558466*^9, + 3.773142505699256*^9}, {3.774589936393152*^9, 3.774589945751444*^9}, { + 3.777109991948604*^9, 3.777109997353167*^9}, {3.820073719683056*^9, + 3.820073720934869*^9}}, + CellLabel->"In[39]:=",ExpressionUUID->"33f47236-ad3c-4a61-8f93-8222aa6cc9fa"], + +Cell[BoxData[{ + RowBox[{ + RowBox[{ + RowBox[{ + RowBox[{"IB", "=", + RowBox[{"{", + RowBox[{ + RowBox[{"{", + RowBox[{"Ixx", ",", "Ixy", ",", "Ixz"}], "}"}], ",", + RowBox[{"{", + RowBox[{"Ixy", ",", "Iyy", ",", "Iyz"}], "}"}], ",", + RowBox[{"{", + RowBox[{"Ixz", ",", "Iyz", ",", "Izz"}], "}"}]}], "}"}]}], ";"}], " ", + RowBox[{"(*", " ", + RowBox[{"Inertia", " ", "matrix"}], " ", "*)"}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"AIRDENSITY", " ", "=", " ", + RowBox[{"Table", "[", + RowBox[{ + RowBox[{"airDensity", "[", "i", "]"}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", "3"}], "}"}]}], "]"}]}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"FTHRUST", " ", "=", " ", + RowBox[{"Table", "[", + RowBox[{ + RowBox[{"fThrust", "[", "i", "]"}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", "3"}], "}"}]}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"FDRAG", " ", "=", " ", + RowBox[{"Table", "[", + RowBox[{ + RowBox[{"fDrag", "[", "i", "]"}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", "4"}], "}"}]}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"FLAT", " ", "=", " ", + RowBox[{"Table", "[", + RowBox[{ + RowBox[{"fLat", "[", "i", "]"}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", "1"}], "}"}]}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"FLIFT", "=", " ", + RowBox[{"Table", "[", + RowBox[{ + RowBox[{"fLift", "[", "i", "]"}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", "2"}], "}"}]}], "]"}]}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MROLL", " ", "=", " ", + RowBox[{"Table", "[", + RowBox[{ + RowBox[{"mRoll", "[", "i", "]"}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", "4"}], "}"}]}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MPITCH", " ", "=", " ", + RowBox[{"Table", "[", + RowBox[{ + RowBox[{"mPitch", "[", "i", "]"}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", "4"}], "}"}]}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"MYAW", " ", "=", " ", + RowBox[{"Table", "[", + RowBox[{ + RowBox[{"mYaw", "[", "i", "]"}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", "3"}], "}"}]}], "]"}]}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"CBAR", " ", "=", " ", + RowBox[{"Table", "[", + RowBox[{ + RowBox[{"cBar", "[", "i", "]"}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", "1"}], "}"}]}], "]"}]}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"CP", " ", "=", " ", + RowBox[{"Table", "[", + RowBox[{ + RowBox[{"cp", "[", "i", "]"}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", "4"}], "}"}]}], "]"}]}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"IBD", " ", "=", " ", + RowBox[{"Table", "[", + RowBox[{ + RowBox[{"ibd", "[", "i", "]"}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", "3"}], "}"}]}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"IBOD", " ", "=", " ", + RowBox[{"Table", "[", + RowBox[{ + RowBox[{"ibod", "[", "i", "]"}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", "3"}], "}"}]}], "]"}]}], + ";"}], "\[IndentingNewLine]"}], "Input", + CellChangeTimes->{{3.6324667198820887`*^9, 3.632466771730083*^9}, { + 3.6324745054453506`*^9, 3.632474645927088*^9}, {3.6324746840634813`*^9, + 3.6324746893289757`*^9}, {3.632484902639776*^9, 3.6324849084840813`*^9}, { + 3.6324849547663603`*^9, 3.632485026020185*^9}, {3.632485970613168*^9, + 3.6324859838780127`*^9}, {3.632486046079789*^9, 3.632486046409316*^9}, { + 3.632486196852049*^9, 3.632486214881503*^9}, 3.6325480929677277`*^9, { + 3.6325633642283154`*^9, 3.632563371518127*^9}, {3.772359621486253*^9, + 3.772359671724037*^9}, {3.772359705820054*^9, 3.772359711259782*^9}, { + 3.772359937605813*^9, 3.772360048550315*^9}, {3.7723600966964912`*^9, + 3.772360359842062*^9}, {3.775206708566532*^9, 3.775206780566957*^9}}, + CellLabel->"In[77]:=",ExpressionUUID->"53a40cfc-f0e8-4e44-9f25-b052baa99c29"], + +Cell[BoxData[ + RowBox[{ + RowBox[{"WINDW", "=", + RowBox[{"Table", "[", + RowBox[{ + RowBox[{"wind", "[", "i", "]"}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", "3"}], "}"}]}], "]"}]}], ";", + RowBox[{"(*", " ", + RowBox[{"wind", " ", "speed", " ", "in", " ", "world", " ", "frame"}], " ", + "*)"}]}]], "Input", + CellChangeTimes->{3.8325884344470167`*^9}, + CellLabel->"In[90]:=",ExpressionUUID->"230fd034-8fee-4db7-94bf-9c0c77325dce"], + +Cell[BoxData[{ + RowBox[{ + RowBox[{ + RowBox[{"n", " ", "=", " ", + RowBox[{"z", "[", "1", "]"}]}], ";"}], " ", + RowBox[{"(*", " ", + RowBox[{"propeller", " ", "speed"}], " ", "*)"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"\[Delta]a", "=", + RowBox[{"z", "[", "2", "]"}]}], ";", + RowBox[{"(*", " ", + RowBox[{"aileron", " ", "deflection"}], " ", "*)"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"\[Delta]e", "=", + RowBox[{"z", "[", "3", "]"}]}], ";", + RowBox[{"(*", " ", + RowBox[{"elevator", " ", "deflection"}], " ", "*)"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{ + RowBox[{"\[Delta]r", "=", + RowBox[{"z", "[", "4", "]"}]}], ";"}], + RowBox[{"(*", " ", + RowBox[{"rudder", " ", "deflection"}], " ", "*)"}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"Z", "=", + RowBox[{"Table", "[", + RowBox[{ + RowBox[{"z", "[", "i", "]"}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", "4"}], "}"}]}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"NOISES", "=", + RowBox[{"Table", "[", + RowBox[{ + RowBox[{"eta", "[", "i", "]"}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", "6"}], "}"}]}], "]"}]}], ";"}]}], "Input", + CellChangeTimes->{{3.632466994423296*^9, 3.6324670023447437`*^9}, { + 3.6324673568630743`*^9, 3.632467391423369*^9}, {3.632473596617989*^9, + 3.632473628189311*^9}, {3.6324860167895203`*^9, 3.6324860253571453`*^9}, { + 3.6324862298118343`*^9, 3.632486253278077*^9}, {3.632488200362911*^9, + 3.632488217048265*^9}}, + CellLabel->"In[91]:=",ExpressionUUID->"3db40cdd-aae8-4aed-bbcf-7b6c917acc34"], + +Cell["Equations", "Subsubtitle", + CellChangeTimes->{{3.573298514193714*^9, 3.5732985158016453`*^9}, { + 3.5814133864550056`*^9, 3.581413389613989*^9}, 3.5814150402587423`*^9, { + 3.600064344573599*^9, + 3.600064346732829*^9}},ExpressionUUID->"3a22e419-d65c-4c9e-84c6-\ +863e409e519a"], + +Cell["Air density model", "Text", + CellChangeTimes->{{3.632464865348349*^9, 3.632464865838887*^9}, { + 3.632464907969676*^9, + 3.632464942989243*^9}},ExpressionUUID->"9ac0d911-0e84-4579-bcb9-\ +2110f48ca902"], + +Cell[BoxData[ + RowBox[{ + RowBox[{"(*", + RowBox[{ + RowBox[{"T", "=", + RowBox[{"T0", + RowBox[{"(", + RowBox[{"1", "+", + RowBox[{"a", " ", + RowBox[{ + RowBox[{"(", + RowBox[{"h0", "+", + RowBox[{"SW", "[", + RowBox[{"[", "3", "]"}], "]"}]}], ")"}], "/", "T0"}]}]}], + ")"}]}]}], ";", "\[IndentingNewLine]", + RowBox[{"\[Rho]", "=", + RowBox[{ + RowBox[{"p0", + RowBox[{ + RowBox[{ + RowBox[{"(", + RowBox[{"1", "+", + RowBox[{"a", + RowBox[{ + RowBox[{"(", + RowBox[{"h0", "+", + RowBox[{"SW", "[", + RowBox[{"[", "3", "]"}], "]"}]}], ")"}], "/", "T0"}]}]}], + ")"}], "^", "5.2561"}], "/", + RowBox[{"(", + RowBox[{"Ra", " ", "T"}], ")"}]}]}], "//", "FullSimplify"}]}], ";"}], + "*)"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"\[Rho]", " ", "=", " ", "1.268"}], ";"}]}]], "Input", + CellChangeTimes->{{3.632462276976709*^9, 3.632462293513674*^9}, { + 3.63246255070463*^9, 3.63246255752766*^9}, {3.632462666581998*^9, + 3.632462767916204*^9}, 3.632462848468005*^9, {3.632462884866085*^9, + 3.632463087325437*^9}, {3.632464836860113*^9, 3.632464838151527*^9}, { + 3.6324649483473377`*^9, 3.632464955096529*^9}, {3.632473673346718*^9, + 3.6324736736899147`*^9}, {3.632484816779133*^9, 3.632484818831092*^9}, { + 3.6324860751887093`*^9, 3.6324860763421392`*^9}, {3.6326351738117943`*^9, + 3.632635175495201*^9}, {3.634305313267096*^9, 3.634305315999455*^9}, + 3.771231605060816*^9, {3.7713227938461018`*^9, 3.771322795852179*^9}, { + 3.772946102534224*^9, 3.772946104786611*^9}, 3.773142614758792*^9, { + 3.781532267573668*^9, 3.781532269711938*^9}, {3.831543702800745*^9, + 3.831543703679405*^9}, {3.8315534192530937`*^9, 3.831553422523533*^9}, + 3.831553568121788*^9, {3.831699456786346*^9, 3.83169945836695*^9}, { + 3.8325862000061197`*^9, 3.83258620771362*^9}, 3.832644757539814*^9}, + CellLabel->"In[97]:=",ExpressionUUID->"9f81cfa6-a749-4450-bfb3-736066b50bce"], + +Cell["Airspeed", "Text", + CellChangeTimes->{{3.6324649632440243`*^9, + 3.6324649676453876`*^9}},ExpressionUUID->"656d798c-a5c9-4cc6-a1dd-\ +06c8dc6c6565"], + +Cell[BoxData[{ + RowBox[{ + RowBox[{"AIRS", "=", + RowBox[{"VSprev", "-", + RowBox[{ + RowBox[{"QuatToRot", "[", + RowBox[{"QuatInv", "[", "QWSprev", "]"}], "]"}], ".", "WINDW"}]}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"\[Alpha]", "=", + RowBox[{"ArcTan", "[", + RowBox[{ + RowBox[{"AIRS", "[", + RowBox[{"[", "1", "]"}], "]"}], ",", + RowBox[{"AIRS", "[", + RowBox[{"[", "3", "]"}], "]"}]}], "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"\[Beta]", "=", + RowBox[{"ArcSin", "[", + RowBox[{ + RowBox[{"AIRS", "[", + RowBox[{"[", "2", "]"}], "]"}], "/", + RowBox[{"L2", "[", "AIRS", "]"}]}], "]"}]}], ";"}], " ", + "\[IndentingNewLine]", + RowBox[{"(*", " ", + RowBox[{"dynamic", " ", "pressure"}], " ", "*)"}]}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"qbar", "=", + RowBox[{"\[Rho]", " ", + RowBox[{ + RowBox[{ + RowBox[{"L2", "[", "AIRS", "]"}], "^", "2"}], "/", "2"}]}]}], + ";"}], "\[IndentingNewLine]"}], "Input", + CellChangeTimes->{{3.632463641043902*^9, 3.632463700014979*^9}, { + 3.632463760723637*^9, 3.6324638026841*^9}, {3.6324638481892347`*^9, + 3.6324638485511303`*^9}, {3.632464012285355*^9, 3.6324640922581472`*^9}, { + 3.63246427497224*^9, 3.6324645819649153`*^9}, {3.632464802514184*^9, + 3.632464847852816*^9}, {3.632464958182373*^9, 3.632464975561612*^9}, { + 3.632548707477417*^9, 3.632548715250105*^9}, {3.774611994153266*^9, + 3.774611997890765*^9}, 3.781531472311548*^9, {3.782109505828165*^9, + 3.782109507995604*^9}, 3.831543728856717*^9, {3.831548449935013*^9, + 3.831548459109014*^9}, {3.831550697106958*^9, 3.831550699078291*^9}, { + 3.832247268983202*^9, 3.832247269898589*^9}, {3.8322473017502937`*^9, + 3.832247302418412*^9}, 3.83224897502195*^9, {3.832249189474696*^9, + 3.8322491900790663`*^9}, {3.832299949819049*^9, 3.832299967291203*^9}, { + 3.832300630206356*^9, 3.8323006337974033`*^9}, {3.8323006971741323`*^9, + 3.832300698717581*^9}, {3.832304078072732*^9, 3.832304081647558*^9}, { + 3.8323052074555264`*^9, 3.832305221126236*^9}, {3.83230937627665*^9, + 3.8323094006135273`*^9}, {3.832309438863019*^9, 3.832309447080554*^9}, { + 3.832309501309993*^9, 3.832309611275866*^9}, {3.832309752797491*^9, + 3.8323098428394012`*^9}, 3.832484877821838*^9, {3.832484914498892*^9, + 3.832484945826915*^9}, {3.832588820959147*^9, 3.83258884035524*^9}}, + CellLabel->"In[98]:=",ExpressionUUID->"29b01d2f-83ef-4d5a-8a25-69d70c2e9207"], + +Cell["Wind frame ", "Text", + CellChangeTimes->{{3.6324649823613243`*^9, + 3.6324649864149113`*^9}},ExpressionUUID->"7aa4501a-e764-4f85-8410-\ +73dc94387049"], + +Cell[BoxData[ + RowBox[{ + RowBox[{"(*", + RowBox[{ + RowBox[{"RWINDS", "=", + RowBox[{ + RowBox[{"Transpose", "[", + RowBox[{"RotationMatrix", "[", + RowBox[{"\[Alpha]", ",", + RowBox[{"{", + RowBox[{"0", ",", "1", ",", "0"}], "}"}]}], "]"}], "]"}], ".", + RowBox[{"RotationMatrix", "[", + RowBox[{"\[Beta]", ",", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "1"}], "}"}]}], "]"}]}]}], ";"}], "*)"}], + "\[IndentingNewLine]", + RowBox[{ + RowBox[{"RWINDS", " ", "=", " ", + RowBox[{ + RowBox[{"{", + RowBox[{ + RowBox[{"{", + RowBox[{ + RowBox[{"Cos", "[", "\[Alpha]", "]"}], ",", "0", ",", + RowBox[{"-", + RowBox[{"Sin", "[", "\[Alpha]", "]"}]}]}], "}"}], ",", + RowBox[{"{", + RowBox[{"0", ",", "1", ",", "0"}], "}"}], ",", + RowBox[{"{", + RowBox[{ + RowBox[{"Sin", "[", "\[Alpha]", "]"}], ",", "0", ",", + RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], "}"}]}], "}"}], ".", + RowBox[{"{", + RowBox[{ + RowBox[{"{", + RowBox[{ + RowBox[{"Cos", "[", "\[Beta]", "]"}], ",", + RowBox[{"-", + RowBox[{"Sin", "[", "\[Beta]", "]"}]}], ",", "0"}], "}"}], ",", + RowBox[{"{", + RowBox[{ + RowBox[{"Sin", "[", "\[Beta]", "]"}], ",", + RowBox[{"Cos", "[", "\[Beta]", "]"}], ",", "0"}], "}"}], ",", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "1"}], "}"}]}], "}"}]}]}], + ";"}]}]], "Input", + CellChangeTimes->{{3.6324646247401857`*^9, 3.632464632600142*^9}, { + 3.632464680101325*^9, 3.632464784467514*^9}, {3.6324649794605837`*^9, + 3.6324649904896584`*^9}, {3.632474117326888*^9, 3.632474118720315*^9}, { + 3.632474151294368*^9, 3.6324741531255283`*^9}, {3.63257138686383*^9, + 3.632571433828497*^9}, {3.632571484241777*^9, 3.632571500711404*^9}, { + 3.634300370544889*^9, 3.634300375506771*^9}, {3.774266318707065*^9, + 3.774266321501566*^9}, {3.774612005261743*^9, 3.774612007778376*^9}, { + 3.774691121917021*^9, 3.7746911255822783`*^9}, 3.7815314791176033`*^9, + 3.831544148398527*^9, 3.832299971483536*^9, 3.832300701453065*^9, + 3.832309961580757*^9, {3.832309996548251*^9, 3.832310121436122*^9}, { + 3.832310268266717*^9, 3.83231027077208*^9}, 3.83248499849096*^9, + 3.83248513698739*^9, 3.832587709752873*^9, {3.832591612964304*^9, + 3.8325916292963552`*^9}}, + CellLabel-> + "In[102]:=",ExpressionUUID->"dbea6cef-0336-433e-92e3-cc59b4a69219"], + +Cell["Thrust force", "Text", + CellChangeTimes->{{3.632464997924268*^9, 3.63246500193495*^9}, + 3.7723597660999403`*^9},ExpressionUUID->"15e73123-e84e-4360-b55b-\ +334ef918a821"], + +Cell[BoxData[ + RowBox[{ + RowBox[{"(*", + RowBox[{ + "this", " ", "can", " ", "cause", " ", "division", " ", "by", " ", + "zero"}], "*)"}], "\[IndentingNewLine]", + RowBox[{"(*", + RowBox[{ + RowBox[{ + RowBox[{"CFT", "[", "J_", "]"}], ":=", + RowBox[{"CFT1", "+", + RowBox[{"CFT2", " ", "J"}], "+", " ", + RowBox[{"CFT3", " ", + RowBox[{"J", "^", "2"}]}]}]}], ";", "\[IndentingNewLine]", + RowBox[{"FT", "=", + RowBox[{"\[Rho]", " ", + RowBox[{"n", "^", "2"}], " ", + RowBox[{"PD", "^", "4"}], " ", + RowBox[{"CFT", "[", + RowBox[{ + RowBox[{"L2", "[", "AIRS", "]"}], "/", + RowBox[{"(", + RowBox[{"PD", " ", "\[Pi]", " ", "n"}], ")"}]}], "]"}]}]}], ";"}], + "*)"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"FT", "=", + RowBox[{"\[Rho]", " ", + RowBox[{"PD", "^", "2"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"CFT1", " ", + RowBox[{"PD", "^", "2"}], " ", + RowBox[{"n", "^", "2"}]}], " ", "+", " ", + RowBox[{"CFT2", " ", "PD", " ", "n", " ", + RowBox[{ + RowBox[{"L2", "[", "AIRS", "]"}], "/", "\[Pi]"}]}], "+", + RowBox[{"CFT3", " ", + RowBox[{ + RowBox[{ + RowBox[{"L2", "[", "AIRS", "]"}], "^", "2"}], "/", + RowBox[{"\[Pi]", "^", "2"}]}]}]}], ")"}]}]}], ";"}]}]], "Input", + CellChangeTimes->{{3.6324630957984457`*^9, 3.6324631592841997`*^9}, { + 3.63246362407451*^9, 3.632463645243698*^9}, {3.632464992403228*^9, + 3.632465054737678*^9}, {3.632465263450095*^9, 3.6324652660503607`*^9}, + 3.632465322251411*^9, 3.632548717997613*^9, 3.634980291260788*^9, { + 3.634980577412641*^9, 3.6349807142179937`*^9}, 3.6349807576397457`*^9, { + 3.6349808735654984`*^9, 3.634980887899046*^9}, {3.634982313791176*^9, + 3.634982334481447*^9}, {3.634988395067772*^9, 3.634988403877116*^9}, { + 3.8315480801115217`*^9, 3.831548105174776*^9}, {3.831552329329358*^9, + 3.831552340183215*^9}}, + CellLabel-> + "In[103]:=",ExpressionUUID->"176642b9-ef21-428e-bda6-88b3064f0fe2"], + +Cell["Drag force", "Text", + CellChangeTimes->{{3.632465883892436*^9, + 3.632465884995759*^9}},ExpressionUUID->"ac9144ba-14f4-4cb2-9d7c-\ +1b84aa717d7b"], + +Cell[BoxData[{ + RowBox[{ + RowBox[{"CFX", "=", + RowBox[{"CFX1", "-", + RowBox[{"CFXalpha1", " ", "\[Alpha]"}], "+", + RowBox[{"CFXalpha2", " ", + RowBox[{"\[Alpha]", "^", "2"}]}], "+", + RowBox[{"CFXbeta2", " ", + RowBox[{"\[Beta]", "^", "2"}]}]}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"FWX", "=", + RowBox[{"qbar", " ", "S", " ", "CFX"}]}], ";"}]}], "Input", + CellChangeTimes->{{3.632465900598139*^9, 3.63246595313962*^9}, { + 3.632466061225836*^9, 3.632466070531445*^9}, 3.632474180051531*^9, { + 3.632486333812204*^9, 3.632486338262673*^9}, {3.831544195582663*^9, + 3.831544196885738*^9}}, + CellLabel-> + "In[104]:=",ExpressionUUID->"f4931c36-9cb3-41d5-9636-f3fbcef428c9"], + +Cell["Lateral force", "Text", + CellChangeTimes->{{3.63246580061057*^9, + 3.632465802148518*^9}},ExpressionUUID->"57ce017d-559b-4d58-ba18-\ +1e7631c436d9"], + +Cell[BoxData[{ + RowBox[{ + RowBox[{"CFY", "=", + RowBox[{"CFY1", " ", "\[Beta]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"FWY", "=", + RowBox[{"qbar", " ", "S", " ", "CFY"}]}], ";"}]}], "Input", + CellChangeTimes->{{3.632465807062015*^9, 3.63246584035797*^9}, + 3.774612027001218*^9, 3.7815314926688843`*^9}, + CellLabel-> + "In[106]:=",ExpressionUUID->"63203ee0-9601-47b1-9606-b67b10ce9a46"], + +Cell["Lift force", "Text", + CellChangeTimes->{{3.6324653247972507`*^9, + 3.6324653267561407`*^9}},ExpressionUUID->"4432b235-bb2b-4e9d-87f8-\ +55bf93c648df"], + +Cell[BoxData[{ + RowBox[{ + RowBox[{"CFZ", "=", + RowBox[{ + RowBox[{"-", "CFZ1"}], "+", + RowBox[{"CFZalpha", " ", "\[Alpha]"}]}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"FWZ", "=", + RowBox[{"qbar", " ", "S", " ", "CFZ"}]}], ";"}]}], "Input", + CellChangeTimes->{{3.6324653325318327`*^9, 3.632465404457531*^9}, { + 3.632465682669859*^9, 3.6324656933429127`*^9}, {3.632465792314137*^9, + 3.632465798108323*^9}, 3.774612028899675*^9, {3.831544264680245*^9, + 3.8315442668236523`*^9}}, + CellLabel-> + "In[108]:=",ExpressionUUID->"c73ee33c-6f07-4ea9-82e2-0f37fad73c8e"], + +Cell["Roll moment", "Text", + CellChangeTimes->{{3.632466790216868*^9, + 3.632466796296864*^9}},ExpressionUUID->"3b78e61b-a31c-43c6-949c-\ +8ab46a558347"], + +Cell[BoxData[{ + RowBox[{ + RowBox[{"\[Omega]tildex", "=", " ", + RowBox[{"b", " ", + RowBox[{ + RowBox[{"WSprev", "[", + RowBox[{"[", "1", "]"}], "]"}], "/", + RowBox[{"(", + RowBox[{"2", " ", + RowBox[{"L2", "[", "AIRS", "]"}]}], ")"}]}]}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"\[Omega]tildez", "=", + RowBox[{"b", " ", + RowBox[{ + RowBox[{"WSprev", "[", + RowBox[{"[", "3", "]"}], "]"}], "/", + RowBox[{"(", + RowBox[{"2", " ", + RowBox[{"L2", "[", "AIRS", "]"}]}], ")"}]}]}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"CMX", "=", + RowBox[{ + RowBox[{"CMXa", " ", "\[Delta]a"}], " ", "-", + RowBox[{"CMXbeta", " ", "\[Beta]"}], "+", + RowBox[{"CMX\[Omega]tildex", " ", "\[Omega]tildex"}], " ", "-", + RowBox[{"CMX\[Omega]tildez", " ", "\[Omega]tildez"}]}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MBX", "=", + RowBox[{"qbar", " ", "S", " ", "b", " ", "CMX"}]}], ";"}]}], "Input", + CellChangeTimes->{{3.632466801116767*^9, 3.6324669518785152`*^9}, { + 3.632467018819668*^9, 3.632467037920843*^9}, {3.632467073135228*^9, + 3.632467123591279*^9}, {3.6324674621580877`*^9, 3.632467462850863*^9}, { + 3.6324862875540953`*^9, 3.6324862888104773`*^9}, {3.6325487193918743`*^9, + 3.632548719873026*^9}, 3.634292400473446*^9, {3.774612038472706*^9, + 3.774612054579494*^9}, {3.7815315014383574`*^9, 3.781531501625888*^9}, { + 3.8315442788487*^9, 3.831544307311618*^9}, {3.832247284671228*^9, + 3.832247288169239*^9}, {3.832304124264847*^9, 3.832304125991396*^9}, { + 3.8323052366918087`*^9, 3.832305240596637*^9}}, + CellLabel-> + "In[110]:=",ExpressionUUID->"7be4a0a1-b2c0-4d7b-b564-9f92dabf375f"], + +Cell["Pitch moment", "Text", + CellChangeTimes->{{3.632467155741108*^9, + 3.632467157541481*^9}},ExpressionUUID->"78b600c8-17c9-43b2-8561-\ +39fdff7b92e4"], + +Cell[BoxData[{ + RowBox[{ + RowBox[{"\[Omega]tildey", "=", + RowBox[{"cbar", " ", + RowBox[{ + RowBox[{"WSprev", "[", + RowBox[{"[", "2", "]"}], "]"}], "/", + RowBox[{"(", + RowBox[{"2", " ", + RowBox[{"L2", "[", "AIRS", "]"}]}], ")"}]}]}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"CMY", "=", + RowBox[{ + RowBox[{"-", "CMY1"}], "-", + RowBox[{"CMYe", " ", "\[Delta]e"}], " ", "+", + RowBox[{"CMY\[Omega]tildey", " ", "\[Omega]tildey"}], "+", + RowBox[{"CMYalpha", " ", "\[Alpha]"}]}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MBY", "=", + RowBox[{"qbar", " ", "S", " ", "cbar", " ", "CMY"}]}], ";"}]}], "Input", + CellChangeTimes->{{3.632467408804394*^9, 3.632467485452004*^9}, { + 3.6324734635941877`*^9, 3.6324734804954777`*^9}, {3.632486355640251*^9, + 3.632486356968248*^9}, 3.6325487214032393`*^9, {3.774612041318342*^9, + 3.774612074499466*^9}, 3.831544287152123*^9, {3.831544336079039*^9, + 3.8315443421869287`*^9}, {3.832247292103303*^9, 3.832247292550302*^9}, + 3.832304119896791*^9, {3.832305243123885*^9, 3.8323052436925697`*^9}}, + CellLabel-> + "In[114]:=",ExpressionUUID->"4ce3e11d-b2d8-42df-94f3-f44671feb140"], + +Cell["Yaw moment", "Text", + CellChangeTimes->{{3.6324733890787363`*^9, + 3.632473392049634*^9}},ExpressionUUID->"c302290e-b2df-4299-8d3e-\ +f59d1fc850b6"], + +Cell[BoxData[{ + RowBox[{ + RowBox[{"CMZ", " ", "=", " ", + RowBox[{ + RowBox[{ + RowBox[{"-", "CMZr"}], " ", "\[Delta]r"}], " ", "+", " ", + RowBox[{"CMZ\[Omega]tildez", " ", "\[Omega]tildez"}], " ", "+", + RowBox[{"CMZbeta", " ", "\[Beta]"}]}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MBZ", "=", + RowBox[{"qbar", " ", "S", " ", "b", " ", "CMZ"}]}], ";"}]}], "Input", + CellChangeTimes->{{3.6324733978818483`*^9, 3.632473492292446*^9}, + 3.632473773078868*^9, 3.632486362438322*^9, {3.632734158387792*^9, + 3.632734159678158*^9}, 3.774612081502236*^9, {3.781531506233139*^9, + 3.781531506482781*^9}, 3.831544367175006*^9}, + CellLabel-> + "In[117]:=",ExpressionUUID->"1a28b43d-e9a2-4ebf-8251-f3ab2ddb7786"], + +Cell["Error equations", "Text", + CellChangeTimes->{{3.6324737754609947`*^9, 3.632473779006583*^9}, { + 3.832248443615851*^9, + 3.832248454604686*^9}},ExpressionUUID->"4ba37bc9-3ca4-4415-be72-\ +e388f0dad47a"], + +Cell[BoxData[{ + RowBox[{ + RowBox[{"forceBfPrev", " ", "=", " ", + RowBox[{ + RowBox[{"{", + RowBox[{"FT", ",", "0", ",", "0"}], "}"}], "+", + RowBox[{"RWINDS", ".", + RowBox[{"{", + RowBox[{"FWX", ",", "FWY", ",", "FWZ"}], "}"}]}]}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"forceBf", " ", "=", " ", + RowBox[{ + RowBox[{"QuatToRot", "[", + RowBox[{"QuatInv", "[", "QWS", "]"}], "]"}], ".", + RowBox[{"QuatToRot", "[", "QWSprev", "]"}], ".", "forceBfPrev"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"momentBfPrev", " ", "=", " ", + RowBox[{"{", + RowBox[{"MBX", ",", "MBY", ",", "MBZ"}], "}"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"momentBf", " ", "=", " ", + RowBox[{ + RowBox[{"QuatToRot", "[", + RowBox[{"QuatInv", "[", "QWS", "]"}], "]"}], ".", + RowBox[{"QuatToRot", "[", "QWSprev", "]"}], " ", ".", "momentBfPrev"}]}], + ";"}], "\[IndentingNewLine]"}], "Input", + CellChangeTimes->{{3.8322484594733133`*^9, 3.8322485749169703`*^9}, { + 3.832248623124647*^9, 3.8322486546207333`*^9}, {3.8322491968290453`*^9, + 3.832249198979947*^9}, {3.832249579349374*^9, 3.832249584091609*^9}, { + 3.832299977117188*^9, 3.8322999859622297`*^9}, 3.832301691372081*^9, { + 3.8323037953164454`*^9, 3.832303798567361*^9}, 3.832304405712686*^9, { + 3.832603447873975*^9, 3.8326034516114483`*^9}, {3.8326043193782263`*^9, + 3.8326043198498*^9}, {3.8326051206906157`*^9, 3.8326051208899183`*^9}}, + CellLabel-> + "In[119]:=",ExpressionUUID->"a2375489-1864-4b87-9537-7325ce23183f"], + +Cell[BoxData[{ + RowBox[{ + RowBox[{"Ahatlong", "=", + RowBox[{ + RowBox[{ + RowBox[{"QuatToRot", "[", + RowBox[{"QuatInv", "[", "QWS", "]"}], "]"}], ".", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", + RowBox[{"-", "g"}]}], "}"}]}], " ", "+", " ", + RowBox[{ + RowBox[{"1", "/", "m"}], " ", + RowBox[{"(", "forceBf", ")"}]}]}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"AlphaHatlong", "=", " ", + RowBox[{ + RowBox[{"Inverse", "[", "IB", "]"}], ".", + RowBox[{"(", + RowBox[{"momentBf", "-", + RowBox[{"WS", "\[Cross]", + RowBox[{"(", + RowBox[{"IB", ".", "WS"}], ")"}]}]}], ")"}]}]}], ";"}]}], "Input", + CellChangeTimes->{{3.632473782667057*^9, 3.632473783948543*^9}, { + 3.632473980553233*^9, 3.632474122781205*^9}, {3.632474156463627*^9, + 3.6324741654080887`*^9}, {3.632474242482204*^9, 3.632474271983945*^9}, { + 3.6324746531850758`*^9, 3.632474665303821*^9}, {3.632474712022402*^9, + 3.632474752296493*^9}, {3.6324863716442823`*^9, 3.632486373451024*^9}, { + 3.6324864511201982`*^9, 3.632486454813035*^9}, {3.6324869920050364`*^9, + 3.632486995777545*^9}, 3.632572048567833*^9, {3.632820407426373*^9, + 3.632820415425437*^9}, {3.6330662719267473`*^9, 3.633066275128694*^9}, + 3.6330672246184607`*^9, {3.633067323386444*^9, 3.633067323446558*^9}, { + 3.633668520364181*^9, 3.6336685404354343`*^9}, {3.6336692322822113`*^9, + 3.633669235904211*^9}, {3.6342782924356194`*^9, 3.634278296987405*^9}, { + 3.6342783329840317`*^9, 3.6342783528443823`*^9}, {3.6344523148400803`*^9, + 3.634452317130845*^9}, {3.7746911481775713`*^9, 3.774691149760127*^9}, { + 3.8319972965581017`*^9, 3.831997300997398*^9}, {3.831997646944586*^9, + 3.831997653276815*^9}, {3.832040513254773*^9, 3.8320405446977453`*^9}, { + 3.832247316431518*^9, 3.832247316918489*^9}, {3.832247398198761*^9, + 3.832247398934546*^9}, {3.8322486164054317`*^9, 3.832248619091113*^9}, + 3.832248663164793*^9, 3.832248733164867*^9, {3.832253775199555*^9, + 3.832253778582693*^9}, {3.832304657623603*^9, 3.832304668959508*^9}, { + 3.83230474180126*^9, 3.832304819047428*^9}, {3.832304866856081*^9, + 3.832304871719405*^9}, {3.832304908031575*^9, 3.832304910295294*^9}, { + 3.832304997408471*^9, 3.832305003871789*^9}, 3.832305054682229*^9, + 3.832305095560555*^9, {3.832305129953635*^9, 3.8323051386755466`*^9}, { + 3.832489603897416*^9, 3.832489625021543*^9}, 3.8325886953739157`*^9, { + 3.832596155349049*^9, 3.832596156246426*^9}, 3.832597219252363*^9}, + CellLabel-> + "In[123]:=",ExpressionUUID->"fe419f0a-adf5-43af-9f08-7b8a7f19180b"], + +Cell[BoxData[{ + RowBox[{ + RowBox[{"Ahat", "=", "Ahatlong"}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"Alphahat", "=", "AlphaHatlong"}], + ";"}], "\[IndentingNewLine]"}], "Input", + CellChangeTimes->{{3.632486455549663*^9, 3.6324865144132833`*^9}, { + 3.632486628670555*^9, 3.632486630005704*^9}, {3.6324868147521963`*^9, + 3.632486817333385*^9}, 3.632486929763055*^9, {3.632487008855073*^9, + 3.632487019375031*^9}, {3.632487069748158*^9, 3.63248707741856*^9}, { + 3.632487112090027*^9, 3.632487150840695*^9}, {3.632487252771805*^9, + 3.6324872660424747`*^9}, {3.632488139926024*^9, 3.632488140072042*^9}, { + 3.632734121716978*^9, 3.632734200327458*^9}, {3.633067298879485*^9, + 3.63306731942362*^9}, {3.634278298237008*^9, 3.63427829964246*^9}, { + 3.63427833587599*^9, 3.634278345660905*^9}, {3.634298584045291*^9, + 3.634298585468253*^9}, {3.7723598046089363`*^9, 3.772359805228352*^9}}, + CellLabel-> + "In[125]:=",ExpressionUUID->"ff5f7d73-add6-4e24-9e55-7b15786660c6"], + +Cell[BoxData[{ + RowBox[{ + RowBox[{"DBG", "=", + RowBox[{"{", + RowBox[{ + "FWX", ",", "FWY", ",", "FWZ", ",", "FT", ",", "MBX", ",", "MBY", ",", + "MBZ"}], "}"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"AhatlongDBG", "=", + RowBox[{ + RowBox[{ + RowBox[{"QuatToRot", "[", + RowBox[{"QuatInv", "[", "QWS", "]"}], "]"}], ".", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", + RowBox[{"-", "g"}]}], "}"}]}], "+", + RowBox[{ + RowBox[{"1", "/", "m"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"{", + RowBox[{ + RowBox[{"f", "[", "4", "]"}], ",", "0", ",", "0"}], "}"}], "+", + RowBox[{ + RowBox[{"Transpose", "[", "RWINDS", "]"}], ".", + RowBox[{"{", + RowBox[{ + RowBox[{"f", "[", "1", "]"}], ",", + RowBox[{"f", "[", "2", "]"}], ",", + RowBox[{"f", "[", "3", "]"}]}], "}"}]}]}], ")"}]}], "-", + RowBox[{"WS", "\[Cross]", "VS"}]}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"AlphahatlongDBG", "=", " ", + RowBox[{ + RowBox[{"Inverse", "[", "IB", "]"}], ".", + RowBox[{"(", + RowBox[{ + RowBox[{"{", + RowBox[{ + RowBox[{"mm", "[", "1", "]"}], ",", + RowBox[{"mm", "[", "2", "]"}], ",", + RowBox[{"mm", "[", "3", "]"}]}], "}"}], "-", + RowBox[{"WS", "\[Cross]", + RowBox[{"(", + RowBox[{"IB", ".", "WS"}], ")"}]}]}], ")"}]}]}], ";"}]}], "Input", + CellChangeTimes->{{3.633234965893896*^9, 3.633234975029994*^9}, { + 3.6332350425322247`*^9, 3.633235051548617*^9}, 3.633244923580419*^9, { + 3.633327208678884*^9, 3.633327209833385*^9}, {3.634298362377328*^9, + 3.6342983969550533`*^9}, {3.634298517168047*^9, 3.63429851785793*^9}, { + 3.634298591096792*^9, 3.634298593067531*^9}, {3.634299639194295*^9, + 3.634299645586197*^9}, {3.634299681394882*^9, 3.6342996858226843`*^9}, + 3.634299852392684*^9, 3.634300111654772*^9, {3.634300178359811*^9, + 3.634300196975257*^9}, 3.8315541730600033`*^9, {3.832036484673256*^9, + 3.832036488336998*^9}, {3.8320404846183357`*^9, 3.832040508374733*^9}}, + CellLabel-> + "In[127]:=",ExpressionUUID->"d17df8c1-9260-4a0c-8495-b5ef6f820848"], + +Cell[BoxData[ + RowBox[{ + RowBox[{ + RowBox[{"ERR", "=", + RowBox[{ + RowBox[{"Flatten", "[", + RowBox[{"{", + RowBox[{ + RowBox[{"AS", "-", "Ahat"}], ",", + RowBox[{"ALPHAS", "-", "Alphahat"}]}], "}"}], "]"}], "+", + "NOISES"}]}], ";"}], "\[IndentingNewLine]"}]], "Input", + CellChangeTimes->{{3.632487913384449*^9, 3.632487921874243*^9}, { + 3.632488026778446*^9, 3.632488059172721*^9}, {3.632488578258338*^9, + 3.632488606817566*^9}, {3.784538832900227*^9, 3.784538833715313*^9}, { + 3.832303486557356*^9, 3.8323034980789423`*^9}, {3.832303665519973*^9, + 3.832303715578887*^9}, {3.832303758180533*^9, 3.832303816907099*^9}, { + 3.832303999935471*^9, 3.832304012743093*^9}, {3.832485226223405*^9, + 3.8324852294423723`*^9}}, + CellLabel-> + "In[130]:=",ExpressionUUID->"8c91f72d-3f47-4547-a792-5e0813bcc352"], + +Cell["Predictor", "Subsubtitle", + CellChangeTimes->{{3.573298851623851*^9, 3.57329885458322*^9}, { + 3.6326367603131237`*^9, + 3.6326367612299*^9}},ExpressionUUID->"b92b1bb0-abe4-4170-8cfc-1e9dd8bc5679"], + +Cell[BoxData[ + RowBox[{ + RowBox[{"(*", " ", + RowBox[{ + "until", " ", "we", " ", "fix", " ", "the", " ", "predictor", " ", + "interface", " ", "we", " ", "just", " ", "predict", " ", "that", " ", + "we", " ", "move", " ", "forward", " ", "at", " ", "1", " ", + RowBox[{"m", "/", "s"}]}], " ", "*)"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"SWhat", "=", + RowBox[{"SW", "+", + RowBox[{ + RowBox[{"QuatToRot", "[", "QWS", "]"}], ".", + RowBox[{"(", + RowBox[{"{", + RowBox[{ + RowBox[{"1", "*", "dt"}], ",", " ", "0", ",", "0"}], "}"}], + ")"}]}]}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"QWShat", "=", "QWS"}], ";"}], "\[IndentingNewLine]"}]}]], "Input",\ + + CellChangeTimes->{{3.632636764848427*^9, 3.632636870351482*^9}, + 3.633327075601856*^9, 3.634538652426509*^9}, + CellLabel-> + "In[131]:=",ExpressionUUID->"dea35a84-3a0f-4600-a2f4-2ab045608d87"], + +Cell["Output", "Subsubtitle", + CellChangeTimes->{{3.573298851623851*^9, + 3.57329885458322*^9}},ExpressionUUID->"bf1afef8-2e19-4e8e-9c26-\ +8b9e596cd10c"], + +Cell[BoxData[ + RowBox[{ + RowBox[{"SetDirectory", "[", + RowBox[{"NotebookDirectory", "[", "]"}], "]"}], ";"}]], "Input", + CellLabel-> + "In[133]:=",ExpressionUUID->"c2f98c75-5faf-44c8-a958-147c2a660678"], + +Cell[CellGroupData[{ + +Cell[BoxData[{ + RowBox[{ + RowBox[{"Err", " ", "=", " ", + RowBox[{"VecToZero", "[", + RowBox[{"ERR", ",", "NOISES"}], "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JErrNoises", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "NOISES", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JErrPose", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "SW", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JErrQ", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "QWS", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JErrV", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "VS", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JErrW", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "WS", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JErrA", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "AS", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JErrAlpha", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "ALPHAS", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{"Dimensions", "[", "ERR", "]"}]}], "Input", + CellChangeTimes->{{3.573299033702265*^9, 3.5732990981024723`*^9}, { + 3.573299164795454*^9, 3.5732991692508307`*^9}, {3.573456001706543*^9, + 3.5734560048187523`*^9}, {3.574152235462706*^9, 3.574152238223208*^9}, { + 3.5798489548623657`*^9, 3.579848955103243*^9}, {3.581401976643291*^9, + 3.5814019885894117`*^9}, {3.58141377998179*^9, 3.581413814574294*^9}, { + 3.581414129291024*^9, 3.581414154679249*^9}, {3.581765163109974*^9, + 3.581765173746241*^9}, {3.581765277500546*^9, 3.581765284545003*^9}, { + 3.6000644741616707`*^9, 3.600064476157126*^9}, {3.600064636065899*^9, + 3.60006464126534*^9}, {3.60610941052632*^9, 3.606109449315563*^9}, { + 3.617517818502956*^9, 3.617517831777896*^9}, 3.617518017239346*^9, + 3.6175224718126917`*^9, {3.617535776012012*^9, 3.6175357977310543`*^9}, { + 3.617691303889946*^9, 3.617691325047617*^9}, {3.6191537025170603`*^9, + 3.619153721782481*^9}, {3.6248780977624187`*^9, 3.6248781199132423`*^9}, { + 3.624878461572874*^9, 3.624878529180564*^9}, {3.626608128585112*^9, + 3.626608147165112*^9}, 3.626608290164939*^9, {3.627970217824462*^9, + 3.6279702738585*^9}, {3.627970352003558*^9, 3.62797036945547*^9}, { + 3.62971028921635*^9, 3.6297105068213043`*^9}, {3.629806116962702*^9, + 3.629806117816551*^9}, {3.630321579757883*^9, 3.630321583197545*^9}, { + 3.6303216981362867`*^9, 3.630321706346642*^9}, {3.630321757205647*^9, + 3.63032178972649*^9}, {3.6303219034786263`*^9, 3.630321908393339*^9}, { + 3.630322038330865*^9, 3.630322043133882*^9}, {3.630322095647093*^9, + 3.630322097242449*^9}, {3.630322176329072*^9, 3.630322192772039*^9}, + 3.630322231534956*^9, {3.6303231357931433`*^9, 3.630323150170701*^9}, { + 3.632488075940673*^9, 3.632488120413509*^9}, {3.6324882328081903`*^9, + 3.632488236596348*^9}, {3.632488545113055*^9, 3.632488557103654*^9}, { + 3.832304244631546*^9, 3.8323042449118233`*^9}}, + CellLabel-> + "In[134]:=",ExpressionUUID->"716ee195-4767-4067-92df-43ed8da3bd15"], + +Cell[BoxData[ + RowBox[{"{", "6", "}"}]], "Output", + CellChangeTimes->{ + 3.832304252829917*^9, 3.832304414353018*^9, 3.8323044766343803`*^9, + 3.832304675181992*^9, 3.832304758656847*^9, 3.832304788848263*^9, + 3.832304823704341*^9, 3.832304919215414*^9, 3.832304957528509*^9, + 3.8323050100942993`*^9, 3.832305060814383*^9, 3.8323051041767406`*^9, + 3.832305150684971*^9, 3.8323052670345993`*^9, 3.832307399835104*^9, + 3.832308045512988*^9, 3.832308268930606*^9, 3.832308569017376*^9, { + 3.83230979560109*^9, 3.832309806289016*^9}, 3.8323102818921347`*^9, + 3.832310366544312*^9, 3.8323809824018517`*^9, 3.832492002479721*^9, + 3.832494705811585*^9, 3.8325916783555603`*^9, 3.832594972029954*^9, + 3.832596182134193*^9, 3.832597315921483*^9, 3.832599921098196*^9, + 3.8326016449186573`*^9, 3.832601735197879*^9, 3.8326041973641443`*^9, + 3.83260434364052*^9, 3.8326045115586023`*^9, 3.832604580015305*^9, + 3.832605150749875*^9, 3.8326439041718693`*^9, 3.8326440463130608`*^9, + 3.832644286295485*^9, 3.8326444028166027`*^9, 3.832644572749074*^9, + 3.8326447963960342`*^9, 3.832647147297798*^9, 3.832648788806467*^9}, + CellLabel-> + "Out[142]=",ExpressionUUID->"a3e65132-b2c3-448a-b0f5-576c81045092"] +}, Open ]], + +Cell[BoxData[{ + RowBox[{ + RowBox[{ + RowBox[{"JErrAirDensity", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "AIRDENSITY", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], + ";"}], "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JErrFThrust", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "FTHRUST", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JErrFDrag", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "FDRAG", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JErrFLat", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "FLAT", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"JErrFLift", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "FLIFT", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JErrMRoll", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "MROLL", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JErrMPitch", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "MPITCH", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"JErrMYaw", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "MYAW", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"JErrCBar", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "CBAR", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JErrCp", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "CP", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JErrIbd", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "IBD", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"JErrIbod", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "IBOD", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JErrWind", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "WINDW", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], + ";"}], "\[IndentingNewLine]"}], "Input", + CellChangeTimes->{{3.632488242861087*^9, 3.63248826986901*^9}, { + 3.632488302807823*^9, 3.632488310708211*^9}, {3.632549179684801*^9, + 3.6325491830062933`*^9}, 3.632549233454729*^9, {3.772359825953252*^9, + 3.772359833140972*^9}, {3.7723603795875998`*^9, 3.7723605440122347`*^9}, { + 3.772364327412218*^9, 3.772364391371952*^9}, {3.774583805536461*^9, + 3.774583830109449*^9}, {3.775206417689166*^9, 3.77520651396764*^9}, { + 3.77520681773381*^9, 3.7752068235363483`*^9}}, + CellLabel-> + "In[143]:=",ExpressionUUID->"cdb0dbd5-e787-49a0-bb76-c4468b401cbc"], + +Cell[BoxData[ + RowBox[{ + RowBox[{"Run", "[", "\"\\"", "]"}], ";"}]], "Input", + CellChangeTimes->{{3.605001578968874*^9, 3.605001586300552*^9}}, + CellLabel-> + "In[156]:=",ExpressionUUID->"41b82835-d8e1-446a-bec9-5cabfad38221"], + +Cell[BoxData[{ + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "Err", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrNoises", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrPose", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrQ", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrV", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrW", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrA", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrAlpha", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrAirDensity", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrFThrust", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrFDrag", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrFLat", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrFLift", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrMRoll", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrMPitch", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrMYaw", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrCBar", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrCp", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrIbd", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrIbod", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrWind", "]"}], ",", + "\"\\""}], "]"}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", + RowBox[{"Flatten", "[", + RowBox[{"{", + RowBox[{"SWhat", ",", "QWShat"}], "}"}], "]"}], "]"}], ",", + "\"\\""}], "]"}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "DBG", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", + RowBox[{"Flatten", "[", + RowBox[{"{", + RowBox[{"AhatlongDBG", ",", "AlphahatlongDBG"}], "}"}], "]"}], "]"}], + ",", "\"\\""}], "]"}], + ";"}]}], "Input", + CellChangeTimes->{{3.57329895973281*^9, 3.5732990549001513`*^9}, { + 3.57329915658729*^9, 3.5732991792126017`*^9}, {3.57329950483379*^9, + 3.573299506552189*^9}, {3.573456025938487*^9, 3.573456050810623*^9}, { + 3.574152257417694*^9, 3.574152270678878*^9}, {3.579848972820836*^9, + 3.579848980632938*^9}, {3.5814019928034163`*^9, 3.58140200617373*^9}, { + 3.581402211401102*^9, 3.581402211596108*^9}, {3.581413827590354*^9, + 3.5814138412416353`*^9}, {3.581414159871148*^9, 3.5814141678166847`*^9}, { + 3.5814153831753387`*^9, 3.581415395342497*^9}, 3.5817651746256*^9, { + 3.581765299311152*^9, 3.581765299456869*^9}, {3.583128034440399*^9, + 3.583128047560751*^9}, 3.600064481693171*^9, {3.6000645447460413`*^9, + 3.600064544768753*^9}, 3.605001560621763*^9, {3.606108718930592*^9, + 3.606108718954677*^9}, {3.606109458423176*^9, 3.6061094764764013`*^9}, { + 3.606109797740724*^9, 3.6061098090662613`*^9}, {3.6061099568596067`*^9, + 3.6061099585045424`*^9}, {3.617517858078869*^9, 3.6175178592248383`*^9}, { + 3.6175179081434183`*^9, 3.617517920863043*^9}, {3.617535815115308*^9, + 3.6175358335236387`*^9}, {3.6176913283286*^9, 3.617691368379365*^9}, { + 3.61915374051689*^9, 3.6191538003658953`*^9}, {3.624878788568387*^9, + 3.6248788166517553`*^9}, {3.6248826307742863`*^9, 3.624882631277665*^9}, { + 3.626606948234263*^9, 3.626606949108878*^9}, {3.626608154489799*^9, + 3.6266082045678787`*^9}, {3.626608392816053*^9, 3.62660839330686*^9}, { + 3.6297095365887547`*^9, 3.629709536615233*^9}, {3.629710549109893*^9, + 3.629710565390929*^9}, {3.630321740435486*^9, 3.630321740476594*^9}, { + 3.63032317635601*^9, 3.630323195570113*^9}, {3.632488330236774*^9, + 3.6324884066409388`*^9}, {3.632549186227305*^9, 3.6325491909421*^9}, { + 3.632636908066628*^9, 3.632636924909555*^9}, {3.633235060777438*^9, + 3.6332350814039917`*^9}, {3.634287350134149*^9, 3.63428738680274*^9}, { + 3.6342984069072943`*^9, 3.634298419729596*^9}, {3.63429849221801*^9, + 3.6342985327140083`*^9}, {3.772364411990938*^9, 3.7723645797179956`*^9}, + 3.831547472610442*^9, 3.8315475368389463`*^9}, + CellLabel-> + "In[157]:=",ExpressionUUID->"cbb713d0-b847-42c0-9c5d-8772380d8dba"], + +Cell[BoxData[{ + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]"}], "Input", + CellChangeTimes->CompressedData[" +1:eJwdzX8s1HEcBnBEQxZFZ9E6plzu1Oby+4/ap2K6VbtdW/NzhLYsxzqzqXR0 +4y4R405Za6qN8yNO6RqTybHFCYlbo3Z3urjs+/1SzsUW0ef9+ePZ649ne57A +zHzRNScHB4cIHLDxh2hO/oxCK5VviPxS5x1QRe13UGCvD/07AkbXWzigYUcv +AFPlE8TPGwvKGSxL6qECRzN2Kjgv8M6q5QFoK3OqAXtm3IixIw1BLh34z3uT ++MjcWrCpodBwTGAhWJ20O8axi0LJ5fpYkGkRZYHnhj8RvXwHt0C50kC0a6NP +xXdTyK+5nli1kLCtnKaQuKSIyH3n5azCGrUsoibHHOdvopAuc57YO9GmBrmC +khbwaMDsFPjkgmEazA2Sc01rFJqMUBBd2fab89hZJV8Cri+s0Fbs/UvRyyCL +lW7i2ylUVphmBkUGLeckVvjYygXt+R701jqFEsfERPemShuYUSL4A/YHpyQt +OdJIyHNNAxm6NTTchUaeee+JOkP/R3C0QTQO2prHv4PtjQEWsPWgzyH2Hhp1 +ex4gqswKNSgx5raAal2Nm8CDRmFn64hi2eVBMO5Lig48cfH86g0vGhVNCojZ +wWFb4NSxzm3w+JA6vQKreemeASbEZ7WBxcXtRBNPX6rFrkmU98BMnqV2dh+N +Bs5s1IEGdtfVr940erptzAb7vt0aMWN7a6P0oPTXh5+R/gwq7lyygYshkvGO +wwz6K+yZBH+P+cjGsFJ9FDHV39QEJvKsRE1fTqWEzSDLxGti9ZXku2I+g5x2 +lRMHZBoZqFh+S6zP9pvLw07HFxBtd4RGMO/0c+LejoZF8NX8bSvpQ1icqocM +ion0JSqlXDWrmkHhAaHE/woYvUU= + "], + CellLabel-> + "In[181]:=",ExpressionUUID->"1a2cbc4e-fbe6-4f7c-8741-b70e886cb2db"], + +Cell[CellGroupData[{ + +Cell[BoxData[ + RowBox[{"Run", "[", + "\"\\"", + "]"}]], "Input", + CellChangeTimes->{{3.6050014532183523`*^9, 3.6050015701885967`*^9}, { + 3.605001619532137*^9, 3.6050016309343433`*^9}, {3.626084473225601*^9, + 3.626084492513814*^9}, {3.6260847134763527`*^9, 3.6260847142374268`*^9}}, + CellLabel-> + "In[212]:=",ExpressionUUID->"92a89388-7fc0-4900-84db-870a39602d88"], + +Cell[BoxData["0"], "Output", + CellChangeTimes->{ + 3.626084595698902*^9, {3.626084692883334*^9, 3.6260847202544928`*^9}, + 3.626085055644226*^9, {3.6266082860420322`*^9, 3.626608294517626*^9}, + 3.6266083422477207`*^9, 3.626608408034442*^9, 3.6279705231130753`*^9, + 3.627974194334207*^9, 3.627976022316907*^9, 3.627982078483562*^9, + 3.627983299927937*^9, 3.627992180265356*^9, 3.627992659732891*^9, + 3.62799287386307*^9, 3.6282290101624613`*^9, 3.6297105708954773`*^9, + 3.6297106574368258`*^9, 3.6298061279971113`*^9, 3.629807720408*^9, + 3.63032321558445*^9, 3.6324884540770473`*^9, 3.632488624588769*^9, + 3.632548341659026*^9, 3.632548980662763*^9, 3.632549207713517*^9, + 3.6325492403682756`*^9, 3.632571746113782*^9, 3.632635561755374*^9, + 3.632636954213489*^9, 3.6328206639342012`*^9, 3.633066575735487*^9, + 3.633067332266951*^9, 3.633235102950844*^9, 3.633327218459009*^9, + 3.633668547850634*^9, 3.633669240128911*^9, 3.6342783048055973`*^9, + 3.634278361480673*^9, 3.634287411095068*^9, 3.6342924066725883`*^9, + 3.634298448119008*^9, {3.634298529980817*^9, 3.6342985373529863`*^9}, + 3.634298597389635*^9, 3.634299652755254*^9, 3.634299691384879*^9, + 3.634299857136918*^9, 3.634299889682819*^9, 3.634299958854151*^9, + 3.63430011568889*^9, {3.634300182757251*^9, 3.634300204063946*^9}, + 3.634300381734619*^9, 3.634301695788665*^9, 3.634302340168003*^9, + 3.634303008624929*^9, 3.6343039186453753`*^9, 3.634452382385035*^9, + 3.634452775895094*^9, 3.634980290608531*^9, 3.634980867087916*^9, { + 3.634982332576049*^9, 3.6349823386672897`*^9}, 3.771327259597065*^9, + 3.771569957428474*^9, 3.7723647052058573`*^9, 3.7724326260823793`*^9, + 3.772432982619478*^9, 3.772946110952771*^9, 3.773142622983836*^9, { + 3.7741789490039988`*^9, 3.774178957149096*^9}, 3.774239768380513*^9, + 3.774266326673469*^9, 3.77458390783916*^9, 3.774612092999992*^9, + 3.774691158272262*^9, 3.7746926287703743`*^9, {3.775206430535994*^9, + 3.775206518604731*^9}, 3.7752066826065903`*^9, 3.775206790200211*^9, + 3.775206827144771*^9, 3.781531524359655*^9, {3.781532274516631*^9, + 3.781532281751*^9}, {3.782109514640354*^9, 3.782109522477334*^9}, + 3.784535070682267*^9, 3.784537809638708*^9, 3.784537843760861*^9, + 3.784537914040141*^9, 3.820073727931135*^9, 3.831544438892476*^9, + 3.831548150530548*^9, 3.831548464699*^9, 3.831550705400441*^9, + 3.831552346161853*^9, 3.831997320121353*^9, 3.8320364983368073`*^9, + 3.8320405504472303`*^9, 3.8320481854565268`*^9, 3.832120936612749*^9, + 3.832248835365497*^9, 3.832248994613755*^9, 3.8322493453949966`*^9, + 3.832249630129558*^9, 3.8322496829962463`*^9, 3.832251733072578*^9, + 3.8322518897639847`*^9, 3.832254017905024*^9, 3.832255272371408*^9, + 3.832294229867085*^9, 3.8322989730148478`*^9, 3.832299579509521*^9, + 3.832299852685203*^9, 3.832303925882906*^9, 3.8323040960864477`*^9, + 3.832304137826005*^9, 3.832304257502684*^9, 3.8323044191014023`*^9, + 3.832304481446743*^9, 3.832304675951449*^9, {3.832304763545319*^9, + 3.832304789622552*^9}, 3.832304824929778*^9, 3.832304923988615*^9, + 3.832304962537294*^9, 3.832305012211985*^9, 3.832305062931774*^9, + 3.8323051054980927`*^9, 3.8323051522702417`*^9, 3.832305279164638*^9, + 3.8323074136604967`*^9, 3.832308059399588*^9, 3.832308280940714*^9, + 3.8323085747629337`*^9, {3.8323098008411922`*^9, 3.832309811466663*^9}, + 3.8323102877592707`*^9, 3.832310372335705*^9, 3.832380987827281*^9, + 3.832492015402467*^9, 3.832494711222521*^9, 3.8325916840551567`*^9, + 3.832594977839239*^9, 3.832596188432527*^9, 3.832597321933096*^9, + 3.832599927189682*^9, 3.8326016650358753`*^9, 3.8326017555851994`*^9, + 3.832604197894318*^9, 3.832604354903805*^9, 3.832604522899815*^9, + 3.832604590922518*^9, 3.832605156804879*^9, 3.832643925746409*^9, + 3.8326440676357603`*^9, 3.832644306421803*^9, 3.832644423549642*^9, + 3.8326445938251762`*^9, 3.8326448171647577`*^9, 3.8326471670070143`*^9, + 3.832648809027214*^9}, + CellLabel-> + "Out[212]=",ExpressionUUID->"09640270-79f6-4254-9784-9b45aac8d47e"] +}, Open ]], + +Cell[BoxData[{ + RowBox[{ + RowBox[{"Run", "[", "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"Run", "[", "\"\\"", "]"}], ";"}]}], "Input", + CellChangeTimes->{{3.8322994472360992`*^9, 3.832299451746531*^9}, { + 3.832300807121028*^9, 3.832300808616116*^9}, {3.83230446185632*^9, + 3.832304469358658*^9}, {3.832304948135626*^9, 3.832304949742977*^9}}, + CellLabel-> + "In[213]:=",ExpressionUUID->"aaff244c-3ad5-4faf-aeb3-7cdbaa9f50f5"], + +Cell[BoxData[" "], "Input", + CellChangeTimes->{3.627992683770735*^9}, + CellLabel-> + "In[215]:=",ExpressionUUID->"8714cb77-63af-49bd-8987-722049f040d7"] +}, +CellGrouping->Manual, +WindowSize->{1386., 782.25}, +WindowMargins->{{1440, Automatic}, {0, Automatic}}, +TaggingRules->{ + "WelcomeScreenSettings" -> {"FEStarting" -> False}, "TryRealOnly" -> False}, +Magnification:>0.9 Inherited, +FrontEndVersion->"12.2 for Linux x86 (64-bit) (December 12, 2020)", +StyleDefinitions->"Default.nb", +ExpressionUUID->"1d696320-bd25-4c6c-bc1e-4ec757dce194" +] +(* End of Notebook Content *) + +(* Internal cache information *) +(*CellTagsOutline +CellTagsIndex->{} +*) +(*CellTagsIndex +CellTagsIndex->{} +*) +(*NotebookFileOutline +Notebook[{ +Cell[558, 20, 220, 3, 28, "Input",ExpressionUUID->"28aebd44-7a23-4603-b370-38149a649cc1"], +Cell[781, 25, 554, 12, 48, "Input",ExpressionUUID->"d5e95da8-5675-4df2-8ff6-af7b8fb219a1"], +Cell[CellGroupData[{ +Cell[1360, 41, 242, 5, 46, "Subtitle",ExpressionUUID->"addb9730-72b2-4859-8e4b-0e776f073328", + InitializationCell->True], +Cell[1605, 48, 964, 30, 41, "Input",ExpressionUUID->"78e5840d-381c-47a9-b9dd-90c3c4dacb03", + InitializationCell->True], +Cell[2572, 80, 1654, 47, 43, "Input",ExpressionUUID->"66ac8851-0848-4866-8069-42c37f27b5de", + InitializationCell->True], +Cell[4229, 129, 944, 29, 62, "Input",ExpressionUUID->"a40aa98c-a7a2-4814-b63b-e7c90c9932a0", + InitializationCell->True], +Cell[5176, 160, 1652, 49, 41, "Input",ExpressionUUID->"ee9449fb-cd56-4179-b90c-f5e3bb0fdcdb", + InitializationCell->True], +Cell[6831, 211, 973, 25, 42, "Input",ExpressionUUID->"b69052f0-f728-4bd6-9bf7-bae15aa76095", + InitializationCell->True], +Cell[7807, 238, 1345, 40, 62, "Input",ExpressionUUID->"905c9ad7-8258-45b1-b64f-2d576f63cf20", + InitializationCell->True], +Cell[9155, 280, 2510, 65, 181, "Input",ExpressionUUID->"a7b9df83-8379-4a26-b658-cd16a2c680e3", + InitializationCell->True] +}, Open ]], +Cell[11680, 348, 188, 3, 89, "Title",ExpressionUUID->"243eeb2a-7eb6-46c5-afb4-9166e290b8da"], +Cell[11871, 353, 156, 3, 29, "Subsubtitle",ExpressionUUID->"934e8056-77ed-401a-af0c-38f7a1beea69"], +Cell[12030, 358, 517, 11, 28, "Input",ExpressionUUID->"89e76b07-ca4f-4879-a9de-3406cd562327"], +Cell[12550, 371, 1532, 40, 121, "Input",ExpressionUUID->"0f88cdc1-a39e-4d13-b528-8472e029b05f"], +Cell[14085, 413, 206, 4, 31, "Text",ExpressionUUID->"e1331a56-c19e-47fe-a1a5-206a0bc812e0"], +Cell[CellGroupData[{ +Cell[14316, 421, 648, 10, 26, "Input",ExpressionUUID->"c3fc8240-75b9-4e9c-b9f8-94b7f5b549ab"], +Cell[14967, 433, 1522, 22, 30, "Output",ExpressionUUID->"d349f066-e80e-4084-98af-ba9e3cf8e0e5"] +}, Open ]], +Cell[16504, 458, 1451, 32, 48, "Input",ExpressionUUID->"26067934-b88a-41ed-a094-38971131318d"], +Cell[17958, 492, 1610, 35, 47, "Input",ExpressionUUID->"7e8141a9-cc68-4d70-a634-12c34f72cfd4"], +Cell[19571, 529, 1531, 34, 47, "Input",ExpressionUUID->"dba75c2f-02b5-4068-8352-432d2c9ea9a3"], +Cell[CellGroupData[{ +Cell[21127, 567, 1180, 30, 83, "Input",ExpressionUUID->"c309ce02-68a6-472c-a37b-6aa6dd325b26"], +Cell[22310, 599, 150, 2, 30, "Output",ExpressionUUID->"f8dcc6e0-aafc-4a19-b381-1011d0ac9033"] +}, Open ]], +Cell[22475, 604, 159, 3, 29, "Subsubtitle",ExpressionUUID->"a1d3915d-f613-4425-8a04-fe27c5187298"], +Cell[22637, 609, 8700, 220, 927, "Input",ExpressionUUID->"33f47236-ad3c-4a61-8f93-8222aa6cc9fa"], +Cell[31340, 831, 4436, 127, 397, "Input",ExpressionUUID->"53a40cfc-f0e8-4e44-9f25-b052baa99c29"], +Cell[35779, 960, 463, 12, 28, "Input",ExpressionUUID->"230fd034-8fee-4db7-94bf-9c0c77325dce"], +Cell[36245, 974, 1691, 48, 142, "Input",ExpressionUUID->"3db40cdd-aae8-4aed-bbcf-7b6c917acc34"], +Cell[37939, 1024, 287, 5, 29, "Subsubtitle",ExpressionUUID->"3a22e419-d65c-4c9e-84c6-863e409e519a"], +Cell[38229, 1031, 208, 4, 31, "Text",ExpressionUUID->"9ac0d911-0e84-4579-bcb9-2110f48ca902"], +Cell[38440, 1037, 2112, 48, 65, "Input",ExpressionUUID->"9f81cfa6-a749-4450-bfb3-736066b50bce"], +Cell[40555, 1087, 154, 3, 31, "Text",ExpressionUUID->"656d798c-a5c9-4cc6-a1dd-06c8dc6c6565"], +Cell[40712, 1092, 2512, 53, 121, "Input",ExpressionUUID->"29b01d2f-83ef-4d5a-8a25-69d70c2e9207"], +Cell[43227, 1147, 157, 3, 31, "Text",ExpressionUUID->"7aa4501a-e764-4f85-8410-73dc94387049"], +Cell[43387, 1152, 2506, 60, 48, "Input",ExpressionUUID->"dbea6cef-0336-433e-92e3-cc59b4a69219"], +Cell[45896, 1214, 178, 3, 31, "Text",ExpressionUUID->"15e73123-e84e-4360-b55b-334ef918a821"], +Cell[46077, 1219, 2059, 51, 83, "Input",ExpressionUUID->"176642b9-ef21-428e-bda6-88b3064f0fe2"], +Cell[48139, 1272, 152, 3, 31, "Text",ExpressionUUID->"ac9144ba-14f4-4cb2-9d7c-1b84aa717d7b"], +Cell[48294, 1277, 717, 17, 46, "Input",ExpressionUUID->"f4931c36-9cb3-41d5-9636-f3fbcef428c9"], +Cell[49014, 1296, 154, 3, 31, "Text",ExpressionUUID->"57ce017d-559b-4d58-ba18-1e7631c436d9"], +Cell[49171, 1301, 411, 10, 46, "Input",ExpressionUUID->"63203ee0-9601-47b1-9606-b67b10ce9a46"], +Cell[49585, 1313, 156, 3, 31, "Text",ExpressionUUID->"4432b235-bb2b-4e9d-87f8-55bf93c648df"], +Cell[49744, 1318, 593, 14, 46, "Input",ExpressionUUID->"c73ee33c-6f07-4ea9-82e2-0f37fad73c8e"], +Cell[50340, 1334, 153, 3, 31, "Text",ExpressionUUID->"3b78e61b-a31c-43c6-949c-8ab46a558347"], +Cell[50496, 1339, 1726, 42, 83, "Input",ExpressionUUID->"7be4a0a1-b2c0-4d7b-b564-9f92dabf375f"], +Cell[52225, 1383, 154, 3, 31, "Text",ExpressionUUID->"78b600c8-17c9-43b2-8561-39fdff7b92e4"], +Cell[52382, 1388, 1204, 28, 65, "Input",ExpressionUUID->"4ce3e11d-b2d8-42df-94f3-f44671feb140"], +Cell[53589, 1418, 154, 3, 31, "Text",ExpressionUUID->"c302290e-b2df-4299-8d3e-f59d1fc850b6"], +Cell[53746, 1423, 740, 16, 46, "Input",ExpressionUUID->"1a28b43d-e9a2-4ebf-8251-f3ab2ddb7786"], +Cell[54489, 1441, 208, 4, 31, "Text",ExpressionUUID->"4ba37bc9-3ca4-4415-be72-e388f0dad47a"], +Cell[54700, 1447, 1571, 37, 102, "Input",ExpressionUUID->"a2375489-1864-4b87-9537-7325ce23183f"], +Cell[56274, 1486, 2610, 48, 46, "Input",ExpressionUUID->"fe419f0a-adf5-43af-9f08-7b8a7f19180b"], +Cell[58887, 1536, 1000, 17, 65, "Input",ExpressionUUID->"ff5f7d73-add6-4e24-9e55-7b15786660c6"], +Cell[59890, 1555, 2189, 55, 65, "Input",ExpressionUUID->"d17df8c1-9260-4a0c-8495-b5ef6f820848"], +Cell[62082, 1612, 847, 19, 46, "Input",ExpressionUUID->"8c91f72d-3f47-4547-a792-5e0813bcc352"], +Cell[62932, 1633, 204, 3, 29, "Subsubtitle",ExpressionUUID->"b92b1bb0-abe4-4170-8cfc-1e9dd8bc5679"], +Cell[63139, 1638, 956, 25, 83, "Input",ExpressionUUID->"dea35a84-3a0f-4600-a2f4-2ab045608d87"], +Cell[64098, 1665, 154, 3, 29, "Subsubtitle",ExpressionUUID->"bf1afef8-2e19-4e8e-9c26-8b9e596cd10c"], +Cell[64255, 1670, 206, 5, 28, "Input",ExpressionUUID->"c2f98c75-5faf-44c8-a958-147c2a660678"], +Cell[CellGroupData[{ +Cell[64486, 1679, 3827, 89, 177, "Input",ExpressionUUID->"716ee195-4767-4067-92df-43ed8da3bd15"], +Cell[68316, 1770, 1230, 19, 30, "Output",ExpressionUUID->"a3e65132-b2c3-448a-b0f5-576c81045092"] +}, Open ]], +Cell[69561, 1792, 3894, 118, 365, "Input",ExpressionUUID->"cdb0dbd5-e787-49a0-bb76-c4468b401cbc"], +Cell[73458, 1912, 243, 5, 26, "Input",ExpressionUUID->"41b82835-d8e1-446a-bec9-5cabfad38221"], +Cell[73704, 1919, 7277, 183, 523, "Input",ExpressionUUID->"cbb713d0-b847-42c0-9c5d-8772380d8dba"], +Cell[80984, 2104, 6393, 197, 700, "Input",ExpressionUUID->"1a2cbc4e-fbe6-4f7c-8741-b70e886cb2db"], +Cell[CellGroupData[{ +Cell[87402, 2305, 427, 8, 28, "Input",ExpressionUUID->"92a89388-7fc0-4900-84db-870a39602d88"], +Cell[87832, 2315, 4101, 59, 30, "Output",ExpressionUUID->"09640270-79f6-4254-9784-9b45aac8d47e"] +}, Open ]], +Cell[91948, 2377, 486, 10, 46, "Input",ExpressionUUID->"aaff244c-3ad5-4faf-aeb3-7cdbaa9f50f5"], +Cell[92437, 2389, 152, 3, 57, "Input",ExpressionUUID->"8714cb77-63af-49bd-8987-722049f040d7"] +} +] +*) + diff --git a/_development/Mathematica/models/AugmentedState_PlaneDynamicModel_corrected.nb b/_development/Mathematica/models/AugmentedState_PlaneDynamicModel_corrected.nb new file mode 100644 index 0000000..e4f3797 --- /dev/null +++ b/_development/Mathematica/models/AugmentedState_PlaneDynamicModel_corrected.nb @@ -0,0 +1,2636 @@ +(* Content-type: application/vnd.wolfram.mathematica *) + +(*** Wolfram Notebook File ***) +(* http://www.wolfram.com/nb *) + +(* CreatedBy='Mathematica 10.0' *) + +(*CacheID: 234*) +(* Internal cache information: +NotebookFileLineBreakTest +NotebookFileLineBreakTest +NotebookDataPosition[ 158, 7] +NotebookDataLength[ 100742, 2628] +NotebookOptionsPosition[ 94245, 2536] +NotebookOutlinePosition[ 94683, 2554] +CellTagsIndexPosition[ 94640, 2551] +WindowFrame->Normal*) + +(* Beginning of Notebook Content *) +Notebook[{ +Cell[BoxData[ + RowBox[{"Clear", "[", "\"\\"", "]"}]], "Input", + CellChangeTimes->{{3.5951624068865957`*^9, 3.595162434106801*^9}}, + CellLabel-> + "In[1194]:=",ExpressionUUID->"28aebd44-7a23-4603-b370-38149a649cc1"], + +Cell[BoxData[{ + RowBox[{"Import", "[", + RowBox[{ + RowBox[{"NotebookDirectory", "[", "]"}], "<>", + "\"\\""}], "]"}], "\[IndentingNewLine]", + RowBox[{"Import", "[", + RowBox[{ + RowBox[{"NotebookDirectory", "[", "]"}], "<>", + "\"\\""}], "]"}]}], "Input", + CellChangeTimes->{{3.595163228510494*^9, 3.595163230854044*^9}, { + 3.6175174204357758`*^9, 3.617517424343709*^9}, {3.632487617988913*^9, + 3.632487633951275*^9}}, + CellLabel-> + "In[1195]:=",ExpressionUUID->"d5e95da8-5675-4df2-8ff6-af7b8fb219a1"], + +Cell[CellGroupData[{ + +Cell["Quaternion Functions", "Subtitle", + InitializationCell->True, + CellChangeTimes->{{3.563606408899285*^9, 3.563606413402224*^9}, { + 3.572951250149337*^9, + 3.572951255936389*^9}},ExpressionUUID->"addb9730-72b2-4859-8e4b-\ +0e776f073328"], + +Cell[BoxData[ + RowBox[{ + RowBox[{"VectProdMatrix", "[", "v_", "]"}], ":=", + RowBox[{"{", + RowBox[{ + RowBox[{"{", + RowBox[{"0", ",", + RowBox[{"-", + RowBox[{"v", "[", + RowBox[{"[", "3", "]"}], "]"}]}], ",", + RowBox[{"v", "[", + RowBox[{"[", "2", "]"}], "]"}]}], "}"}], ",", + RowBox[{"{", + RowBox[{ + RowBox[{"v", "[", + RowBox[{"[", "3", "]"}], "]"}], ",", "0", ",", + RowBox[{"-", + RowBox[{"v", "[", + RowBox[{"[", "1", "]"}], "]"}]}]}], "}"}], ",", + RowBox[{"{", + RowBox[{ + RowBox[{"-", + RowBox[{"v", "[", + RowBox[{"[", "2", "]"}], "]"}]}], ",", + RowBox[{"v", "[", + RowBox[{"[", "1", "]"}], "]"}], ",", "0"}], "}"}]}], "}"}]}]], "Input",\ + + InitializationCell->True, + CellChangeTimes->{{3.563607037606062*^9, 3.563607124416689*^9}, + 3.563608231083735*^9}, + CellLabel-> + "In[1197]:=",ExpressionUUID->"78e5840d-381c-47a9-b9dd-90c3c4dacb03"], + +Cell[BoxData[ + RowBox[{ + RowBox[{"PertStateSubs", "[", + RowBox[{"X_", ",", "dX_"}], "]"}], ":=", + RowBox[{"Flatten", "[", + RowBox[{"{", + RowBox[{ + RowBox[{"Table", "[", + RowBox[{ + RowBox[{ + RowBox[{"X", "[", + RowBox[{"[", "i", "]"}], "]"}], "\[Rule]", + RowBox[{ + RowBox[{"X", "[", + RowBox[{"[", "i", "]"}], "]"}], "+", + RowBox[{"dX", "[", + RowBox[{"[", "i", "]"}], "]"}]}]}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", "3"}], "}"}]}], "]"}], ",", + RowBox[{"Table", "[", + RowBox[{ + RowBox[{ + RowBox[{"X", "[", + RowBox[{"[", + RowBox[{"i", "+", "3"}], "]"}], "]"}], "\[Rule]", + RowBox[{ + RowBox[{"(", + RowBox[{"QuatInc", "[", + RowBox[{ + RowBox[{"X", "[", + RowBox[{"[", + RowBox[{"{", + RowBox[{"4", ",", "5", ",", "6", ",", "7"}], "}"}], "]"}], + "]"}], ",", + RowBox[{"dX", "[", + RowBox[{"[", + RowBox[{"{", + RowBox[{"4", ",", "5", ",", "6"}], "}"}], "]"}], "]"}]}], + "]"}], ")"}], "[", + RowBox[{"[", "i", "]"}], "]"}]}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", "4"}], "}"}]}], "]"}]}], "}"}], + "]"}]}]], "Input", + InitializationCell->True, + CellChangeTimes->{{3.571130663264105*^9, 3.571130765329321*^9}, { + 3.57113111793888*^9, 3.57113116341093*^9}, {3.571131195958424*^9, + 3.571131254626845*^9}, 3.571133049520565*^9}, + CellLabel-> + "In[1198]:=",ExpressionUUID->"66ac8851-0848-4866-8069-42c37f27b5de"], + +Cell[BoxData[{ + RowBox[{ + RowBox[{ + RowBox[{"L2", "[", "x_", "]"}], ":=", + RowBox[{"Sqrt", "[", + RowBox[{"Sum", "[", + RowBox[{ + RowBox[{ + RowBox[{"x", "[", + RowBox[{"[", "j", "]"}], "]"}], "^", "2"}], ",", + RowBox[{"{", + RowBox[{"j", ",", "1", ",", + RowBox[{"Length", "[", "x", "]"}]}], "}"}]}], "]"}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"VecNorm", "[", "x_", "]"}], ":=", + RowBox[{"Table", "[", + RowBox[{ + RowBox[{ + RowBox[{"x", "[", + RowBox[{"[", "i", "]"}], "]"}], "/", + RowBox[{"L2", "[", "x", "]"}]}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", + RowBox[{"Length", "[", "x", "]"}]}], "}"}]}], "]"}]}], ";"}]}], "Input",\ + + InitializationCell->True, + CellChangeTimes->{{3.568453075302482*^9, 3.568453143924776*^9}}, + CellLabel-> + "In[1199]:=",ExpressionUUID->"a40aa98c-a7a2-4814-b63b-e7c90c9932a0"], + +Cell[BoxData[ + RowBox[{ + RowBox[{ + RowBox[{"SkewMatrix", "[", "v_", "]"}], ":=", + RowBox[{ + RowBox[{"{", + RowBox[{ + RowBox[{"{", + RowBox[{"0", ",", + RowBox[{"-", + RowBox[{"v", "[", + RowBox[{"[", "1", "]"}], "]"}]}], ",", + RowBox[{"-", + RowBox[{"v", "[", + RowBox[{"[", "2", "]"}], "]"}]}], ",", + RowBox[{"-", + RowBox[{"v", "[", + RowBox[{"[", "3", "]"}], "]"}]}]}], "}"}], ",", + RowBox[{"{", + RowBox[{ + RowBox[{"v", "[", + RowBox[{"[", "1", "]"}], "]"}], ",", "0", ",", + RowBox[{"v", "[", + RowBox[{"[", "3", "]"}], "]"}], ",", + RowBox[{"-", + RowBox[{"v", "[", + RowBox[{"[", "2", "]"}], "]"}]}]}], "}"}], ",", + RowBox[{"{", + RowBox[{ + RowBox[{"v", "[", + RowBox[{"[", "2", "]"}], "]"}], ",", + RowBox[{"-", + RowBox[{"v", "[", + RowBox[{"[", "3", "]"}], "]"}]}], ",", "0", ",", + RowBox[{"v", "[", + RowBox[{"[", "1", "]"}], "]"}]}], "}"}], ",", + RowBox[{"{", + RowBox[{ + RowBox[{"v", "[", + RowBox[{"[", "3", "]"}], "]"}], ",", + RowBox[{"v", "[", + RowBox[{"[", "2", "]"}], "]"}], ",", + RowBox[{"-", + RowBox[{"v", "[", + RowBox[{"[", "1", "]"}], "]"}]}], ",", "0"}], "}"}]}], "}"}], "/", + "2"}]}], ";"}]], "Input", + InitializationCell->True, + CellChangeTimes->{{3.568443187261707*^9, 3.568443273520089*^9}, { + 3.568458568630984*^9, 3.568458568967643*^9}}, + CellLabel-> + "In[1201]:=",ExpressionUUID->"ee9449fb-cd56-4179-b90c-f5e3bb0fdcdb"], + +Cell[BoxData[ + RowBox[{ + RowBox[{ + RowBox[{"ConstantAngularSpeeDtMatrix", "[", + RowBox[{"w_", ",", "Dt_"}], "]"}], ":=", + RowBox[{ + RowBox[{ + RowBox[{"Cos", "[", + RowBox[{ + RowBox[{"L2", "[", "w", "]"}], + RowBox[{"Dt", "/", "2"}]}], "]"}], " ", + RowBox[{"IdentityMatrix", "[", "4", "]"}]}], "+", + RowBox[{"2", + RowBox[{ + RowBox[{"Sin", "[", + RowBox[{ + RowBox[{"L2", "[", "w", "]"}], + RowBox[{"Dt", "/", "2"}]}], "]"}], "/", + RowBox[{"L2", "[", "w", "]"}]}], " ", + RowBox[{"SkewMatrix", "[", "w", "]"}]}]}]}], ";"}]], "Input", + InitializationCell->True, + CellChangeTimes->{{3.5684429732904*^9, 3.56844300516892*^9}, { + 3.568443103374373*^9, 3.568443183239631*^9}, {3.56844338461282*^9, + 3.568443409373211*^9}, {3.568453008511107*^9, 3.568453014783046*^9}, { + 3.60835655838758*^9, 3.608356558395439*^9}}, + CellLabel-> + "In[1202]:=",ExpressionUUID->"b69052f0-f728-4bd6-9bf7-bae15aa76095"], + +Cell[BoxData[{ + RowBox[{ + RowBox[{ + RowBox[{"SubVec", "[", + RowBox[{"exp_", ",", "v1_", ",", "v2_"}], "]"}], ":=", + RowBox[{"exp", "/.", + RowBox[{"Table", "[", + RowBox[{ + RowBox[{ + RowBox[{"v1", "[", + RowBox[{"[", "i", "]"}], "]"}], "\[Rule]", + RowBox[{"v2", "[", + RowBox[{"[", "i", "]"}], "]"}]}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", + RowBox[{ + RowBox[{"Dimensions", "[", + RowBox[{"v1", ",", "1"}], "]"}], "[", + RowBox[{"[", "1", "]"}], "]"}]}], "}"}]}], "]"}]}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"VecToZero", "[", + RowBox[{"exp_", ",", "v_"}], "]"}], ":=", + RowBox[{"exp", "/.", + RowBox[{"Table", "[", + RowBox[{ + RowBox[{ + RowBox[{"v", "[", + RowBox[{"[", "i", "]"}], "]"}], "\[Rule]", "0"}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", + RowBox[{ + RowBox[{"Dimensions", "[", + RowBox[{"v", ",", "1"}], "]"}], "[", + RowBox[{"[", "1", "]"}], "]"}]}], "}"}]}], "]"}]}]}], + ";"}]}], "Input", + InitializationCell->True, + CellChangeTimes->{{3.568444740747602*^9, 3.568444776506934*^9}, { + 3.568444838483142*^9, 3.568444844345901*^9}}, + CellLabel-> + "In[1203]:=",ExpressionUUID->"905c9ad7-8258-45b1-b64f-2d576f63cf20"], + +Cell[BoxData[{ + RowBox[{ + RowBox[{"ToGoodC", "[", "exp_", "]"}], ":=", + RowBox[{"Module", "[", + RowBox[{ + RowBox[{"{", "oexp", "}"}], ",", " ", + RowBox[{ + RowBox[{"oexp", "=", + RowBox[{"Experimental`OptimizeExpression", "[", "exp", "]"}]}], ";", + "\[IndentingNewLine]", + RowBox[{"If", "[", + RowBox[{ + RowBox[{ + RowBox[{ + RowBox[{"Dimensions", " ", "[", + RowBox[{ + RowBox[{"StringPosition", "[", + RowBox[{ + RowBox[{"ToString", "[", + RowBox[{"InputForm", "[", "oexp", "]"}], "]"}], ",", + "\"\\""}], "]"}], ",", "1"}], "]"}], "[", + RowBox[{"[", "1", "]"}], "]"}], ">", "0"}], ",", + RowBox[{"Block", "[", + RowBox[{ + RowBox[{"{", " ", + RowBox[{"locals", ",", " ", "code"}], "}"}], ",", + RowBox[{ + RowBox[{ + RowBox[{"{", + RowBox[{"locals", ",", "code"}], "}"}], "=", + RowBox[{"ReleaseHold", "[", + RowBox[{ + RowBox[{"(", + RowBox[{"Hold", "@@", "oexp"}], ")"}], "/.", + RowBox[{ + RowBox[{ + RowBox[{"Verbatim", "[", "Block", "]"}], "[", + RowBox[{"vars_", ",", "seq_"}], "]"}], "\[RuleDelayed]", + RowBox[{"{", + RowBox[{"vars", ",", + RowBox[{"Hold", "[", "seq", "]"}]}], "}"}]}]}], "]"}]}], ";", + + RowBox[{"ToString", "[", + RowBox[{"CForm", "[", "code", "]"}], "]"}]}]}], "]"}], + "\[IndentingNewLine]", ",", " ", + RowBox[{"ToString", "[", + RowBox[{"CForm", "[", "exp", "]"}], "]"}]}], "]"}]}]}], + "]"}]}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{"str_", ",", "file_"}], "]"}], ":=", + RowBox[{"Module", "[", + RowBox[{ + RowBox[{"{", "stream", "}"}], ",", + RowBox[{ + RowBox[{"stream", "=", + RowBox[{"OpenWrite", "[", "file", "]"}]}], ";", + RowBox[{"WriteString", "[", + RowBox[{"stream", ",", "str"}], "]"}], ";", + RowBox[{"Close", "[", "stream", "]"}], ";"}]}], "]"}]}]}], "Input", + InitializationCell->True, + CellChangeTimes->{ + 3.568440350374321*^9, {3.56844481385726*^9, 3.56844481884285*^9}, { + 3.568446816971497*^9, 3.568446831371454*^9}, {3.56844689667688*^9, + 3.568446897039928*^9}, {3.571133081827365*^9, 3.571133085326789*^9}}, + CellLabel-> + "In[1205]:=",ExpressionUUID->"a7b9df83-8379-4a26-b658-cd16a2c680e3"] +}, Open ]], + +Cell["Beginning of stuff", "Title", + CellChangeTimes->{{3.5951624112710238`*^9, 3.5951624314967413`*^9}, + 3.595309863589487*^9},ExpressionUUID->"243eeb2a-7eb6-46c5-afb4-\ +9166e290b8da"], + +Cell["State", "Subsubtitle", + CellChangeTimes->{{3.5732982476551723`*^9, + 3.573298257964068*^9}},ExpressionUUID->"934e8056-77ed-401a-af0c-\ +38f7a1beea69"], + +Cell[BoxData[ + RowBox[{ + RowBox[{"X", "=", + RowBox[{"Table", "[", + RowBox[{ + RowBox[{"x", "[", "i", "]"}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", "26"}], "}"}]}], "]"}]}], ";"}]], "Input", + CellChangeTimes->{{3.5732982634483347`*^9, 3.573298275094433*^9}, { + 3.606108793708241*^9, 3.6061087941202383`*^9}}, + CellLabel-> + "In[1207]:=",ExpressionUUID->"89e76b07-ca4f-4879-a9de-3406cd562327"], + +Cell[BoxData[{ + RowBox[{ + RowBox[{"SW", "=", + RowBox[{"X", "[", + RowBox[{"[", + RowBox[{"1", ";;", "3"}], "]"}], "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"QWS", "=", + RowBox[{"X", "[", + RowBox[{"[", + RowBox[{"4", ";;", "7"}], "]"}], "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"VS", "=", + RowBox[{"X", "[", + RowBox[{"[", + RowBox[{"8", ";;", "10"}], "]"}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"WS", "=", + RowBox[{"X", "[", + RowBox[{"[", + RowBox[{"11", ";;", "13"}], "]"}], "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"AS", "=", + RowBox[{"X", "[", + RowBox[{"[", + RowBox[{"14", ";;", "16"}], "]"}], "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"ALPHAS", "=", + RowBox[{"X", "[", + RowBox[{"[", + RowBox[{"17", ";;", "19"}], "]"}], "]"}]}], ";"}]}], "Input", + CellChangeTimes->{{3.632463823621646*^9, 3.632463843454915*^9}, { + 3.632463896430626*^9, 3.632463897928619*^9}, {3.632464021491871*^9, + 3.632464039150372*^9}, {3.6324668182980833`*^9, 3.6324668268371*^9}, { + 3.63248479427204*^9, 3.632484802957326*^9}, {3.6324860670961533`*^9, + 3.6324860672576303`*^9}, 3.6324863191229258`*^9, {3.632487946501395*^9, + 3.6324879678038177`*^9}, {3.632488008725754*^9, 3.632488019377554*^9}}, + CellLabel-> + "In[1208]:=",ExpressionUUID->"0f88cdc1-a39e-4d13-b528-8472e029b05f"], + +Cell["Parameters", "Subsubtitle", + CellChangeTimes->{{3.576920320535687*^9, + 3.576920325728017*^9}},ExpressionUUID->"a1d3915d-f613-4425-8a04-\ +fe27c5187298"], + +Cell[BoxData[{ + RowBox[{ + RowBox[{"h0", " ", "=", + RowBox[{"airDensity", "[", "1", "]"}]}], ";", + RowBox[{"(*", " ", + RowBox[{ + "base", " ", "point", " ", "altitude", " ", "above", " ", "sea", " ", + "level"}], " ", "*)"}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"T0", "=", + RowBox[{"airDensity", "[", "2", "]"}]}], ";", " ", + RowBox[{"(*", " ", + RowBox[{ + "ambient", " ", "temperature", " ", "at", " ", "mean", " ", "sea", " ", + "level"}], " ", "*)"}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"p0", "=", + RowBox[{"airDensity", "[", "3", "]"}]}], ";", " ", + RowBox[{"(*", " ", + RowBox[{"pressure", " ", "at", " ", "sea", " ", "level"}], " ", "*)"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"a", " ", "=", " ", + RowBox[{ + RowBox[{"-", "6.5"}], " ", + RowBox[{"10", "^", + RowBox[{"-", "3"}]}]}]}], ";", " ", + RowBox[{"(*", " ", + RowBox[{"temperature", " ", "gradient"}], " ", "*)"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"Ra", " ", "=", " ", "287.30"}], ";", " ", + RowBox[{"(*", " ", + RowBox[{"gas", " ", "constant", " ", "for", " ", "air"}], " ", "*)"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"g", "=", " ", + RowBox[{"gravity", "[", "1", "]"}]}], ";", " ", + RowBox[{"(*", " ", "gravity", " ", "*)"}], ";"}], " ", + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"CFT1", "=", + RowBox[{"fThrust", "[", "1", "]"}]}], ";", " ", + RowBox[{"(*", " ", + RowBox[{"thrust", " ", "force", " ", "coefficients"}], "*)"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"CFT2", "=", + RowBox[{"fThrust", "[", "2", "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"CFT3", "=", + RowBox[{"fThrust", "[", "3", "]"}]}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"CFX1", "=", + RowBox[{"fDrag", "[", "1", "]"}]}], ";", " ", + RowBox[{"(*", " ", + RowBox[{"drag", " ", "force", " ", "coefficients"}], " ", "*)"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"CFXalpha1", " ", "=", " ", + RowBox[{"fDrag", "[", "2", "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"CFXalpha2", " ", "=", " ", + RowBox[{"fDrag", "[", "3", "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"CFXbeta2", " ", "=", " ", + RowBox[{"fDrag", "[", "4", "]"}]}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"CFY1", "=", + RowBox[{"fLat", "[", "1", "]"}]}], ";", " ", + RowBox[{"(*", " ", + RowBox[{"lateral", " ", "force", " ", "coefficient"}], " ", "*)"}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"CFZ1", "=", + RowBox[{"fLift", "[", "1", "]"}]}], ";", " ", + RowBox[{"(*", " ", + RowBox[{"lift", " ", "force", " ", "coefficients"}], " ", "*)"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"CFZalpha", "=", + RowBox[{"fLift", "[", "2", "]"}]}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"CMXa", "=", + RowBox[{"mRoll", "[", "1", "]"}]}], ";", + RowBox[{"(*", " ", + RowBox[{"roll", " ", "moment", " ", "coefficients"}], " ", "*)"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"CMXbeta", "=", + RowBox[{"mRoll", "[", "2", "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"CMX\[Omega]tildex", "=", + RowBox[{"mRoll", "[", "3", "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"CMX\[Omega]tildez", "=", + RowBox[{"mRoll", "[", "4", "]"}]}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"CMY1", "=", + RowBox[{"mPitch", "[", "1", "]"}]}], ";", + RowBox[{"(*", " ", + RowBox[{"pitch", " ", "moment", " ", "coefficients"}], " ", "*)"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"CMYe", "=", + RowBox[{"mPitch", "[", "2", "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"CMYalpha", "=", + RowBox[{"mPitch", "[", "3", "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"CMY\[Omega]tildey", "=", + RowBox[{"mPitch", "[", "4", "]"}]}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"CMZr", "=", + RowBox[{"mYaw", "[", "1", "]"}]}], ";"}], " ", + RowBox[{"(*", " ", + RowBox[{"yaw", " ", "moment", " ", "coefficients"}], " ", "*)"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"CMZbeta", "=", + RowBox[{"mYaw", "[", "2", "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"CMZ\[Omega]tildez", "=", + RowBox[{"mYaw", "[", "3", "]"}]}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"cbar", " ", "=", " ", + RowBox[{"cBar", "[", "1", "]"}]}], ";", " ", + RowBox[{"(*", " ", + RowBox[{"mean", " ", "aereodynamic", " ", "chord"}], " ", "*)"}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"PD", "=", + RowBox[{"cp", "[", "1", "]"}]}], ";", " ", + RowBox[{"(*", " ", + RowBox[{"propeller", " ", "diameter"}], " ", "*)"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"S", "=", + RowBox[{"cp", "[", "2", "]"}]}], ";", " ", + RowBox[{"(*", " ", + RowBox[{"wing", " ", "surface"}], " ", "*)"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"b", "=", + RowBox[{"cp", "[", "3", "]"}]}], ";", " ", + RowBox[{"(*", " ", + RowBox[{"wind", " ", "span"}], " ", "*)"}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"m", " ", "=", " ", + RowBox[{"cp", "[", "4", "]"}]}], ";", " ", + RowBox[{"(*", " ", "mass", " ", "*)"}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"Ixx", "=", + RowBox[{"ibd", "[", "1", "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"Iyy", "=", + RowBox[{"ibd", "[", "2", "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"Izz", " ", "=", + RowBox[{"ibd", "[", "3", "]"}]}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"Ixz", "=", + RowBox[{"ibod", "[", "1", "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"Ixy", "=", + RowBox[{"ibod", "[", "2", "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"Iyz", "=", + RowBox[{"ibod", "[", "3", "]"}]}], ";"}]}], "Input", + CellChangeTimes->{{3.630156966488817*^9, 3.630156997327407*^9}, { + 3.630240787595443*^9, 3.630240871119405*^9}, {3.632462563480411*^9, + 3.6324626574790983`*^9}, {3.6324627822557497`*^9, + 3.6324627840911913`*^9}, {3.6324628429667397`*^9, 3.632462845395872*^9}, { + 3.632463452013672*^9, 3.632463468595188*^9}, {3.6324635424969807`*^9, + 3.632463578847823*^9}, {3.6324638570723257`*^9, 3.632463892398457*^9}, + 3.6324640836270533`*^9, {3.632465074140349*^9, 3.632465180642975*^9}, { + 3.632465229204035*^9, 3.632465255301177*^9}, {3.632465722100479*^9, + 3.632465736568845*^9}, {3.6324658501384373`*^9, 3.632465874972086*^9}, { + 3.632466231247014*^9, 3.6324662436985607`*^9}, {3.632466293804319*^9, + 3.63246666807226*^9}, {3.632466702556988*^9, 3.632466739506076*^9}, { + 3.6324669714561977`*^9, 3.632466989419321*^9}, {3.6324671749054317`*^9, + 3.632467191927196*^9}, {3.632467242245677*^9, 3.632467340635708*^9}, { + 3.632467371717039*^9, 3.632467375747436*^9}, {3.6324674914282084`*^9, + 3.632467492058816*^9}, {3.6324735043656588`*^9, 3.632473540164185*^9}, + 3.632473659229198*^9, {3.6324739348426247`*^9, 3.632473970395046*^9}, { + 3.632474373588602*^9, 3.63247449276618*^9}, {3.632484917459989*^9, + 3.6324849486938457`*^9}, {3.632485035091144*^9, 3.632485038949534*^9}, { + 3.6324859054962873`*^9, 3.632485963402033*^9}, {3.632486128812416*^9, + 3.632486192068241*^9}, {3.6325479968094463`*^9, 3.632548014105666*^9}, { + 3.6325480606534348`*^9, 3.632548077440715*^9}, {3.632563365761787*^9, + 3.6325633685352783`*^9}, {3.634301394539613*^9, 3.6343014011226387`*^9}, { + 3.6343016879675493`*^9, 3.634301688364636*^9}, {3.63430226729615*^9, + 3.634302271560999*^9}, 3.634302333067884*^9, 3.634303000998124*^9, { + 3.634303882252087*^9, 3.634303882568714*^9}, 3.634303913676345*^9, { + 3.7715699472059917`*^9, 3.771569949298133*^9}, {3.772359310486154*^9, + 3.772359365392565*^9}, {3.7723594227641973`*^9, 3.772359613795624*^9}, { + 3.772360215586792*^9, 3.77236021597759*^9}, {3.77314250558466*^9, + 3.773142505699256*^9}, {3.774589936393152*^9, 3.774589945751444*^9}, { + 3.777109991948604*^9, 3.777109997353167*^9}, {3.820073719683056*^9, + 3.820073720934869*^9}}, + CellLabel-> + "In[1214]:=",ExpressionUUID->"33f47236-ad3c-4a61-8f93-8222aa6cc9fa"], + +Cell[BoxData[{ + RowBox[{ + RowBox[{ + RowBox[{ + RowBox[{"IB", "=", + RowBox[{"{", + RowBox[{ + RowBox[{"{", + RowBox[{"Ixx", ",", "Ixy", ",", "Ixz"}], "}"}], ",", + RowBox[{"{", + RowBox[{"Ixy", ",", "Iyy", ",", "Iyz"}], "}"}], ",", + RowBox[{"{", + RowBox[{"Ixz", ",", "Iyz", ",", "Izz"}], "}"}]}], "}"}]}], ";"}], " ", + RowBox[{"(*", " ", + RowBox[{"Inertia", " ", "matrix"}], " ", "*)"}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"AIRDENSITY", " ", "=", " ", + RowBox[{"Table", "[", + RowBox[{ + RowBox[{"airDensity", "[", "i", "]"}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", "3"}], "}"}]}], "]"}]}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"FTHRUST", " ", "=", " ", + RowBox[{"Table", "[", + RowBox[{ + RowBox[{"fThrust", "[", "i", "]"}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", "3"}], "}"}]}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"FDRAG", " ", "=", " ", + RowBox[{"Table", "[", + RowBox[{ + RowBox[{"fDrag", "[", "i", "]"}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", "4"}], "}"}]}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"FLAT", " ", "=", " ", + RowBox[{"Table", "[", + RowBox[{ + RowBox[{"fLat", "[", "i", "]"}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", "1"}], "}"}]}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"FLIFT", "=", " ", + RowBox[{"Table", "[", + RowBox[{ + RowBox[{"fLift", "[", "i", "]"}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", "2"}], "}"}]}], "]"}]}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MROLL", " ", "=", " ", + RowBox[{"Table", "[", + RowBox[{ + RowBox[{"mRoll", "[", "i", "]"}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", "4"}], "}"}]}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MPITCH", " ", "=", " ", + RowBox[{"Table", "[", + RowBox[{ + RowBox[{"mPitch", "[", "i", "]"}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", "4"}], "}"}]}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"MYAW", " ", "=", " ", + RowBox[{"Table", "[", + RowBox[{ + RowBox[{"mYaw", "[", "i", "]"}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", "3"}], "}"}]}], "]"}]}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"CBAR", " ", "=", " ", + RowBox[{"Table", "[", + RowBox[{ + RowBox[{"cBar", "[", "i", "]"}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", "1"}], "}"}]}], "]"}]}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"CP", " ", "=", " ", + RowBox[{"Table", "[", + RowBox[{ + RowBox[{"cp", "[", "i", "]"}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", "4"}], "}"}]}], "]"}]}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"IBD", " ", "=", " ", + RowBox[{"Table", "[", + RowBox[{ + RowBox[{"ibd", "[", "i", "]"}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", "3"}], "}"}]}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"IBOD", " ", "=", " ", + RowBox[{"Table", "[", + RowBox[{ + RowBox[{"ibod", "[", "i", "]"}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", "3"}], "}"}]}], "]"}]}], + ";"}], "\[IndentingNewLine]"}], "Input", + CellChangeTimes->{{3.6324667198820887`*^9, 3.632466771730083*^9}, { + 3.6324745054453506`*^9, 3.632474645927088*^9}, {3.6324746840634813`*^9, + 3.6324746893289757`*^9}, {3.632484902639776*^9, 3.6324849084840813`*^9}, { + 3.6324849547663603`*^9, 3.632485026020185*^9}, {3.632485970613168*^9, + 3.6324859838780127`*^9}, {3.632486046079789*^9, 3.632486046409316*^9}, { + 3.632486196852049*^9, 3.632486214881503*^9}, 3.6325480929677277`*^9, { + 3.6325633642283154`*^9, 3.632563371518127*^9}, {3.772359621486253*^9, + 3.772359671724037*^9}, {3.772359705820054*^9, 3.772359711259782*^9}, { + 3.772359937605813*^9, 3.772360048550315*^9}, {3.7723600966964912`*^9, + 3.772360359842062*^9}, {3.775206708566532*^9, 3.775206780566957*^9}}, + CellLabel-> + "In[1252]:=",ExpressionUUID->"53a40cfc-f0e8-4e44-9f25-b052baa99c29"], + +Cell[BoxData[ + RowBox[{ + RowBox[{ + RowBox[{"WINDW", "=", + RowBox[{"Table", "[", + RowBox[{ + RowBox[{"wind", "[", "i", "]"}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", "3"}], "}"}]}], "]"}]}], ";"}], + RowBox[{"(*", " ", + RowBox[{"wind", " ", "speed", " ", "in", " ", "world", " ", "frame"}], " ", + "*)"}]}]], "Input", + CellLabel-> + "In[1265]:=",ExpressionUUID->"be98cbcc-62a9-4b36-a592-7c1827bbba19"], + +Cell[BoxData[{ + RowBox[{ + RowBox[{ + RowBox[{"n", " ", "=", " ", + RowBox[{"z", "[", "1", "]"}]}], ";"}], " ", + RowBox[{"(*", " ", + RowBox[{"propeller", " ", "speed"}], " ", "*)"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"\[Delta]a", "=", + RowBox[{"z", "[", "2", "]"}]}], ";", + RowBox[{"(*", " ", + RowBox[{"aileron", " ", "deflection"}], " ", "*)"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"\[Delta]e", "=", + RowBox[{"z", "[", "3", "]"}]}], ";", + RowBox[{"(*", " ", + RowBox[{"elevator", " ", "deflection"}], " ", "*)"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{ + RowBox[{"\[Delta]r", "=", + RowBox[{"z", "[", "4", "]"}]}], ";"}], + RowBox[{"(*", " ", + RowBox[{"rudder", " ", "deflection"}], " ", "*)"}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"Z", "=", + RowBox[{"Table", "[", + RowBox[{ + RowBox[{"z", "[", "i", "]"}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", "4"}], "}"}]}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"NOISES", "=", + RowBox[{"Table", "[", + RowBox[{ + RowBox[{"eta", "[", "i", "]"}], ",", + RowBox[{"{", + RowBox[{"i", ",", "1", ",", "6"}], "}"}]}], "]"}]}], ";"}]}], "Input", + CellChangeTimes->{{3.632466994423296*^9, 3.6324670023447437`*^9}, { + 3.6324673568630743`*^9, 3.632467391423369*^9}, {3.632473596617989*^9, + 3.632473628189311*^9}, {3.6324860167895203`*^9, 3.6324860253571453`*^9}, { + 3.6324862298118343`*^9, 3.632486253278077*^9}, {3.632488200362911*^9, + 3.632488217048265*^9}}, + CellLabel-> + "In[1266]:=",ExpressionUUID->"3db40cdd-aae8-4aed-bbcf-7b6c917acc34"], + +Cell["Equations", "Subsubtitle", + CellChangeTimes->{{3.573298514193714*^9, 3.5732985158016453`*^9}, { + 3.5814133864550056`*^9, 3.581413389613989*^9}, 3.5814150402587423`*^9, { + 3.600064344573599*^9, + 3.600064346732829*^9}},ExpressionUUID->"3a22e419-d65c-4c9e-84c6-\ +863e409e519a"], + +Cell["Air density model", "Text", + CellChangeTimes->{{3.632464865348349*^9, 3.632464865838887*^9}, { + 3.632464907969676*^9, + 3.632464942989243*^9}},ExpressionUUID->"9ac0d911-0e84-4579-bcb9-\ +2110f48ca902"], + +Cell[CellGroupData[{ + +Cell[BoxData[{ + RowBox[{ + RowBox[{"T", "=", + RowBox[{"T0", + RowBox[{"(", + RowBox[{"1", "+", + RowBox[{"a", " ", + RowBox[{ + RowBox[{"(", + RowBox[{"h0", "+", + RowBox[{"SW", "[", + RowBox[{"[", "3", "]"}], "]"}]}], ")"}], "/", "T0"}]}]}], + ")"}]}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"\[Rho]", "=", + RowBox[{ + RowBox[{"p0", + RowBox[{ + RowBox[{ + RowBox[{"(", + RowBox[{"1", "+", + RowBox[{"a", + RowBox[{ + RowBox[{"(", + RowBox[{"h0", "+", + RowBox[{"SW", "[", + RowBox[{"[", "3", "]"}], "]"}]}], ")"}], "/", "T0"}]}]}], ")"}], + "^", "5.2561"}], "/", + RowBox[{"(", + RowBox[{"Ra", " ", "T"}], ")"}]}]}], "//", "FullSimplify"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"\[Rho]", " ", "=", " ", "1.268"}], ";", + "\[AliasDelimiter]"}]}], "Input", + CellChangeTimes->{{3.632462276976709*^9, 3.632462293513674*^9}, { + 3.63246255070463*^9, 3.63246255752766*^9}, {3.632462666581998*^9, + 3.632462767916204*^9}, 3.632462848468005*^9, {3.632462884866085*^9, + 3.632463087325437*^9}, {3.632464836860113*^9, 3.632464838151527*^9}, { + 3.6324649483473377`*^9, 3.632464955096529*^9}, {3.632473673346718*^9, + 3.6324736736899147`*^9}, {3.632484816779133*^9, 3.632484818831092*^9}, { + 3.6324860751887093`*^9, 3.6324860763421392`*^9}, {3.6326351738117943`*^9, + 3.632635175495201*^9}, {3.634305313267096*^9, 3.634305315999455*^9}, + 3.771231605060816*^9, {3.7713227938461018`*^9, 3.771322795852179*^9}, { + 3.772946102534224*^9, 3.772946104786611*^9}, 3.773142614758792*^9, { + 3.781532267573668*^9, 3.781532269711938*^9}, {3.831543702800745*^9, + 3.831543703679405*^9}, {3.8315534192530937`*^9, 3.831553422523533*^9}, + 3.831553568121788*^9, {3.831699456786346*^9, 3.83169945836695*^9}}, + CellLabel-> + "In[1272]:=",ExpressionUUID->"9f81cfa6-a749-4450-bfb3-736066b50bce"], + +Cell[BoxData["\[AliasDelimiter]"], "Output", + CellChangeTimes->{3.831997318978107*^9, 3.832036497145423*^9, + 3.832040489392352*^9, 3.832040549152165*^9, 3.832048184145877*^9}, + CellLabel-> + "Out[1274]=",ExpressionUUID->"c15d1718-07db-4a27-8450-bddfe12047c1"] +}, Open ]], + +Cell["Airspeed", "Text", + CellChangeTimes->{{3.6324649632440243`*^9, + 3.6324649676453876`*^9}},ExpressionUUID->"656d798c-a5c9-4cc6-a1dd-\ +06c8dc6c6565"], + +Cell[BoxData[{ + RowBox[{ + RowBox[{"AIRS", "=", + RowBox[{"VS", "-", + RowBox[{ + RowBox[{"QuatToRot", "[", + RowBox[{"QuatInv", "[", "QWS", "]"}], "]"}], ".", "WINDW"}]}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"\[Alpha]", "=", + RowBox[{"ArcTan", "[", + RowBox[{ + RowBox[{"AIRS", "[", + RowBox[{"[", "1", "]"}], "]"}], ",", + RowBox[{"AIRS", "[", + RowBox[{"[", "3", "]"}], "]"}]}], "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"\[Beta]", "=", + RowBox[{"ArcSin", "[", + RowBox[{ + RowBox[{"AIRS", "[", + RowBox[{"[", "2", "]"}], "]"}], "/", + RowBox[{"L2", "[", "AIRS", "]"}]}], "]"}]}], ";"}], " ", + "\[IndentingNewLine]", + RowBox[{"(*", " ", + RowBox[{"dynamic", " ", "pressure"}], " ", "*)"}]}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"qbar", "=", + RowBox[{"\[Rho]", " ", + RowBox[{ + RowBox[{ + RowBox[{"L2", "[", "AIRS", "]"}], "^", "2"}], "/", "2"}]}]}], + ";"}]}], "Input", + CellChangeTimes->{{3.632463641043902*^9, 3.632463700014979*^9}, { + 3.632463760723637*^9, 3.6324638026841*^9}, {3.6324638481892347`*^9, + 3.6324638485511303`*^9}, {3.632464012285355*^9, 3.6324640922581472`*^9}, { + 3.63246427497224*^9, 3.6324645819649153`*^9}, {3.632464802514184*^9, + 3.632464847852816*^9}, {3.632464958182373*^9, 3.632464975561612*^9}, { + 3.632548707477417*^9, 3.632548715250105*^9}, {3.774611994153266*^9, + 3.774611997890765*^9}, 3.781531472311548*^9, {3.782109505828165*^9, + 3.782109507995604*^9}, 3.831543728856717*^9, {3.831548449935013*^9, + 3.831548459109014*^9}, {3.831550697106958*^9, 3.831550699078291*^9}}, + CellLabel-> + "In[1275]:=",ExpressionUUID->"29b01d2f-83ef-4d5a-8a25-69d70c2e9207"], + +Cell["Wind frame ", "Text", + CellChangeTimes->{{3.6324649823613243`*^9, + 3.6324649864149113`*^9}},ExpressionUUID->"7aa4501a-e764-4f85-8410-\ +73dc94387049"], + +Cell[BoxData[ + RowBox[{ + RowBox[{"RWINDS", "=", + RowBox[{ + RowBox[{"Transpose", "[", + RowBox[{"RotationMatrix", "[", + RowBox[{"\[Alpha]", ",", + RowBox[{"{", + RowBox[{"0", ",", "1", ",", "0"}], "}"}]}], "]"}], "]"}], ".", + RowBox[{"RotationMatrix", "[", + RowBox[{"\[Beta]", ",", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "1"}], "}"}]}], "]"}]}]}], ";"}]], "Input",\ + + CellChangeTimes->{{3.6324646247401857`*^9, 3.632464632600142*^9}, { + 3.632464680101325*^9, 3.632464784467514*^9}, {3.6324649794605837`*^9, + 3.6324649904896584`*^9}, {3.632474117326888*^9, 3.632474118720315*^9}, { + 3.632474151294368*^9, 3.6324741531255283`*^9}, {3.63257138686383*^9, + 3.632571433828497*^9}, {3.632571484241777*^9, 3.632571500711404*^9}, { + 3.634300370544889*^9, 3.634300375506771*^9}, {3.774266318707065*^9, + 3.774266321501566*^9}, {3.774612005261743*^9, 3.774612007778376*^9}, { + 3.774691121917021*^9, 3.7746911255822783`*^9}, 3.7815314791176033`*^9, + 3.831544148398527*^9}, + CellLabel-> + "In[1279]:=",ExpressionUUID->"dbea6cef-0336-433e-92e3-cc59b4a69219"], + +Cell[BoxData[ + RowBox[{ + RowBox[{ + RowBox[{"Transpose", "[", + RowBox[{"RotationMatrix", "[", + RowBox[{"jb", ",", + RowBox[{"{", + RowBox[{"0", ",", "1", ",", "0"}], "}"}]}], "]"}], "]"}], ".", + RowBox[{"RotationMatrix", "[", + RowBox[{"co", ",", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "1"}], "}"}]}], "]"}]}], ";"}]], "Input", + CellChangeTimes->{{3.831554523781294*^9, 3.83155452803343*^9}}, + CellLabel-> + "In[1280]:=",ExpressionUUID->"6d11debc-7473-46de-b505-2a1ab0c568e8"], + +Cell["Thrust force", "Text", + CellChangeTimes->{{3.632464997924268*^9, 3.63246500193495*^9}, + 3.7723597660999403`*^9},ExpressionUUID->"15e73123-e84e-4360-b55b-\ +334ef918a821"], + +Cell[BoxData[ + RowBox[{ + RowBox[{"(*", + RowBox[{ + "this", " ", "can", " ", "cause", " ", "division", " ", "by", " ", + "zero"}], "*)"}], "\[IndentingNewLine]", + RowBox[{"(*", + RowBox[{ + RowBox[{ + RowBox[{"CFT", "[", "J_", "]"}], ":=", + RowBox[{"CFT1", "+", + RowBox[{"CFT2", " ", "J"}], "+", " ", + RowBox[{"CFT3", " ", + RowBox[{"J", "^", "2"}]}]}]}], ";", "\[IndentingNewLine]", + RowBox[{"FT", "=", + RowBox[{"\[Rho]", " ", + RowBox[{"n", "^", "2"}], " ", + RowBox[{"PD", "^", "4"}], " ", + RowBox[{"CFT", "[", + RowBox[{ + RowBox[{"L2", "[", "AIRS", "]"}], "/", + RowBox[{"(", + RowBox[{"PD", " ", "\[Pi]", " ", "n"}], ")"}]}], "]"}]}]}], ";"}], + "*)"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"FT", "=", + RowBox[{"\[Rho]", " ", + RowBox[{"PD", "^", "2"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"CFT1", " ", + RowBox[{"PD", "^", "2"}], " ", + RowBox[{"n", "^", "2"}]}], " ", "+", " ", + RowBox[{"CFT2", " ", "PD", " ", "n", " ", + RowBox[{ + RowBox[{"L2", "[", "AIRS", "]"}], "/", "\[Pi]"}]}], "+", + RowBox[{"CFT3", " ", + RowBox[{ + RowBox[{ + RowBox[{"L2", "[", "AIRS", "]"}], "^", "2"}], "/", + RowBox[{"\[Pi]", "^", "2"}]}]}]}], ")"}]}]}], ";"}]}]], "Input", + CellChangeTimes->{{3.6324630957984457`*^9, 3.6324631592841997`*^9}, { + 3.63246362407451*^9, 3.632463645243698*^9}, {3.632464992403228*^9, + 3.632465054737678*^9}, {3.632465263450095*^9, 3.6324652660503607`*^9}, + 3.632465322251411*^9, 3.632548717997613*^9, 3.634980291260788*^9, { + 3.634980577412641*^9, 3.6349807142179937`*^9}, 3.6349807576397457`*^9, { + 3.6349808735654984`*^9, 3.634980887899046*^9}, {3.634982313791176*^9, + 3.634982334481447*^9}, {3.634988395067772*^9, 3.634988403877116*^9}, { + 3.8315480801115217`*^9, 3.831548105174776*^9}, {3.831552329329358*^9, + 3.831552340183215*^9}}, + CellLabel-> + "In[1281]:=",ExpressionUUID->"176642b9-ef21-428e-bda6-88b3064f0fe2"], + +Cell["Drag force", "Text", + CellChangeTimes->{{3.632465883892436*^9, + 3.632465884995759*^9}},ExpressionUUID->"ac9144ba-14f4-4cb2-9d7c-\ +1b84aa717d7b"], + +Cell[BoxData[{ + RowBox[{ + RowBox[{"CFX", "=", + RowBox[{"CFX1", "-", + RowBox[{"CFXalpha1", " ", "\[Alpha]"}], "+", + RowBox[{"CFXalpha2", " ", + RowBox[{"\[Alpha]", "^", "2"}]}], "+", + RowBox[{"CFXbeta2", " ", + RowBox[{"\[Beta]", "^", "2"}]}]}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"FWX", "=", + RowBox[{"qbar", " ", "S", " ", "CFX"}]}], ";"}]}], "Input", + CellChangeTimes->{{3.632465900598139*^9, 3.63246595313962*^9}, { + 3.632466061225836*^9, 3.632466070531445*^9}, 3.632474180051531*^9, { + 3.632486333812204*^9, 3.632486338262673*^9}, {3.831544195582663*^9, + 3.831544196885738*^9}}, + CellLabel-> + "In[1282]:=",ExpressionUUID->"f4931c36-9cb3-41d5-9636-f3fbcef428c9"], + +Cell["Lateral force", "Text", + CellChangeTimes->{{3.63246580061057*^9, + 3.632465802148518*^9}},ExpressionUUID->"57ce017d-559b-4d58-ba18-\ +1e7631c436d9"], + +Cell[BoxData[{ + RowBox[{ + RowBox[{"CFY", "=", + RowBox[{"CFY1", " ", "\[Beta]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"FWY", "=", + RowBox[{"qbar", " ", "S", " ", "CFY"}]}], ";"}]}], "Input", + CellChangeTimes->{{3.632465807062015*^9, 3.63246584035797*^9}, + 3.774612027001218*^9, 3.7815314926688843`*^9}, + CellLabel-> + "In[1284]:=",ExpressionUUID->"63203ee0-9601-47b1-9606-b67b10ce9a46"], + +Cell["Lift force", "Text", + CellChangeTimes->{{3.6324653247972507`*^9, + 3.6324653267561407`*^9}},ExpressionUUID->"4432b235-bb2b-4e9d-87f8-\ +55bf93c648df"], + +Cell[BoxData[{ + RowBox[{ + RowBox[{"CFZ", "=", + RowBox[{ + RowBox[{"-", "CFZ1"}], "+", + RowBox[{"CFZalpha", " ", "\[Alpha]"}]}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"FWZ", "=", + RowBox[{"qbar", " ", "S", " ", "CFZ"}]}], ";"}]}], "Input", + CellChangeTimes->{{3.6324653325318327`*^9, 3.632465404457531*^9}, { + 3.632465682669859*^9, 3.6324656933429127`*^9}, {3.632465792314137*^9, + 3.632465798108323*^9}, 3.774612028899675*^9, {3.831544264680245*^9, + 3.8315442668236523`*^9}}, + CellLabel-> + "In[1286]:=",ExpressionUUID->"c73ee33c-6f07-4ea9-82e2-0f37fad73c8e"], + +Cell["Roll moment", "Text", + CellChangeTimes->{{3.632466790216868*^9, + 3.632466796296864*^9}},ExpressionUUID->"3b78e61b-a31c-43c6-949c-\ +8ab46a558347"], + +Cell[BoxData[{ + RowBox[{ + RowBox[{"\[Omega]tildex", "=", " ", + RowBox[{"b", " ", + RowBox[{ + RowBox[{"WS", "[", + RowBox[{"[", "1", "]"}], "]"}], "/", + RowBox[{"(", + RowBox[{"2", " ", + RowBox[{"L2", "[", "AIRS", "]"}]}], ")"}]}]}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"\[Omega]tildez", "=", + RowBox[{"b", " ", + RowBox[{ + RowBox[{"WS", "[", + RowBox[{"[", "3", "]"}], "]"}], "/", + RowBox[{"(", + RowBox[{"2", " ", + RowBox[{"L2", "[", "AIRS", "]"}]}], ")"}]}]}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"CMX", "=", + RowBox[{ + RowBox[{"CMXa", " ", "\[Delta]a"}], " ", "-", + RowBox[{"CMXbeta", " ", "\[Beta]"}], "+", + RowBox[{"CMX\[Omega]tildex", " ", "\[Omega]tildex"}], " ", "-", + RowBox[{"CMX\[Omega]tildez", " ", "\[Omega]tildez"}]}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MBX", "=", + RowBox[{"qbar", " ", "S", " ", "b", " ", "CMX"}]}], ";"}]}], "Input", + CellChangeTimes->{{3.632466801116767*^9, 3.6324669518785152`*^9}, { + 3.632467018819668*^9, 3.632467037920843*^9}, {3.632467073135228*^9, + 3.632467123591279*^9}, {3.6324674621580877`*^9, 3.632467462850863*^9}, { + 3.6324862875540953`*^9, 3.6324862888104773`*^9}, {3.6325487193918743`*^9, + 3.632548719873026*^9}, 3.634292400473446*^9, {3.774612038472706*^9, + 3.774612054579494*^9}, {3.7815315014383574`*^9, 3.781531501625888*^9}, { + 3.8315442788487*^9, 3.831544307311618*^9}}, + CellLabel-> + "In[1288]:=",ExpressionUUID->"7be4a0a1-b2c0-4d7b-b564-9f92dabf375f"], + +Cell["Pitch moment", "Text", + CellChangeTimes->{{3.632467155741108*^9, + 3.632467157541481*^9}},ExpressionUUID->"78b600c8-17c9-43b2-8561-\ +39fdff7b92e4"], + +Cell[BoxData[{ + RowBox[{ + RowBox[{"\[Omega]tildey", "=", + RowBox[{"cbar", " ", + RowBox[{ + RowBox[{"WS", "[", + RowBox[{"[", "2", "]"}], "]"}], "/", + RowBox[{"(", + RowBox[{"2", " ", + RowBox[{"L2", "[", "AIRS", "]"}]}], ")"}]}]}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"CMY", "=", + RowBox[{ + RowBox[{"-", "CMY1"}], "-", + RowBox[{"CMYe", " ", "\[Delta]e"}], " ", "+", + RowBox[{"CMY\[Omega]tildey", " ", "\[Omega]tildey"}], "+", + RowBox[{"CMYalpha", " ", "\[Alpha]"}]}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MBY", "=", + RowBox[{"qbar", " ", "S", " ", "cbar", " ", "CMY"}]}], ";"}]}], "Input", + CellChangeTimes->{{3.632467408804394*^9, 3.632467485452004*^9}, { + 3.6324734635941877`*^9, 3.6324734804954777`*^9}, {3.632486355640251*^9, + 3.632486356968248*^9}, 3.6325487214032393`*^9, {3.774612041318342*^9, + 3.774612074499466*^9}, 3.831544287152123*^9, {3.831544336079039*^9, + 3.8315443421869287`*^9}}, + CellLabel-> + "In[1292]:=",ExpressionUUID->"4ce3e11d-b2d8-42df-94f3-f44671feb140"], + +Cell["Yaw moment", "Text", + CellChangeTimes->{{3.6324733890787363`*^9, + 3.632473392049634*^9}},ExpressionUUID->"c302290e-b2df-4299-8d3e-\ +f59d1fc850b6"], + +Cell[BoxData[{ + RowBox[{ + RowBox[{"CMZ", " ", "=", " ", + RowBox[{ + RowBox[{ + RowBox[{"-", "CMZr"}], " ", "\[Delta]r"}], " ", "+", " ", + RowBox[{"CMZ\[Omega]tildez", " ", "\[Omega]tildez"}], " ", "+", + RowBox[{"CMZbeta", " ", "\[Beta]"}]}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MBZ", "=", + RowBox[{"qbar", " ", "S", " ", "b", " ", "CMZ"}]}], ";"}]}], "Input", + CellChangeTimes->{{3.6324733978818483`*^9, 3.632473492292446*^9}, + 3.632473773078868*^9, 3.632486362438322*^9, {3.632734158387792*^9, + 3.632734159678158*^9}, 3.774612081502236*^9, {3.781531506233139*^9, + 3.781531506482781*^9}, 3.831544367175006*^9}, + CellLabel-> + "In[1295]:=",ExpressionUUID->"1a28b43d-e9a2-4ebf-8251-f3ab2ddb7786"], + +Cell["Error equations", "Text", + CellChangeTimes->{{3.6324737754609947`*^9, + 3.632473779006583*^9}},ExpressionUUID->"4ba37bc9-3ca4-4415-be72-\ +e388f0dad47a"], + +Cell[BoxData[{ + RowBox[{ + RowBox[{"Ahatlong", "=", + RowBox[{ + RowBox[{ + RowBox[{"QuatToRot", "[", + RowBox[{"QuatInv", "[", "QWS", "]"}], "]"}], ".", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", + RowBox[{"-", "g"}]}], "}"}]}], "+", + RowBox[{ + RowBox[{"1", "/", "m"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"{", + RowBox[{"FT", ",", "0", ",", "0"}], "}"}], "+", + RowBox[{"RWINDS", ".", + RowBox[{"{", + RowBox[{"FWX", ",", "FWY", ",", "FWZ"}], "}"}]}]}], ")"}]}]}]}], + RowBox[{"(*", + RowBox[{"-", + RowBox[{"WS", "\[Cross]", "VS"}]}], "*)"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"AlphaHatlong", "=", " ", + RowBox[{ + RowBox[{"Inverse", "[", "IB", "]"}], ".", + RowBox[{"(", + RowBox[{ + RowBox[{"{", + RowBox[{"MBX", ",", "MBY", ",", "MBZ"}], "}"}], "-", + RowBox[{"WS", "\[Cross]", + RowBox[{"(", + RowBox[{"IB", ".", "WS"}], ")"}]}]}], ")"}]}]}], ";"}]}], "Input", + CellChangeTimes->{{3.632473782667057*^9, 3.632473783948543*^9}, { + 3.632473980553233*^9, 3.632474122781205*^9}, {3.632474156463627*^9, + 3.6324741654080887`*^9}, {3.632474242482204*^9, 3.632474271983945*^9}, { + 3.6324746531850758`*^9, 3.632474665303821*^9}, {3.632474712022402*^9, + 3.632474752296493*^9}, {3.6324863716442823`*^9, 3.632486373451024*^9}, { + 3.6324864511201982`*^9, 3.632486454813035*^9}, {3.6324869920050364`*^9, + 3.632486995777545*^9}, 3.632572048567833*^9, {3.632820407426373*^9, + 3.632820415425437*^9}, {3.6330662719267473`*^9, 3.633066275128694*^9}, + 3.6330672246184607`*^9, {3.633067323386444*^9, 3.633067323446558*^9}, { + 3.633668520364181*^9, 3.6336685404354343`*^9}, {3.6336692322822113`*^9, + 3.633669235904211*^9}, {3.6342782924356194`*^9, 3.634278296987405*^9}, { + 3.6342783329840317`*^9, 3.6342783528443823`*^9}, {3.6344523148400803`*^9, + 3.634452317130845*^9}, {3.7746911481775713`*^9, 3.774691149760127*^9}, { + 3.8319972965581017`*^9, 3.831997300997398*^9}, {3.831997646944586*^9, + 3.831997653276815*^9}, {3.832040513254773*^9, 3.8320405446977453`*^9}}, + CellLabel-> + "In[1297]:=",ExpressionUUID->"fe419f0a-adf5-43af-9f08-7b8a7f19180b"], + +Cell[BoxData[{ + RowBox[{ + RowBox[{"Ahat", "=", "Ahatlong"}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"Alphahat", "=", "AlphaHatlong"}], + ";"}], "\[IndentingNewLine]"}], "Input", + CellChangeTimes->{{3.632486455549663*^9, 3.6324865144132833`*^9}, { + 3.632486628670555*^9, 3.632486630005704*^9}, {3.6324868147521963`*^9, + 3.632486817333385*^9}, 3.632486929763055*^9, {3.632487008855073*^9, + 3.632487019375031*^9}, {3.632487069748158*^9, 3.63248707741856*^9}, { + 3.632487112090027*^9, 3.632487150840695*^9}, {3.632487252771805*^9, + 3.6324872660424747`*^9}, {3.632488139926024*^9, 3.632488140072042*^9}, { + 3.632734121716978*^9, 3.632734200327458*^9}, {3.633067298879485*^9, + 3.63306731942362*^9}, {3.634278298237008*^9, 3.63427829964246*^9}, { + 3.63427833587599*^9, 3.634278345660905*^9}, {3.634298584045291*^9, + 3.634298585468253*^9}, {3.7723598046089363`*^9, 3.772359805228352*^9}}, + CellLabel-> + "In[1299]:=",ExpressionUUID->"ff5f7d73-add6-4e24-9e55-7b15786660c6"], + +Cell[BoxData[{ + RowBox[{ + RowBox[{"DBG", "=", + RowBox[{"{", + RowBox[{ + "FWX", ",", "FWY", ",", "FWZ", ",", "FT", ",", "MBX", ",", "MBY", ",", + "MBZ"}], "}"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"AhatlongDBG", "=", + RowBox[{ + RowBox[{ + RowBox[{"QuatToRot", "[", + RowBox[{"QuatInv", "[", "QWS", "]"}], "]"}], ".", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", + RowBox[{"-", "g"}]}], "}"}]}], "+", + RowBox[{ + RowBox[{"1", "/", "m"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"{", + RowBox[{ + RowBox[{"f", "[", "4", "]"}], ",", "0", ",", "0"}], "}"}], "+", + RowBox[{ + RowBox[{"Transpose", "[", "RWINDS", "]"}], ".", + RowBox[{"{", + RowBox[{ + RowBox[{"f", "[", "1", "]"}], ",", + RowBox[{"f", "[", "2", "]"}], ",", + RowBox[{"f", "[", "3", "]"}]}], "}"}]}]}], ")"}]}], "-", + RowBox[{"WS", "\[Cross]", "VS"}]}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"AlphahatlongDBG", "=", " ", + RowBox[{ + RowBox[{"Inverse", "[", "IB", "]"}], ".", + RowBox[{"(", + RowBox[{ + RowBox[{"{", + RowBox[{ + RowBox[{"mm", "[", "1", "]"}], ",", + RowBox[{"mm", "[", "2", "]"}], ",", + RowBox[{"mm", "[", "3", "]"}]}], "}"}], "-", + RowBox[{"WS", "\[Cross]", + RowBox[{"(", + RowBox[{"IB", ".", "WS"}], ")"}]}]}], ")"}]}]}], ";"}]}], "Input", + CellChangeTimes->{{3.633234965893896*^9, 3.633234975029994*^9}, { + 3.6332350425322247`*^9, 3.633235051548617*^9}, 3.633244923580419*^9, { + 3.633327208678884*^9, 3.633327209833385*^9}, {3.634298362377328*^9, + 3.6342983969550533`*^9}, {3.634298517168047*^9, 3.63429851785793*^9}, { + 3.634298591096792*^9, 3.634298593067531*^9}, {3.634299639194295*^9, + 3.634299645586197*^9}, {3.634299681394882*^9, 3.6342996858226843`*^9}, + 3.634299852392684*^9, 3.634300111654772*^9, {3.634300178359811*^9, + 3.634300196975257*^9}, 3.8315541730600033`*^9, {3.832036484673256*^9, + 3.832036488336998*^9}, {3.8320404846183357`*^9, 3.832040508374733*^9}}, + CellLabel-> + "In[1301]:=",ExpressionUUID->"d17df8c1-9260-4a0c-8495-b5ef6f820848"], + +Cell[CellGroupData[{ + +Cell[BoxData[{ + RowBox[{ + RowBox[{"ERR", "=", + RowBox[{ + RowBox[{"Flatten", "[", + RowBox[{"{", + RowBox[{ + RowBox[{"AS", "-", "Ahat"}], ",", + RowBox[{"ALPHAS", "-", "Alphahat"}]}], "}"}], "]"}], "+", "NOISES"}]}], + ";"}], "\[IndentingNewLine]", "ERR"}], "Input", + CellChangeTimes->{{3.632487913384449*^9, 3.632487921874243*^9}, { + 3.632488026778446*^9, 3.632488059172721*^9}, {3.632488578258338*^9, + 3.632488606817566*^9}, {3.784538832900227*^9, 3.784538833715313*^9}}, + CellLabel-> + "In[1304]:=",ExpressionUUID->"8c91f72d-3f47-4547-a792-5e0813bcc352"], + +Cell[BoxData[ + InterpretationBox[ + TagBox[ + FrameBox[GridBox[{ + { + ItemBox[ + TagBox[ + RowBox[{"{", + RowBox[{ + RowBox[{ + RowBox[{"eta", "[", "1", "]"}], "+", + RowBox[{"2", " ", + RowBox[{"gravity", "[", "1", "]"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"-", + RowBox[{"x", "[", "4", "]"}]}], " ", + RowBox[{"x", "[", "6", "]"}]}], "+", + RowBox[{ + RowBox[{"x", "[", "5", "]"}], " ", + RowBox[{"x", "[", "7", "]"}]}]}], ")"}]}], "+", + RowBox[{"x", "[", "14", "]"}], "-", + FractionBox[ + RowBox[{ + FractionBox[ + RowBox[{"0.634`", " ", + RowBox[{"ArcSin", "[", + FractionBox[ + TemplateBox[{"1"}, + "OutputSizeLimit`Skeleton"], + SqrtBox[ + TemplateBox[{"1"}, + "OutputSizeLimit`Skeleton"]]], "]"}], " ", + TemplateBox[{"3"}, + "OutputSizeLimit`Skeleton"], " ", + RowBox[{"(", + TemplateBox[{"1"}, + "OutputSizeLimit`Skeleton"], ")"}], " ", + SqrtBox[ + RowBox[{ + SuperscriptBox[ + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"-", "2"}], " ", + RowBox[{"wind", "[", "2", "]"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"x", "[", "5", "]"}], " ", + RowBox[{"x", "[", "6", "]"}]}], "+", + RowBox[{ + RowBox[{"x", "[", "4", "]"}], " ", + TemplateBox[{"1"}, + "OutputSizeLimit`Skeleton"]}]}], ")"}]}], "-", + TemplateBox[{"1"}, + "OutputSizeLimit`Skeleton"], "-", + TemplateBox[{"1"}, + "OutputSizeLimit`Skeleton"], "+", + RowBox[{"x", "[", "8", "]"}]}], ")"}], "2"], "+", + SuperscriptBox[ + RowBox[{"(", + TemplateBox[{"1"}, + "OutputSizeLimit`Skeleton"], ")"}], "2"], "+", + SuperscriptBox[ + RowBox[{"(", + TemplateBox[{"1"}, + "OutputSizeLimit`Skeleton"], ")"}], "2"]}]]}], + SqrtBox[ + RowBox[{ + SuperscriptBox[ + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"-", "2"}], " ", + RowBox[{"wind", "[", "2", "]"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"x", "[", "5", "]"}], " ", + RowBox[{"x", "[", "6", "]"}]}], "+", + RowBox[{ + RowBox[{"x", "[", "4", "]"}], " ", + RowBox[{"x", "[", "7", "]"}]}]}], ")"}]}], "-", + RowBox[{"2", " ", + TemplateBox[{"1"}, + "OutputSizeLimit`Skeleton"], " ", + RowBox[{"(", + TemplateBox[{"1"}, + "OutputSizeLimit`Skeleton"], ")"}]}], "-", + TemplateBox[{"1"}, + "OutputSizeLimit`Skeleton"], "+", + RowBox[{"x", "[", "8", "]"}]}], ")"}], "2"], "+", + SuperscriptBox[ + RowBox[{"(", + TemplateBox[{"1"}, + "OutputSizeLimit`Skeleton"], ")"}], "2"]}]]], "+", + FractionBox[ + TemplateBox[{"1"}, + "OutputSizeLimit`Skeleton"], + SqrtBox[ + TemplateBox[{"1"}, + "OutputSizeLimit`Skeleton"]]], "+", + TemplateBox[{"1"}, + "OutputSizeLimit`Skeleton"], "+", + RowBox[{"1.268`", " ", + SuperscriptBox[ + RowBox[{"cp", "[", "1", "]"}], "2"], " ", + RowBox[{"(", + RowBox[{ + FractionBox[ + RowBox[{ + RowBox[{"fThrust", "[", "3", "]"}], " ", + RowBox[{"(", + TemplateBox[{"1"}, + "OutputSizeLimit`Skeleton"], ")"}]}], + SuperscriptBox["\[Pi]", "2"]], "+", + FractionBox[ + TemplateBox[{"1"}, + "OutputSizeLimit`Skeleton"], "\[Pi]"], "+", + RowBox[{ + SuperscriptBox[ + RowBox[{"cp", "[", "1", "]"}], "2"], " ", + RowBox[{"fThrust", "[", "1", "]"}], " ", + SuperscriptBox[ + RowBox[{"z", "[", "1", "]"}], "2"]}]}], ")"}]}]}], + RowBox[{"cp", "[", "4", "]"}]]}], ",", + TemplateBox[{"4"}, + "OutputSizeLimit`Skeleton"], ",", + RowBox[{ + RowBox[{"eta", "[", "6", "]"}], "+", + RowBox[{"x", "[", "19", "]"}], "-", + FractionBox[ + RowBox[{ + RowBox[{"(", + TemplateBox[{"1"}, + "OutputSizeLimit`Skeleton"], ")"}], " ", + TemplateBox[{"1"}, + "OutputSizeLimit`Skeleton"]}], + TemplateBox[{"1"}, + "OutputSizeLimit`Skeleton"]], "-", + FractionBox[ + RowBox[{ + RowBox[{"(", + TemplateBox[{"1"}, + "OutputSizeLimit`Skeleton"], ")"}], " ", + RowBox[{"(", + TemplateBox[{"1"}, + "OutputSizeLimit`Skeleton"], ")"}]}], + TemplateBox[{"1"}, + "OutputSizeLimit`Skeleton"]], "-", + FractionBox[ + RowBox[{ + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"ibd", "[", "1", "]"}], " ", + RowBox[{"ibd", "[", "2", "]"}]}], "-", + SuperscriptBox[ + RowBox[{"ibod", "[", "2", "]"}], "2"]}], ")"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"-", + RowBox[{"ibod", "[", "2", "]"}]}], " ", + SuperscriptBox[ + RowBox[{"x", "[", "11", "]"}], "2"]}], "+", + TemplateBox[{"7"}, + "OutputSizeLimit`Skeleton"], "+", + RowBox[{"0.634`", " ", + RowBox[{"cp", "[", "2", "]"}], " ", + RowBox[{"cp", "[", "3", "]"}], " ", + RowBox[{"(", + RowBox[{ + SuperscriptBox[ + RowBox[{"(", + TemplateBox[{"1"}, + "OutputSizeLimit`Skeleton"], ")"}], "2"], "+", + SuperscriptBox[ + RowBox[{"(", + TemplateBox[{"1"}, + "OutputSizeLimit`Skeleton"], ")"}], "2"], "+", + SuperscriptBox[ + RowBox[{"(", + TemplateBox[{"1"}, + "OutputSizeLimit`Skeleton"], ")"}], "2"]}], ")"}], " ", + RowBox[{"(", + TemplateBox[{"1"}, + "OutputSizeLimit`Skeleton"], ")"}]}]}], ")"}]}], + RowBox[{ + RowBox[{ + RowBox[{"ibd", "[", "1", "]"}], " ", + RowBox[{"ibd", "[", "2", "]"}], " ", + RowBox[{"ibd", "[", "3", "]"}]}], "-", + RowBox[{ + RowBox[{"ibd", "[", "2", "]"}], " ", + SuperscriptBox[ + RowBox[{"ibod", "[", "1", "]"}], "2"]}], "-", + RowBox[{ + RowBox[{"ibd", "[", "3", "]"}], " ", + SuperscriptBox[ + RowBox[{"ibod", "[", "2", "]"}], "2"]}], "+", + RowBox[{"2", " ", + RowBox[{"ibod", "[", "1", "]"}], " ", + RowBox[{"ibod", "[", "2", "]"}], " ", + RowBox[{"ibod", "[", "3", "]"}]}], "-", + RowBox[{ + RowBox[{"ibd", "[", "1", "]"}], " ", + SuperscriptBox[ + RowBox[{"ibod", "[", "3", "]"}], "2"]}]}]]}]}], "}"}], + Short[#, 5]& ], + BaseStyle->{Deployed -> False}, + StripOnInput->False]}, + {GridBox[{ + { + PaneBox[ + TagBox[ + TooltipBox[ + StyleBox[ + StyleBox[ + DynamicBox[ToBoxes[ + FEPrivate`FrontEndResource[ + "FEStrings", "sizeBriefExplanation"], StandardForm], + ImageSizeCache->{46.18359375, {2., 6.}}], + StripOnInput->False, + DynamicUpdating->True, + LineSpacing->{1, 2}, + LineIndent->0, + LinebreakAdjustments->{1., 100, 0, 0, 0}], "OSLText", + StripOnInput->False], + StyleBox[ + DynamicBox[ + ToBoxes[ + FEPrivate`FrontEndResource["FEStrings", "sizeExplanation"], + StandardForm]], DynamicUpdating -> True, LineIndent -> 0, + LinebreakAdjustments -> {1., 100, 0, 0, 0}, + LineSpacing -> {1, 2}, StripOnInput -> False]], + Annotation[#, + Style[ + Dynamic[ + FEPrivate`FrontEndResource["FEStrings", "sizeExplanation"]], + DynamicUpdating -> True, LineIndent -> 0, + LinebreakAdjustments -> {1., 100, 0, 0, 0}, + LineSpacing -> {1, 2}], "Tooltip"]& ], + Alignment->Center, + BaselinePosition->Baseline, + ImageSize->{Automatic, {25, Full}}], + ButtonBox[ + PaneSelectorBox[{False-> + StyleBox[ + StyleBox[ + DynamicBox[ToBoxes[ + FEPrivate`FrontEndResource["FEStrings", "sizeShowLess"], + StandardForm], + ImageSizeCache->{41.015625, {0., 6.}}], + StripOnInput->False, + DynamicUpdating->True, + LineSpacing->{1, 2}, + LineIndent->0, + LinebreakAdjustments->{1., 100, 0, 0, 0}], "OSLControl", + StripOnInput->False], True-> + StyleBox[ + StyleBox[ + DynamicBox[ToBoxes[ + FEPrivate`FrontEndResource["FEStrings", "sizeShowLess"], + StandardForm]], + StripOnInput->False, + DynamicUpdating->True, + LineSpacing->{1, 2}, + LineIndent->0, + LinebreakAdjustments->{1., 100, 0, 0, 0}], "OSLControlActive", + StripOnInput->False]}, Dynamic[ + CurrentValue["MouseOver"]], + Alignment->Center, + FrameMargins->0, + ImageSize->{Automatic, {25, Full}}], + Appearance->None, + BaselinePosition->Baseline, + + ButtonFunction:>OutputSizeLimit`ButtonFunction[ + OutputSizeLimit`Defer, 1305, 21937265413343555711, 5/2], + Enabled->True, + Evaluator->Automatic, + Method->"Queued"], + ButtonBox[ + PaneSelectorBox[{False-> + StyleBox[ + StyleBox[ + DynamicBox[ToBoxes[ + FEPrivate`FrontEndResource["FEStrings", "sizeShowMore"], + StandardForm], + ImageSizeCache->{46.18359375, {0., 6.}}], + StripOnInput->False, + DynamicUpdating->True, + LineSpacing->{1, 2}, + LineIndent->0, + LinebreakAdjustments->{1., 100, 0, 0, 0}], "OSLControl", + StripOnInput->False], True-> + StyleBox[ + StyleBox[ + DynamicBox[ToBoxes[ + FEPrivate`FrontEndResource["FEStrings", "sizeShowMore"], + StandardForm]], + StripOnInput->False, + DynamicUpdating->True, + LineSpacing->{1, 2}, + LineIndent->0, + LinebreakAdjustments->{1., 100, 0, 0, 0}], "OSLControlActive", + StripOnInput->False]}, Dynamic[ + CurrentValue["MouseOver"]], + Alignment->Center, + FrameMargins->0, + ImageSize->{Automatic, {25, Full}}], + Appearance->None, + BaselinePosition->Baseline, + + ButtonFunction:>OutputSizeLimit`ButtonFunction[ + OutputSizeLimit`Defer, 1305, 21937265413343555711, 5 2], + Enabled->True, + Evaluator->Automatic, + Method->"Queued"], + ButtonBox[ + PaneSelectorBox[{False-> + StyleBox[ + StyleBox[ + DynamicBox[ToBoxes[ + FEPrivate`FrontEndResource["FEStrings", "sizeShowAll"], + StandardForm], + ImageSizeCache->{34.640625, {0., 6.}}], + StripOnInput->False, + DynamicUpdating->True, + LineSpacing->{1, 2}, + LineIndent->0, + LinebreakAdjustments->{1., 100, 0, 0, 0}], "OSLControl", + StripOnInput->False], True-> + StyleBox[ + StyleBox[ + DynamicBox[ToBoxes[ + FEPrivate`FrontEndResource["FEStrings", "sizeShowAll"], + StandardForm]], + StripOnInput->False, + DynamicUpdating->True, + LineSpacing->{1, 2}, + LineIndent->0, + LinebreakAdjustments->{1., 100, 0, 0, 0}], "OSLControlActive", + StripOnInput->False]}, Dynamic[ + CurrentValue["MouseOver"]], + Alignment->Center, + FrameMargins->0, + ImageSize->{Automatic, {25, Full}}], + Appearance->None, + BaselinePosition->Baseline, + + ButtonFunction:>OutputSizeLimit`ButtonFunction[ + OutputSizeLimit`Defer, 1305, 21937265413343555711, Infinity], + Enabled->True, + Evaluator->Automatic, + Method->"Queued"], + ButtonBox[ + PaneSelectorBox[{False-> + StyleBox[ + StyleBox[ + DynamicBox[ToBoxes[ + FEPrivate`FrontEndResource["FEStrings", "sizeChangeLimit"], + StandardForm], + ImageSizeCache->{62.40234375, {0., 6.}}], + StripOnInput->False, + DynamicUpdating->True, + LineSpacing->{1, 2}, + LineIndent->0, + LinebreakAdjustments->{1., 100, 0, 0, 0}], "OSLControl", + StripOnInput->False], True-> + StyleBox[ + StyleBox[ + DynamicBox[ToBoxes[ + FEPrivate`FrontEndResource["FEStrings", "sizeChangeLimit"], + StandardForm]], + StripOnInput->False, + DynamicUpdating->True, + LineSpacing->{1, 2}, + LineIndent->0, + LinebreakAdjustments->{1., 100, 0, 0, 0}], "OSLControlActive", + StripOnInput->False]}, Dynamic[ + CurrentValue["MouseOver"]], + Alignment->Center, + FrameMargins->0, + ImageSize->{Automatic, {25, Full}}], + Appearance->None, + BaselinePosition->Baseline, + ButtonFunction:>FrontEndExecute[{ + FrontEnd`SetOptions[ + FrontEnd`$FrontEnd, + FrontEnd`PreferencesSettings -> {"Page" -> "Advanced"}], + FrontEnd`FrontEndToken["PreferencesDialog"]}], + Evaluator->None, + Method->"Preemptive"]} + }, + AutoDelete->False, + FrameStyle->GrayLevel[0.85], + GridBoxDividers->{"Columns" -> {False, {True}}}, + GridBoxItemSize->{"Columns" -> {{Automatic}}, "Rows" -> {{Automatic}}}, + GridBoxSpacings->{"Columns" -> {{2}}}]} + }, + DefaultBaseStyle->"Column", + GridBoxAlignment->{"Columns" -> {{Left}}, "Rows" -> {{Baseline}}}, + GridBoxDividers->{"Columns" -> {{False}}, "Rows" -> {{False}}}, + GridBoxItemSize->{"Columns" -> {{Automatic}}, "Rows" -> {{1.}}}, + GridBoxSpacings->{"Columns" -> { + Offset[0.27999999999999997`], { + Offset[0.5599999999999999]}, + Offset[0.27999999999999997`]}, "Rows" -> { + Offset[0.2], + Offset[1.2], { + Offset[0.4]}, + Offset[0.2]}}], + BaseStyle->"OutputSizeLimit", + FrameMargins->{{12, 12}, {0, 15}}, + FrameStyle->GrayLevel[0.85], + RoundingRadius->5, + StripOnInput->False], + Deploy, + DefaultBaseStyle->"Deploy"], + If[21937265413343555711 === $SessionID, + Out[1305], Message[ + MessageName[Syntax, "noinfoker"]]; Missing["NotAvailable"]; + Null]]], "Output", + CellChangeTimes->{3.820073726788056*^9, 3.831544437869727*^9, + 3.831548149604374*^9, 3.831548464155443*^9, 3.83155070443477*^9, + 3.831552345207345*^9, 3.831997319202773*^9, 3.832036497367484*^9, + 3.8320405494103107`*^9, 3.832048184387746*^9}, + CellLabel-> + "Out[1305]=",ExpressionUUID->"418050e7-4d56-4166-bdad-8ceb9141adfa"] +}, Open ]], + +Cell["Predictor", "Subsubtitle", + CellChangeTimes->{{3.573298851623851*^9, 3.57329885458322*^9}, { + 3.6326367603131237`*^9, + 3.6326367612299*^9}},ExpressionUUID->"b92b1bb0-abe4-4170-8cfc-1e9dd8bc5679"], + +Cell[BoxData[ + RowBox[{ + RowBox[{"(*", " ", + RowBox[{ + "until", " ", "we", " ", "fix", " ", "the", " ", "predictor", " ", + "interface", " ", "we", " ", "just", " ", "predict", " ", "that", " ", + "we", " ", "move", " ", "forward", " ", "at", " ", "1", " ", + RowBox[{"m", "/", "s"}]}], " ", "*)"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"SWhat", "=", + RowBox[{"SW", "+", + RowBox[{ + RowBox[{"QuatToRot", "[", "QWS", "]"}], ".", + RowBox[{"(", + RowBox[{"{", + RowBox[{ + RowBox[{"1", "*", "dt"}], ",", " ", "0", ",", "0"}], "}"}], + ")"}]}]}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"QWShat", "=", "QWS"}], ";"}], "\[IndentingNewLine]"}]}]], "Input",\ + + CellChangeTimes->{{3.632636764848427*^9, 3.632636870351482*^9}, + 3.633327075601856*^9, 3.634538652426509*^9}, + CellLabel-> + "In[1306]:=",ExpressionUUID->"dea35a84-3a0f-4600-a2f4-2ab045608d87"], + +Cell["Output", "Subsubtitle", + CellChangeTimes->{{3.573298851623851*^9, + 3.57329885458322*^9}},ExpressionUUID->"bf1afef8-2e19-4e8e-9c26-\ +8b9e596cd10c"], + +Cell[BoxData[ + RowBox[{ + RowBox[{"SetDirectory", "[", + RowBox[{"NotebookDirectory", "[", "]"}], "]"}], ";"}]], "Input", + CellLabel-> + "In[1308]:=",ExpressionUUID->"c2f98c75-5faf-44c8-a958-147c2a660678"], + +Cell[BoxData[{ + RowBox[{ + RowBox[{"Err", " ", "=", " ", + RowBox[{"VecToZero", "[", + RowBox[{"ERR", ",", "NOISES"}], "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JErrNoises", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "NOISES", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JErrPose", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "SW", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JErrQ", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "QWS", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JErrV", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "VS", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JErrW", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "WS", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JErrA", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "AS", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JErrAlpha", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "ALPHAS", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], + ";"}]}], "Input", + CellChangeTimes->{{3.573299033702265*^9, 3.5732990981024723`*^9}, { + 3.573299164795454*^9, 3.5732991692508307`*^9}, {3.573456001706543*^9, + 3.5734560048187523`*^9}, {3.574152235462706*^9, 3.574152238223208*^9}, { + 3.5798489548623657`*^9, 3.579848955103243*^9}, {3.581401976643291*^9, + 3.5814019885894117`*^9}, {3.58141377998179*^9, 3.581413814574294*^9}, { + 3.581414129291024*^9, 3.581414154679249*^9}, {3.581765163109974*^9, + 3.581765173746241*^9}, {3.581765277500546*^9, 3.581765284545003*^9}, { + 3.6000644741616707`*^9, 3.600064476157126*^9}, {3.600064636065899*^9, + 3.60006464126534*^9}, {3.60610941052632*^9, 3.606109449315563*^9}, { + 3.617517818502956*^9, 3.617517831777896*^9}, 3.617518017239346*^9, + 3.6175224718126917`*^9, {3.617535776012012*^9, 3.6175357977310543`*^9}, { + 3.617691303889946*^9, 3.617691325047617*^9}, {3.6191537025170603`*^9, + 3.619153721782481*^9}, {3.6248780977624187`*^9, 3.6248781199132423`*^9}, { + 3.624878461572874*^9, 3.624878529180564*^9}, {3.626608128585112*^9, + 3.626608147165112*^9}, 3.626608290164939*^9, {3.627970217824462*^9, + 3.6279702738585*^9}, {3.627970352003558*^9, 3.62797036945547*^9}, { + 3.62971028921635*^9, 3.6297105068213043`*^9}, {3.629806116962702*^9, + 3.629806117816551*^9}, {3.630321579757883*^9, 3.630321583197545*^9}, { + 3.6303216981362867`*^9, 3.630321706346642*^9}, {3.630321757205647*^9, + 3.63032178972649*^9}, {3.6303219034786263`*^9, 3.630321908393339*^9}, { + 3.630322038330865*^9, 3.630322043133882*^9}, {3.630322095647093*^9, + 3.630322097242449*^9}, {3.630322176329072*^9, 3.630322192772039*^9}, + 3.630322231534956*^9, {3.6303231357931433`*^9, 3.630323150170701*^9}, { + 3.632488075940673*^9, 3.632488120413509*^9}, {3.6324882328081903`*^9, + 3.632488236596348*^9}, {3.632488545113055*^9, 3.632488557103654*^9}}, + CellLabel-> + "In[1309]:=",ExpressionUUID->"716ee195-4767-4067-92df-43ed8da3bd15"], + +Cell[BoxData[{ + RowBox[{ + RowBox[{ + RowBox[{"JErrAirDensity", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "AIRDENSITY", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], + ";"}], "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JErrFThrust", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "FTHRUST", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JErrFDrag", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "FDRAG", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JErrFLat", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "FLAT", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"JErrFLift", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "FLIFT", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JErrMRoll", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "MROLL", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JErrMPitch", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "MPITCH", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"JErrMYaw", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "MYAW", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"JErrCBar", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "CBAR", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JErrCp", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "CP", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JErrIbd", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "IBD", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"JErrIbod", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "IBOD", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JErrWind", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{"ERR", ",", + RowBox[{"{", "WINDW", "}"}]}], "]"}], ",", "NOISES"}], "]"}]}], + ";"}], "\[IndentingNewLine]"}], "Input", + CellChangeTimes->{{3.632488242861087*^9, 3.63248826986901*^9}, { + 3.632488302807823*^9, 3.632488310708211*^9}, {3.632549179684801*^9, + 3.6325491830062933`*^9}, 3.632549233454729*^9, {3.772359825953252*^9, + 3.772359833140972*^9}, {3.7723603795875998`*^9, 3.7723605440122347`*^9}, { + 3.772364327412218*^9, 3.772364391371952*^9}, {3.774583805536461*^9, + 3.774583830109449*^9}, {3.775206417689166*^9, 3.77520651396764*^9}, { + 3.77520681773381*^9, 3.7752068235363483`*^9}}, + CellLabel-> + "In[1317]:=",ExpressionUUID->"cdb0dbd5-e787-49a0-bb76-c4468b401cbc"], + +Cell[BoxData[ + RowBox[{ + RowBox[{"Run", "[", "\"\\"", "]"}], ";"}]], "Input", + CellChangeTimes->{{3.605001578968874*^9, 3.605001586300552*^9}}, + CellLabel-> + "In[1330]:=",ExpressionUUID->"41b82835-d8e1-446a-bec9-5cabfad38221"], + +Cell[BoxData[{ + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "Err", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrNoises", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrPose", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrQ", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrV", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrW", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrA", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrAlpha", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrAirDensity", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrFThrust", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrFDrag", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrFLat", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrFLift", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrMRoll", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrMPitch", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrMYaw", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrCBar", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrCp", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrIbd", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrIbod", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JErrWind", "]"}], ",", + "\"\\""}], "]"}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", + RowBox[{"Flatten", "[", + RowBox[{"{", + RowBox[{"SWhat", ",", "QWShat"}], "}"}], "]"}], "]"}], ",", + "\"\\""}], "]"}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "DBG", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", + RowBox[{"Flatten", "[", + RowBox[{"{", + RowBox[{"AhatlongDBG", ",", "AlphahatlongDBG"}], "}"}], "]"}], "]"}], + ",", "\"\\""}], "]"}], + ";"}]}], "Input", + CellChangeTimes->{{3.57329895973281*^9, 3.5732990549001513`*^9}, { + 3.57329915658729*^9, 3.5732991792126017`*^9}, {3.57329950483379*^9, + 3.573299506552189*^9}, {3.573456025938487*^9, 3.573456050810623*^9}, { + 3.574152257417694*^9, 3.574152270678878*^9}, {3.579848972820836*^9, + 3.579848980632938*^9}, {3.5814019928034163`*^9, 3.58140200617373*^9}, { + 3.581402211401102*^9, 3.581402211596108*^9}, {3.581413827590354*^9, + 3.5814138412416353`*^9}, {3.581414159871148*^9, 3.5814141678166847`*^9}, { + 3.5814153831753387`*^9, 3.581415395342497*^9}, 3.5817651746256*^9, { + 3.581765299311152*^9, 3.581765299456869*^9}, {3.583128034440399*^9, + 3.583128047560751*^9}, 3.600064481693171*^9, {3.6000645447460413`*^9, + 3.600064544768753*^9}, 3.605001560621763*^9, {3.606108718930592*^9, + 3.606108718954677*^9}, {3.606109458423176*^9, 3.6061094764764013`*^9}, { + 3.606109797740724*^9, 3.6061098090662613`*^9}, {3.6061099568596067`*^9, + 3.6061099585045424`*^9}, {3.617517858078869*^9, 3.6175178592248383`*^9}, { + 3.6175179081434183`*^9, 3.617517920863043*^9}, {3.617535815115308*^9, + 3.6175358335236387`*^9}, {3.6176913283286*^9, 3.617691368379365*^9}, { + 3.61915374051689*^9, 3.6191538003658953`*^9}, {3.624878788568387*^9, + 3.6248788166517553`*^9}, {3.6248826307742863`*^9, 3.624882631277665*^9}, { + 3.626606948234263*^9, 3.626606949108878*^9}, {3.626608154489799*^9, + 3.6266082045678787`*^9}, {3.626608392816053*^9, 3.62660839330686*^9}, { + 3.6297095365887547`*^9, 3.629709536615233*^9}, {3.629710549109893*^9, + 3.629710565390929*^9}, {3.630321740435486*^9, 3.630321740476594*^9}, { + 3.63032317635601*^9, 3.630323195570113*^9}, {3.632488330236774*^9, + 3.6324884066409388`*^9}, {3.632549186227305*^9, 3.6325491909421*^9}, { + 3.632636908066628*^9, 3.632636924909555*^9}, {3.633235060777438*^9, + 3.6332350814039917`*^9}, {3.634287350134149*^9, 3.63428738680274*^9}, { + 3.6342984069072943`*^9, 3.634298419729596*^9}, {3.63429849221801*^9, + 3.6342985327140083`*^9}, {3.772364411990938*^9, 3.7723645797179956`*^9}, + 3.831547472610442*^9, 3.8315475368389463`*^9}, + CellLabel-> + "In[1331]:=",ExpressionUUID->"cbb713d0-b847-42c0-9c5d-8772380d8dba"], + +Cell[BoxData[{ + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], ";"}], + "\[IndentingNewLine]"}], "\[IndentingNewLine]"}], "Input", + CellChangeTimes->CompressedData[" +1:eJwdzW1Ik1EYxvFpGmqSlrZIoymWq60CZ+X0Q3Eqh42KsSB8RVODJDdpIljZ +tKFbZii6rSRCC3S+pLNsoZiYU0hnaqYjtNhmS5c8z7ScSyFNO/f5cPH7dv3D +M/PE1z0ZDEY0Hlj3QzyjrKfQUsUbIq/EawvUULsZKuyNgX8HQb7WzgbNWyYh +mKocI35em1NPYZlyfw04nLFVzn6Bf5btD0FXqWcV2DXlS4wbqo3wbsO9oHXi +Y1tz/rqeQoOx4QVgZdL2WI8OCiWXmeJAZ5M4Czw/+IkYuLd/A1SqzUS3gX9a +0EmhkEYt8dFcwqZ6kkKS4kIi512glwZrMTCJ+hxbfKiVQsbMWWL3WIsO5AiL +m8BDYdMT4NOL5kkwN0LJsa5QaPykiujDct+axU6reTJwdW6JdmAfXOYvgkxm +upXnplBpQZoNFJsN7Gis6ImDA7rz/OmNVQoljkiIfg0VLjCjWPgH7I1MSVrw +oJGI65MGOunmoye8aRQgfU80mns/gsO14lHQ1Tj6HWytC7ODzfuC97N20Kgz +YA9RY1PpQJkltwnUGat8hf40ijpXQ5QorvSD8V9SjODxSxeWbwbSqHBcSMyO +jNoAJw63b4LHBnTp5Vj9S78MMEGQ1QIWFbUSrVxTiQG7IlPfBzO59urpXTTq +O7tWA5pZHde+BtHo2aYlG+z5dnvIhu2ujjGB8l8ffp4KdaKi9gUXOH9ENtp2 +wIn+irrGwd8jwYoRrNwUQ0wNtTaAiVwHUd+TUyFjOZF97DWx8mryPQnPiTy3 +lRH7FHoFqFp8S9Rmh8xIsZOCfKLrrsgCSs88J+5sq50HX83ecYD/ASe6sAk= + + "], + CellLabel-> + "In[1355]:=",ExpressionUUID->"1a2cbc4e-fbe6-4f7c-8741-b70e886cb2db"], + +Cell[CellGroupData[{ + +Cell[BoxData[ + RowBox[{"Run", "[", + "\"\\"", + "]"}]], "Input", + CellChangeTimes->{{3.6050014532183523`*^9, 3.6050015701885967`*^9}, { + 3.605001619532137*^9, 3.6050016309343433`*^9}, {3.626084473225601*^9, + 3.626084492513814*^9}, {3.6260847134763527`*^9, 3.6260847142374268`*^9}}, + CellLabel-> + "In[1386]:=",ExpressionUUID->"92a89388-7fc0-4900-84db-870a39602d88"], + +Cell[BoxData["0"], "Output", + CellChangeTimes->{ + 3.626084595698902*^9, {3.626084692883334*^9, 3.6260847202544928`*^9}, + 3.626085055644226*^9, {3.6266082860420322`*^9, 3.626608294517626*^9}, + 3.6266083422477207`*^9, 3.626608408034442*^9, 3.6279705231130753`*^9, + 3.627974194334207*^9, 3.627976022316907*^9, 3.627982078483562*^9, + 3.627983299927937*^9, 3.627992180265356*^9, 3.627992659732891*^9, + 3.62799287386307*^9, 3.6282290101624613`*^9, 3.6297105708954773`*^9, + 3.6297106574368258`*^9, 3.6298061279971113`*^9, 3.629807720408*^9, + 3.63032321558445*^9, 3.6324884540770473`*^9, 3.632488624588769*^9, + 3.632548341659026*^9, 3.632548980662763*^9, 3.632549207713517*^9, + 3.6325492403682756`*^9, 3.632571746113782*^9, 3.632635561755374*^9, + 3.632636954213489*^9, 3.6328206639342012`*^9, 3.633066575735487*^9, + 3.633067332266951*^9, 3.633235102950844*^9, 3.633327218459009*^9, + 3.633668547850634*^9, 3.633669240128911*^9, 3.6342783048055973`*^9, + 3.634278361480673*^9, 3.634287411095068*^9, 3.6342924066725883`*^9, + 3.634298448119008*^9, {3.634298529980817*^9, 3.6342985373529863`*^9}, + 3.634298597389635*^9, 3.634299652755254*^9, 3.634299691384879*^9, + 3.634299857136918*^9, 3.634299889682819*^9, 3.634299958854151*^9, + 3.63430011568889*^9, {3.634300182757251*^9, 3.634300204063946*^9}, + 3.634300381734619*^9, 3.634301695788665*^9, 3.634302340168003*^9, + 3.634303008624929*^9, 3.6343039186453753`*^9, 3.634452382385035*^9, + 3.634452775895094*^9, 3.634980290608531*^9, 3.634980867087916*^9, { + 3.634982332576049*^9, 3.6349823386672897`*^9}, 3.771327259597065*^9, + 3.771569957428474*^9, 3.7723647052058573`*^9, 3.7724326260823793`*^9, + 3.772432982619478*^9, 3.772946110952771*^9, 3.773142622983836*^9, { + 3.7741789490039988`*^9, 3.774178957149096*^9}, 3.774239768380513*^9, + 3.774266326673469*^9, 3.77458390783916*^9, 3.774612092999992*^9, + 3.774691158272262*^9, 3.7746926287703743`*^9, {3.775206430535994*^9, + 3.775206518604731*^9}, 3.7752066826065903`*^9, 3.775206790200211*^9, + 3.775206827144771*^9, 3.781531524359655*^9, {3.781532274516631*^9, + 3.781532281751*^9}, {3.782109514640354*^9, 3.782109522477334*^9}, + 3.784535070682267*^9, 3.784537809638708*^9, 3.784537843760861*^9, + 3.784537914040141*^9, 3.820073727931135*^9, 3.831544438892476*^9, + 3.831548150530548*^9, 3.831548464699*^9, 3.831550705400441*^9, + 3.831552346161853*^9, 3.831997320121353*^9, 3.8320364983368073`*^9, + 3.8320405504472303`*^9, 3.8320481854565268`*^9}, + CellLabel-> + "Out[1386]=",ExpressionUUID->"8f21e56d-014c-4c8d-b2d3-5a5578453982"] +}, Open ]], + +Cell[BoxData[{ + RowBox[{ + RowBox[{"Run", "[", "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"Run", "[", "\"\\"", "]"}], ";"}]}], "Input", + CellLabel-> + "In[1387]:=",ExpressionUUID->"aaff244c-3ad5-4faf-aeb3-7cdbaa9f50f5"], + +Cell[BoxData[" "], "Input", + CellChangeTimes->{3.627992683770735*^9}, + CellLabel-> + "In[1389]:=",ExpressionUUID->"8714cb77-63af-49bd-8987-722049f040d7"] +}, +CellGrouping->Manual, +WindowSize->{1386, 852}, +WindowMargins->{{0., 0.}, {-90., 27.75}}, +Magnification:>0.9 Inherited, +FrontEndVersion->"12.2 for Linux x86 (64-bit) (December 12, 2020)", +StyleDefinitions->"Default.nb", +ExpressionUUID->"1d696320-bd25-4c6c-bc1e-4ec757dce194" +] +(* End of Notebook Content *) + +(* Internal cache information *) +(*CellTagsOutline +CellTagsIndex->{} +*) +(*CellTagsIndex +CellTagsIndex->{} +*) +(*NotebookFileOutline +Notebook[{ +Cell[558, 20, 225, 4, 26, "Input",ExpressionUUID->"28aebd44-7a23-4603-b370-38149a649cc1"], +Cell[786, 26, 559, 13, 46, "Input",ExpressionUUID->"d5e95da8-5675-4df2-8ff6-af7b8fb219a1"], +Cell[CellGroupData[{ +Cell[1370, 43, 242, 5, 47, "Subtitle",ExpressionUUID->"addb9730-72b2-4859-8e4b-0e776f073328", + InitializationCell->True], +Cell[1615, 50, 969, 31, 40, "Input",ExpressionUUID->"78e5840d-381c-47a9-b9dd-90c3c4dacb03", + InitializationCell->True], +Cell[2587, 83, 1659, 48, 40, "Input",ExpressionUUID->"66ac8851-0848-4866-8069-42c37f27b5de", + InitializationCell->True], +Cell[4249, 133, 949, 30, 61, "Input",ExpressionUUID->"a40aa98c-a7a2-4814-b63b-e7c90c9932a0", + InitializationCell->True], +Cell[5201, 165, 1657, 50, 40, "Input",ExpressionUUID->"ee9449fb-cd56-4179-b90c-f5e3bb0fdcdb", + InitializationCell->True], +Cell[6861, 217, 978, 26, 42, "Input",ExpressionUUID->"b69052f0-f728-4bd6-9bf7-bae15aa76095", + InitializationCell->True], +Cell[7842, 245, 1350, 41, 60, "Input",ExpressionUUID->"905c9ad7-8258-45b1-b64f-2d576f63cf20", + InitializationCell->True], +Cell[9195, 288, 2515, 66, 171, "Input",ExpressionUUID->"a7b9df83-8379-4a26-b658-cd16a2c680e3", + InitializationCell->True] +}, Open ]], +Cell[11725, 357, 188, 3, 88, "Title",ExpressionUUID->"243eeb2a-7eb6-46c5-afb4-9166e290b8da"], +Cell[11916, 362, 156, 3, 27, "Subsubtitle",ExpressionUUID->"934e8056-77ed-401a-af0c-38f7a1beea69"], +Cell[12075, 367, 423, 11, 26, "Input",ExpressionUUID->"89e76b07-ca4f-4879-a9de-3406cd562327"], +Cell[12501, 380, 1437, 39, 120, "Input",ExpressionUUID->"0f88cdc1-a39e-4d13-b528-8472e029b05f"], +Cell[13941, 421, 159, 3, 27, "Subsubtitle",ExpressionUUID->"a1d3915d-f613-4425-8a04-fe27c5187298"], +Cell[14103, 426, 8705, 221, 918, "Input",ExpressionUUID->"33f47236-ad3c-4a61-8f93-8222aa6cc9fa"], +Cell[22811, 649, 4441, 128, 380, "Input",ExpressionUUID->"53a40cfc-f0e8-4e44-9f25-b052baa99c29"], +Cell[27255, 779, 443, 13, 26, "Input",ExpressionUUID->"be98cbcc-62a9-4b36-a592-7c1827bbba19"], +Cell[27701, 794, 1696, 49, 138, "Input",ExpressionUUID->"3db40cdd-aae8-4aed-bbcf-7b6c917acc34"], +Cell[29400, 845, 287, 5, 27, "Subsubtitle",ExpressionUUID->"3a22e419-d65c-4c9e-84c6-863e409e519a"], +Cell[29690, 852, 208, 4, 31, "Text",ExpressionUUID->"9ac0d911-0e84-4579-bcb9-2110f48ca902"], +Cell[CellGroupData[{ +Cell[29923, 860, 1985, 48, 65, "Input",ExpressionUUID->"9f81cfa6-a749-4450-bfb3-736066b50bce"], +Cell[31911, 910, 261, 4, 30, "Output",ExpressionUUID->"c15d1718-07db-4a27-8450-bddfe12047c1"] +}, Open ]], +Cell[32187, 917, 154, 3, 31, "Text",ExpressionUUID->"656d798c-a5c9-4cc6-a1dd-06c8dc6c6565"], +Cell[32344, 922, 1748, 44, 102, "Input",ExpressionUUID->"29b01d2f-83ef-4d5a-8a25-69d70c2e9207"], +Cell[34095, 968, 157, 3, 31, "Text",ExpressionUUID->"7aa4501a-e764-4f85-8410-73dc94387049"], +Cell[34255, 973, 1118, 24, 28, "Input",ExpressionUUID->"dbea6cef-0336-433e-92e3-cc59b4a69219"], +Cell[35376, 999, 519, 14, 28, "Input",ExpressionUUID->"6d11debc-7473-46de-b505-2a1ab0c568e8"], +Cell[35898, 1015, 178, 3, 31, "Text",ExpressionUUID->"15e73123-e84e-4360-b55b-334ef918a821"], +Cell[36079, 1020, 2060, 51, 83, "Input",ExpressionUUID->"176642b9-ef21-428e-bda6-88b3064f0fe2"], +Cell[38142, 1073, 152, 3, 31, "Text",ExpressionUUID->"ac9144ba-14f4-4cb2-9d7c-1b84aa717d7b"], +Cell[38297, 1078, 718, 17, 46, "Input",ExpressionUUID->"f4931c36-9cb3-41d5-9636-f3fbcef428c9"], +Cell[39018, 1097, 154, 3, 31, "Text",ExpressionUUID->"57ce017d-559b-4d58-ba18-1e7631c436d9"], +Cell[39175, 1102, 412, 10, 46, "Input",ExpressionUUID->"63203ee0-9601-47b1-9606-b67b10ce9a46"], +Cell[39590, 1114, 156, 3, 31, "Text",ExpressionUUID->"4432b235-bb2b-4e9d-87f8-55bf93c648df"], +Cell[39749, 1119, 594, 14, 46, "Input",ExpressionUUID->"c73ee33c-6f07-4ea9-82e2-0f37fad73c8e"], +Cell[40346, 1135, 153, 3, 31, "Text",ExpressionUUID->"3b78e61b-a31c-43c6-949c-8ab46a558347"], +Cell[40502, 1140, 1571, 40, 83, "Input",ExpressionUUID->"7be4a0a1-b2c0-4d7b-b564-9f92dabf375f"], +Cell[42076, 1182, 154, 3, 31, "Text",ExpressionUUID->"78b600c8-17c9-43b2-8561-39fdff7b92e4"], +Cell[42233, 1187, 1081, 27, 65, "Input",ExpressionUUID->"4ce3e11d-b2d8-42df-94f3-f44671feb140"], +Cell[43317, 1216, 154, 3, 31, "Text",ExpressionUUID->"c302290e-b2df-4299-8d3e-f59d1fc850b6"], +Cell[43474, 1221, 741, 16, 46, "Input",ExpressionUUID->"1a28b43d-e9a2-4ebf-8251-f3ab2ddb7786"], +Cell[44218, 1239, 159, 3, 31, "Text",ExpressionUUID->"4ba37bc9-3ca4-4415-be72-e388f0dad47a"], +Cell[44380, 1244, 2219, 50, 46, "Input",ExpressionUUID->"fe419f0a-adf5-43af-9f08-7b8a7f19180b"], +Cell[46602, 1296, 1001, 17, 65, "Input",ExpressionUUID->"ff5f7d73-add6-4e24-9e55-7b15786660c6"], +Cell[47606, 1315, 2190, 55, 65, "Input",ExpressionUUID->"d17df8c1-9260-4a0c-8495-b5ef6f820848"], +Cell[CellGroupData[{ +Cell[49821, 1374, 588, 14, 46, "Input",ExpressionUUID->"8c91f72d-3f47-4547-a792-5e0813bcc352"], +Cell[50412, 1390, 17260, 432, 220, "Output",ExpressionUUID->"418050e7-4d56-4166-bdad-8ceb9141adfa"] +}, Open ]], +Cell[67687, 1825, 204, 3, 29, "Subsubtitle",ExpressionUUID->"b92b1bb0-abe4-4170-8cfc-1e9dd8bc5679"], +Cell[67894, 1830, 957, 25, 83, "Input",ExpressionUUID->"dea35a84-3a0f-4600-a2f4-2ab045608d87"], +Cell[68854, 1857, 154, 3, 29, "Subsubtitle",ExpressionUUID->"bf1afef8-2e19-4e8e-9c26-8b9e596cd10c"], +Cell[69011, 1862, 207, 5, 28, "Input",ExpressionUUID->"c2f98c75-5faf-44c8-a958-147c2a660678"], +Cell[69221, 1869, 3710, 87, 158, "Input",ExpressionUUID->"716ee195-4767-4067-92df-43ed8da3bd15"], +Cell[72934, 1958, 3895, 118, 365, "Input",ExpressionUUID->"cdb0dbd5-e787-49a0-bb76-c4468b401cbc"], +Cell[76832, 2078, 244, 5, 27, "Input",ExpressionUUID->"41b82835-d8e1-446a-bec9-5cabfad38221"], +Cell[77079, 2085, 7278, 183, 510, "Input",ExpressionUUID->"cbb713d0-b847-42c0-9c5d-8772380d8dba"], +Cell[84360, 2270, 6366, 197, 684, "Input",ExpressionUUID->"1a2cbc4e-fbe6-4f7c-8741-b70e886cb2db"], +Cell[CellGroupData[{ +Cell[90751, 2471, 428, 8, 28, "Input",ExpressionUUID->"92a89388-7fc0-4900-84db-870a39602d88"], +Cell[91182, 2481, 2612, 38, 30, "Output",ExpressionUUID->"8f21e56d-014c-4c8d-b2d3-5a5578453982"] +}, Open ]], +Cell[93809, 2522, 276, 7, 46, "Input",ExpressionUUID->"aaff244c-3ad5-4faf-aeb3-7cdbaa9f50f5"], +Cell[94088, 2531, 153, 3, 26, "Input",ExpressionUUID->"8714cb77-63af-49bd-8987-722049f040d7"] +} +] +*) + diff --git a/_development/Mathematica/models/BackwardAugmentedStateEstimator_Splitted_v7.nb b/_development/Mathematica/models/BackwardAugmentedStateEstimator_Splitted_v7.nb index 7362bb2..db902fd 100644 --- a/_development/Mathematica/models/BackwardAugmentedStateEstimator_Splitted_v7.nb +++ b/_development/Mathematica/models/BackwardAugmentedStateEstimator_Splitted_v7.nb @@ -10,10 +10,10 @@ NotebookFileLineBreakTest NotebookFileLineBreakTest NotebookDataPosition[ 145, 7] -NotebookDataLength[ 322388, 8377] -NotebookOptionsPosition[ 318360, 8258] -NotebookOutlinePosition[ 318778, 8276] -CellTagsIndexPosition[ 318735, 8273] +NotebookDataLength[ 357674, 9230] +NotebookOptionsPosition[ 349678, 9108] +NotebookOutlinePosition[ 350245, 9129] +CellTagsIndexPosition[ 350202, 9126] WindowFrame->Normal*) (* Beginning of Notebook Content *) @@ -26,7 +26,8 @@ Cell[BoxData[ 3.5904073811525173`*^9}, 3.590407436078867*^9, 3.590407623241819*^9, { 3.5904747733191013`*^9, 3.590474776295134*^9}, {3.590479653130921*^9, 3.5904796555282288`*^9}, {3.6083589169527884`*^9, 3.608358917999106*^9}}, - ExpressionUUID -> "e0db13eb-9582-4c6d-b546-c853c2642b35"], + CellLabel-> + "In[334]:=",ExpressionUUID->"e0db13eb-9582-4c6d-b546-c853c2642b35"], Cell[BoxData[{ RowBox[{"Import", "[", @@ -48,7 +49,8 @@ Cell[BoxData[{ 3.606027060840419*^9, 3.606027061775948*^9}, {3.6065840017110653`*^9, 3.606584007040091*^9}, {3.6083590571209*^9, 3.608359062288601*^9}, { 3.608439837053685*^9, 3.6084398384415483`*^9}}, - ExpressionUUID -> "3c600d9d-2e79-41d3-8083-e1831361c722"], + CellLabel-> + "In[335]:=",ExpressionUUID->"3c600d9d-2e79-41d3-8083-e1831361c722"], Cell["Quaternion Functions", "Subtitle", InitializationCell->True, @@ -87,7 +89,8 @@ Cell[BoxData[ InitializationCell->True, CellChangeTimes->{{3.563607037606062*^9, 3.563607124416689*^9}, 3.563608231083735*^9}, - ExpressionUUID -> "f86ff61e-6031-4947-996f-c15c46271dbb"], + CellLabel-> + "In[339]:=",ExpressionUUID->"f86ff61e-6031-4947-996f-c15c46271dbb"], Cell[BoxData[ RowBox[{ @@ -136,7 +139,8 @@ Cell[BoxData[ CellChangeTimes->{{3.571130663264105*^9, 3.571130765329321*^9}, { 3.57113111793888*^9, 3.57113116341093*^9}, {3.571131195958424*^9, 3.571131254626845*^9}, 3.571133049520565*^9}, - ExpressionUUID -> "c6408360-b2e8-4480-9616-8c693b01c013"], + CellLabel-> + "In[340]:=",ExpressionUUID->"c6408360-b2e8-4480-9616-8c693b01c013"], Cell[BoxData[{ RowBox[{ @@ -167,7 +171,8 @@ Cell[BoxData[{ InitializationCell->True, CellChangeTimes->{{3.568453075302482*^9, 3.568453143924776*^9}}, - ExpressionUUID -> "277e1463-1c11-4b65-b1bc-b28d2b0e5524"], + CellLabel-> + "In[341]:=",ExpressionUUID->"277e1463-1c11-4b65-b1bc-b28d2b0e5524"], Cell[BoxData[ RowBox[{ @@ -218,7 +223,8 @@ Cell[BoxData[ InitializationCell->True, CellChangeTimes->{{3.568443187261707*^9, 3.568443273520089*^9}, { 3.568458568630984*^9, 3.568458568967643*^9}}, - ExpressionUUID -> "2d39ecd2-7dd3-4880-9ef7-4ddd41eaff19"], + CellLabel-> + "In[343]:=",ExpressionUUID->"2d39ecd2-7dd3-4880-9ef7-4ddd41eaff19"], Cell[BoxData[ RowBox[{ @@ -245,7 +251,8 @@ Cell[BoxData[ 3.568443103374373*^9, 3.568443183239631*^9}, {3.56844338461282*^9, 3.568443409373211*^9}, {3.568453008511107*^9, 3.568453014783046*^9}, { 3.60835655838758*^9, 3.608356558395439*^9}}, - ExpressionUUID -> "a8b63d28-8415-46db-898f-c535129b02a0"], + CellLabel-> + "In[344]:=",ExpressionUUID->"a8b63d28-8415-46db-898f-c535129b02a0"], Cell[BoxData[{ RowBox[{ @@ -287,7 +294,8 @@ Cell[BoxData[{ InitializationCell->True, CellChangeTimes->{{3.568444740747602*^9, 3.568444776506934*^9}, { 3.568444838483142*^9, 3.568444844345901*^9}}, - ExpressionUUID -> "13e8fd29-a86a-4514-9ef2-bbe37133ffab"], + CellLabel-> + "In[345]:=",ExpressionUUID->"13e8fd29-a86a-4514-9ef2-bbe37133ffab"], Cell[BoxData[{ RowBox[{ @@ -354,7 +362,8 @@ Cell[BoxData[{ 3.568440350374321*^9, {3.56844481385726*^9, 3.56844481884285*^9}, { 3.568446816971497*^9, 3.568446831371454*^9}, {3.56844689667688*^9, 3.568446897039928*^9}, {3.571133081827365*^9, 3.571133085326789*^9}}, - ExpressionUUID -> "e2231016-d80a-43fc-841d-ee590b1c2a49"], + CellLabel-> + "In[347]:=",ExpressionUUID->"e2231016-d80a-43fc-841d-ee590b1c2a49"], Cell["Variables", "Subsubtitle", InitializationCell->True, @@ -438,7 +447,8 @@ Cell[BoxData[{ 3.571122932846132*^9}, {3.571123154234276*^9, 3.571123155473277*^9}, { 3.571123269113247*^9, 3.571123271208196*^9}, 3.571123853538535*^9, 3.573198433015477*^9}, - ExpressionUUID -> "03f4c2ce-f63a-48b8-8e96-2ef2f3664780"], + CellLabel-> + "In[349]:=",ExpressionUUID->"03f4c2ce-f63a-48b8-8e96-2ef2f3664780"], Cell[BoxData[{ RowBox[{ @@ -458,7 +468,8 @@ Cell[BoxData[{ RowBox[{"i", ",", "1", ",", "3"}], "}"}]}], "]"}]}], ";"}]}], "Input", InitializationCell->True, CellChangeTimes->{{3.571130560401196*^9, 3.571130590779293*^9}}, - ExpressionUUID -> "8356b1fb-37a1-4b3c-a6a1-002dc40c7ee1"], + CellLabel-> + "In[358]:=",ExpressionUUID->"8356b1fb-37a1-4b3c-a6a1-002dc40c7ee1"], Cell[BoxData[{ RowBox[{ @@ -487,7 +498,8 @@ Cell[BoxData[{ 3.576324418444131*^9}, {3.5873662786916847`*^9, 3.5873662817327967`*^9}, { 3.5873663356887217`*^9, 3.5873663411038713`*^9}, {3.606622958496975*^9, 3.606622959804261*^9}, {3.7624930325049877`*^9, 3.762493059518791*^9}}, - ExpressionUUID -> "3cd926d4-767b-409d-b5ee-f0b985910568"], + CellLabel-> + "In[360]:=",ExpressionUUID->"3cd926d4-767b-409d-b5ee-f0b985910568"], Cell["Equations", "Subsubtitle", CellChangeTimes->{{3.571119620568287*^9, 3.571119644637387*^9}, { @@ -505,18 +517,32 @@ Cell[BoxData[{ RowBox[{"(", RowBox[{"POSE2", "-", "POSE1"}], ")"}], "/", "Dt12"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"VWprev", "=", + RowBox[{ + RowBox[{"(", + RowBox[{"POSE1", "-", "POSE0"}], ")"}], "/", "Dt01"}]}], + ";"}], "\[IndentingNewLine]", RowBox[{ RowBox[{"OmegaW", "=", RowBox[{ RowBox[{"QuatBoxMinusLeft", "[", RowBox[{"Q2", ",", "Q1"}], "]"}], "/", "Dt12"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"OmegaWprev", "=", + RowBox[{ + RowBox[{"QuatBoxMinusLeft", "[", + RowBox[{"Q1", ",", "Q0"}], "]"}], "/", "Dt01"}]}], + ";"}], "\[IndentingNewLine]", RowBox[{"VW", "//", "MatrixForm"}], "\[IndentingNewLine]", RowBox[{"OmegaW", "//", "MatrixForm"}]}], "Input", CellChangeTimes->{{3.608356260450452*^9, 3.608356302540146*^9}, { 3.608356558162717*^9, 3.6083565581740026`*^9}, {3.608438730640964*^9, - 3.608438742493989*^9}}, - ExpressionUUID -> "2618f75a-15b8-4eab-8502-5b0faafa4be4"], + 3.608438742493989*^9}, {3.832653513796719*^9, 3.832653528028558*^9}, { + 3.8326535603881607`*^9, 3.8326535753318233`*^9}}, + CellLabel-> + "In[362]:=",ExpressionUUID->"2618f75a-15b8-4eab-8502-5b0faafa4be4"], Cell[BoxData[ TagBox[ @@ -541,16 +567,14 @@ Cell[BoxData[ RowBox[{"x1", "[", "3", "]"}]}], "+", RowBox[{"x2", "[", "3", "]"}]}], "Dt12"]} }, - GridBoxAlignment->{ - "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, - "RowsIndexed" -> {}}, + GridBoxAlignment->{"Columns" -> {{Center}}, "Rows" -> {{Baseline}}}, GridBoxSpacings->{"Columns" -> { Offset[0.27999999999999997`], { Offset[0.5599999999999999]}, - Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> { + Offset[0.27999999999999997`]}, "Rows" -> { Offset[0.2], { Offset[0.4]}, - Offset[0.2]}, "RowsIndexed" -> {}}], + Offset[0.2]}}], Column], "\[NoBreak]", ")"}], Function[BoxForm`e$, MatrixForm[BoxForm`e$]]]], "Output", @@ -562,71 +586,75 @@ Cell[BoxData[ 3.634452465927414*^9, 3.697261401748468*^9, 3.6972615974398727`*^9, 3.697263590022244*^9, 3.697263626811467*^9, 3.752577459678912*^9, 3.7528134404503517`*^9, 3.762494040298297*^9, 3.763877880508068*^9, - 3.823863453641492*^9, 3.823863630022009*^9, 3.823933120232155*^9}, - ExpressionUUID -> "b3ab19c1-ac7e-4b2b-a502-b75a85232038"], + 3.823863453641492*^9, 3.823863630022009*^9, 3.823933120232155*^9, + 3.832654020444796*^9, 3.832654298791222*^9, 3.832655256536832*^9, + 3.832682635135798*^9, 3.832841847282155*^9}, + CellLabel-> + "Out[366]//MatrixForm=",ExpressionUUID->"18271bfb-de94-406b-bcae-\ +51c0d0b6c135"], Cell[BoxData[ TagBox[ RowBox[{"(", "\[NoBreak]", TagBox[GridBox[{ { - FractionBox[ - RowBox[{"2", " ", - RowBox[{"(", - RowBox[{ - RowBox[{ - RowBox[{"-", - RowBox[{"x1", "[", "5", "]"}]}], " ", - RowBox[{"x2", "[", "4", "]"}]}], "+", - RowBox[{ - RowBox[{"x1", "[", "4", "]"}], " ", - RowBox[{"x2", "[", "5", "]"}]}], "-", + RowBox[{"-", + FractionBox[ + RowBox[{"2", " ", + RowBox[{"(", RowBox[{ - RowBox[{"x1", "[", "7", "]"}], " ", - RowBox[{"x2", "[", "6", "]"}]}], "+", + RowBox[{ + RowBox[{"x1", "[", "5", "]"}], " ", + RowBox[{"x2", "[", "4", "]"}]}], "-", + RowBox[{ + RowBox[{"x1", "[", "4", "]"}], " ", + RowBox[{"x2", "[", "5", "]"}]}], "+", + RowBox[{ + RowBox[{"x1", "[", "7", "]"}], " ", + RowBox[{"x2", "[", "6", "]"}]}], "-", + RowBox[{ + RowBox[{"x1", "[", "6", "]"}], " ", + RowBox[{"x2", "[", "7", "]"}]}]}], ")"}]}], + RowBox[{"Dt12", " ", + RowBox[{"(", RowBox[{ - RowBox[{"x1", "[", "6", "]"}], " ", - RowBox[{"x2", "[", "7", "]"}]}]}], ")"}]}], - RowBox[{"Dt12", " ", - RowBox[{"(", - RowBox[{ - SuperscriptBox[ - RowBox[{"x1", "[", "4", "]"}], "2"], "+", - SuperscriptBox[ - RowBox[{"x1", "[", "5", "]"}], "2"], "+", - SuperscriptBox[ - RowBox[{"x1", "[", "6", "]"}], "2"], "+", - SuperscriptBox[ - RowBox[{"x1", "[", "7", "]"}], "2"]}], ")"}]}]]}, + SuperscriptBox[ + RowBox[{"x1", "[", "4", "]"}], "2"], "+", + SuperscriptBox[ + RowBox[{"x1", "[", "5", "]"}], "2"], "+", + SuperscriptBox[ + RowBox[{"x1", "[", "6", "]"}], "2"], "+", + SuperscriptBox[ + RowBox[{"x1", "[", "7", "]"}], "2"]}], ")"}]}]]}]}, { - FractionBox[ - RowBox[{"2", " ", - RowBox[{"(", - RowBox[{ - RowBox[{ - RowBox[{"-", - RowBox[{"x1", "[", "6", "]"}]}], " ", - RowBox[{"x2", "[", "4", "]"}]}], "+", - RowBox[{ - RowBox[{"x1", "[", "7", "]"}], " ", - RowBox[{"x2", "[", "5", "]"}]}], "+", + RowBox[{"-", + FractionBox[ + RowBox[{"2", " ", + RowBox[{"(", RowBox[{ - RowBox[{"x1", "[", "4", "]"}], " ", - RowBox[{"x2", "[", "6", "]"}]}], "-", + RowBox[{ + RowBox[{"x1", "[", "6", "]"}], " ", + RowBox[{"x2", "[", "4", "]"}]}], "-", + RowBox[{ + RowBox[{"x1", "[", "7", "]"}], " ", + RowBox[{"x2", "[", "5", "]"}]}], "-", + RowBox[{ + RowBox[{"x1", "[", "4", "]"}], " ", + RowBox[{"x2", "[", "6", "]"}]}], "+", + RowBox[{ + RowBox[{"x1", "[", "5", "]"}], " ", + RowBox[{"x2", "[", "7", "]"}]}]}], ")"}]}], + RowBox[{"Dt12", " ", + RowBox[{"(", RowBox[{ - RowBox[{"x1", "[", "5", "]"}], " ", - RowBox[{"x2", "[", "7", "]"}]}]}], ")"}]}], - RowBox[{"Dt12", " ", - RowBox[{"(", - RowBox[{ - SuperscriptBox[ - RowBox[{"x1", "[", "4", "]"}], "2"], "+", - SuperscriptBox[ - RowBox[{"x1", "[", "5", "]"}], "2"], "+", - SuperscriptBox[ - RowBox[{"x1", "[", "6", "]"}], "2"], "+", - SuperscriptBox[ - RowBox[{"x1", "[", "7", "]"}], "2"]}], ")"}]}]]}, + SuperscriptBox[ + RowBox[{"x1", "[", "4", "]"}], "2"], "+", + SuperscriptBox[ + RowBox[{"x1", "[", "5", "]"}], "2"], "+", + SuperscriptBox[ + RowBox[{"x1", "[", "6", "]"}], "2"], "+", + SuperscriptBox[ + RowBox[{"x1", "[", "7", "]"}], "2"]}], ")"}]}]]}]}, { FractionBox[ RowBox[{"2", " ", @@ -657,16 +685,14 @@ Cell[BoxData[ SuperscriptBox[ RowBox[{"x1", "[", "7", "]"}], "2"]}], ")"}]}]]} }, - GridBoxAlignment->{ - "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, - "RowsIndexed" -> {}}, + GridBoxAlignment->{"Columns" -> {{Center}}, "Rows" -> {{Baseline}}}, GridBoxSpacings->{"Columns" -> { Offset[0.27999999999999997`], { Offset[0.5599999999999999]}, - Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> { + Offset[0.27999999999999997`]}, "Rows" -> { Offset[0.2], { Offset[0.4]}, - Offset[0.2]}, "RowsIndexed" -> {}}], + Offset[0.2]}}], Column], "\[NoBreak]", ")"}], Function[BoxForm`e$, MatrixForm[BoxForm`e$]]]], "Output", @@ -678,8 +704,12 @@ Cell[BoxData[ 3.634452465927414*^9, 3.697261401748468*^9, 3.6972615974398727`*^9, 3.697263590022244*^9, 3.697263626811467*^9, 3.752577459678912*^9, 3.7528134404503517`*^9, 3.762494040298297*^9, 3.763877880508068*^9, - 3.823863453641492*^9, 3.823863630022009*^9, 3.823933120233395*^9}, - ExpressionUUID -> "b3ab19c1-ac7e-4b2b-a502-b75a85232038"] + 3.823863453641492*^9, 3.823863630022009*^9, 3.823933120232155*^9, + 3.832654020444796*^9, 3.832654298791222*^9, 3.832655256536832*^9, + 3.832682635135798*^9, 3.832841847286085*^9}, + CellLabel-> + "Out[367]//MatrixForm=",ExpressionUUID->"5845398f-f75f-49e2-bd78-\ +296d43853c95"] }, Open ]], Cell[CellGroupData[{ @@ -724,7 +754,8 @@ Cell[BoxData[{ 3.608356361899322*^9, 3.6083563733748083`*^9}, {3.608356558187619*^9, 3.6083565582437563`*^9}, {3.6084387739011183`*^9, 3.6084387740252523`*^9}}, - ExpressionUUID -> "a4331214-3c74-4555-af4c-a6061694ee5d"], + CellLabel-> + "In[368]:=",ExpressionUUID->"a4331214-3c74-4555-af4c-a6061694ee5d"], Cell[BoxData[ TagBox[ @@ -776,16 +807,14 @@ Cell[BoxData[ RowBox[{"x2", "[", "3", "]"}]}], "Dt12"]}], ")"}]}], RowBox[{"Dt01", "+", "Dt12"}]]} }, - GridBoxAlignment->{ - "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, - "RowsIndexed" -> {}}, + GridBoxAlignment->{"Columns" -> {{Center}}, "Rows" -> {{Baseline}}}, GridBoxSpacings->{"Columns" -> { Offset[0.27999999999999997`], { Offset[0.5599999999999999]}, - Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> { + Offset[0.27999999999999997`]}, "Rows" -> { Offset[0.2], { Offset[0.4]}, - Offset[0.2]}, "RowsIndexed" -> {}}], + Offset[0.2]}}], Column], "\[NoBreak]", ")"}], Function[BoxForm`e$, MatrixForm[BoxForm`e$]]]], "Output", @@ -797,8 +826,12 @@ Cell[BoxData[ 3.634452467462957*^9, 3.697261403246743*^9, 3.697261597510825*^9, 3.697263590119794*^9, 3.6972636268928127`*^9, 3.752577461017374*^9, 3.7528134414840927`*^9, 3.762494041280753*^9, 3.763877881506652*^9, - 3.8238634542442617`*^9, 3.823863631431241*^9, 3.823933121020006*^9}, - ExpressionUUID -> "79b1877e-46df-4200-8160-b1452dd9fb59"], + 3.8238634542442617`*^9, 3.823863631431241*^9, 3.823933121020006*^9, + 3.832654021233724*^9, 3.832654299561537*^9, 3.832655256582026*^9, + 3.83268263587398*^9, 3.8328418480104523`*^9}, + CellLabel-> + "Out[370]//MatrixForm=",ExpressionUUID->"51d66e53-6b4f-452c-9a84-\ +7eaa720e67e6"], Cell[BoxData[ TagBox[ @@ -988,16 +1021,14 @@ Cell[BoxData[ RowBox[{"x1", "[", "7", "]"}], "2"]}], ")"}]}]]}], ")"}]}], RowBox[{"Dt01", "+", "Dt12"}]]} }, - GridBoxAlignment->{ - "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, - "RowsIndexed" -> {}}, + GridBoxAlignment->{"Columns" -> {{Center}}, "Rows" -> {{Baseline}}}, GridBoxSpacings->{"Columns" -> { Offset[0.27999999999999997`], { Offset[0.5599999999999999]}, - Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> { + Offset[0.27999999999999997`]}, "Rows" -> { Offset[0.2], { Offset[0.4]}, - Offset[0.2]}, "RowsIndexed" -> {}}], + Offset[0.2]}}], Column], "\[NoBreak]", ")"}], Function[BoxForm`e$, MatrixForm[BoxForm`e$]]]], "Output", @@ -1009,8 +1040,12 @@ Cell[BoxData[ 3.634452467462957*^9, 3.697261403246743*^9, 3.697261597510825*^9, 3.697263590119794*^9, 3.6972636268928127`*^9, 3.752577461017374*^9, 3.7528134414840927`*^9, 3.762494041280753*^9, 3.763877881506652*^9, - 3.8238634542442617`*^9, 3.823863631431241*^9, 3.823933121022031*^9}, - ExpressionUUID -> "79b1877e-46df-4200-8160-b1452dd9fb59"] + 3.8238634542442617`*^9, 3.823863631431241*^9, 3.823933121020006*^9, + 3.832654021233724*^9, 3.832654299561537*^9, 3.832655256582026*^9, + 3.83268263587398*^9, 3.832841848012992*^9}, + CellLabel-> + "Out[371]//MatrixForm=",ExpressionUUID->"6c35c2d1-9c9d-464c-be6d-\ +256d48d18b3d"] }, Open ]], Cell[CellGroupData[{ @@ -1022,12 +1057,18 @@ Cell[BoxData[{ RowBox[{"QuatToRot", "[", RowBox[{"QuatInv", "[", "Q2", "]"}], "]"}], ".", "VW"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"WOlong", "=", + RowBox[{ + RowBox[{"QuatToRot", "[", + RowBox[{"QuatInv", "[", "Q2", "]"}], "]"}], ".", "OmegaW"}]}], + ";"}], "\[IndentingNewLine]", RowBox[{ RowBox[{ - RowBox[{"WOlong", "=", + RowBox[{"VOprevlong", "=", RowBox[{ RowBox[{"QuatToRot", "[", - RowBox[{"QuatInv", "[", "Q2", "]"}], "]"}], ".", "OmegaW"}]}], ";"}], + RowBox[{"QuatInv", "[", "Q1", "]"}], "]"}], ".", "VWprev"}]}], ";"}], "\[IndentingNewLine]", RowBox[{"(*", " ", RowBox[{ @@ -1065,8 +1106,10 @@ Cell[BoxData[{ 3.60843893145955*^9}, {3.608438979522884*^9, 3.608438981507514*^9}, { 3.6084395590417833`*^9, 3.608439568506847*^9}, 3.633669260171479*^9, { 3.633669335161825*^9, 3.63366933867423*^9}, {3.6336693690624857`*^9, - 3.633669380794092*^9}, {3.634452414675407*^9, 3.634452462890491*^9}}, - ExpressionUUID -> "471e0847-624c-413e-9a45-53b7ac630836"], + 3.633669380794092*^9}, {3.634452414675407*^9, 3.634452462890491*^9}, { + 3.832653600891818*^9, 3.832653628427477*^9}, 3.832841776748234*^9}, + CellLabel-> + "In[372]:=",ExpressionUUID->"471e0847-624c-413e-9a45-53b7ac630836"], Cell[BoxData[{ RowBox[{ @@ -1095,6 +1138,19 @@ Cell[BoxData[{ RowBox[{"{", RowBox[{"Dt12", ">", "0"}], "}"}]}], "]"}]}], "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"VOprev", "=", + RowBox[{"SimplifyQLite", "[", + RowBox[{"VOprevlong", ",", + RowBox[{"{", "Q1", "}"}], ",", + RowBox[{"Join", "[", + RowBox[{ + RowBox[{"AreReals", "[", + RowBox[{"{", + RowBox[{"POSE0", ",", "POSE1", ",", "Dt01"}], "}"}], "]"}], ",", + RowBox[{"{", + RowBox[{"Dt01", ">", "0"}], "}"}]}], "]"}]}], "]"}]}], + ";"}], "\[IndentingNewLine]", RowBox[{ RowBox[{"AO", "=", RowBox[{"SimplifyQLite", "[", @@ -1132,9 +1188,12 @@ Cell[BoxData[{ RowBox[{"Dt01", ">", "0"}]}], "}"}]}], "]"}]}], "]"}]}], ";"}]}], "Input", CellChangeTimes->{{3.6084388854743156`*^9, 3.6084389164072933`*^9}, { - 3.608438984538837*^9, 3.60843904612913*^9}, {3.608439574124473*^9, - 3.6084396107885313`*^9}, {3.6175129510513573`*^9, 3.6175130165735292`*^9}}, - ExpressionUUID -> "f44ac9b5-4f66-4593-b64a-dac5fee848ae"] + 3.608438984538837*^9, 3.60843904612913*^9}, {3.608439574124473*^9, + 3.6084396107885313`*^9}, {3.6175129510513573`*^9, + 3.6175130165735292`*^9}, {3.8326536370918093`*^9, 3.832653691539423*^9}, + 3.832841770093069*^9}, + CellLabel-> + "In[377]:=",ExpressionUUID->"f44ac9b5-4f66-4593-b64a-dac5fee848ae"] }, Open ]], Cell[CellGroupData[{ @@ -1147,7 +1206,8 @@ Cell[BoxData[{ CellChangeTimes->{{3.6084390475969543`*^9, 3.608439061724821*^9}, { 3.60843909186427*^9, 3.608439113432*^9}, {3.608439623784514*^9, 3.608439627977594*^9}}, - ExpressionUUID -> "0a6da266-07fb-4595-9bfa-54e81dfa376e"], + CellLabel-> + "In[382]:=",ExpressionUUID->"0a6da266-07fb-4595-9bfa-54e81dfa376e"], Cell[BoxData[ TagBox[ @@ -1287,16 +1347,14 @@ Cell[BoxData[ RowBox[{"x2", "[", "6", "]"}], " ", RowBox[{"x2", "[", "7", "]"}]}]}], ")"}]}]}], "Dt12"]} }, - GridBoxAlignment->{ - "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, - "RowsIndexed" -> {}}, + GridBoxAlignment->{"Columns" -> {{Center}}, "Rows" -> {{Baseline}}}, GridBoxSpacings->{"Columns" -> { Offset[0.27999999999999997`], { Offset[0.5599999999999999]}, - Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> { + Offset[0.27999999999999997`]}, "Rows" -> { Offset[0.2], { Offset[0.4]}, - Offset[0.2]}, "RowsIndexed" -> {}}], + Offset[0.2]}}], Column], "\[NoBreak]", ")"}], Function[BoxForm`e$, MatrixForm[BoxForm`e$]]]], "Output", @@ -1310,8 +1368,12 @@ Cell[BoxData[ 3.697261407909176*^9, 3.6972615984816628`*^9, 3.697263591188946*^9, 3.697263627987894*^9, 3.752577464840166*^9, 3.7528134449103947`*^9, 3.7624940447139874`*^9, 3.763877884663149*^9, 3.823863459000699*^9, - 3.82386363811887*^9, 3.8239331273219624`*^9}, - ExpressionUUID -> "dae735bc-2f48-4919-ba7e-b2138814ac48"], + 3.82386363811887*^9, 3.8239331273219624`*^9, 3.832654027342184*^9, + 3.8326543054149427`*^9, 3.832655256789167*^9, 3.832682642925626*^9, + 3.832841854913952*^9}, + CellLabel-> + "Out[382]//MatrixForm=",ExpressionUUID->"3c1d34a7-4c5d-4cd7-8048-\ +277376e7ede9"], Cell[BoxData[ TagBox[ @@ -1372,16 +1434,14 @@ Cell[BoxData[ RowBox[{"x1", "[", "4", "]"}], " ", RowBox[{"x2", "[", "7", "]"}]}]}], ")"}]}], "Dt12"]} }, - GridBoxAlignment->{ - "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, - "RowsIndexed" -> {}}, + GridBoxAlignment->{"Columns" -> {{Center}}, "Rows" -> {{Baseline}}}, GridBoxSpacings->{"Columns" -> { Offset[0.27999999999999997`], { Offset[0.5599999999999999]}, - Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> { + Offset[0.27999999999999997`]}, "Rows" -> { Offset[0.2], { Offset[0.4]}, - Offset[0.2]}, "RowsIndexed" -> {}}], + Offset[0.2]}}], Column], "\[NoBreak]", ")"}], Function[BoxForm`e$, MatrixForm[BoxForm`e$]]]], "Output", @@ -1395,8 +1455,12 @@ Cell[BoxData[ 3.697261407909176*^9, 3.6972615984816628`*^9, 3.697263591188946*^9, 3.697263627987894*^9, 3.752577464840166*^9, 3.7528134449103947`*^9, 3.7624940447139874`*^9, 3.763877884663149*^9, 3.823863459000699*^9, - 3.82386363811887*^9, 3.823933127322895*^9}, - ExpressionUUID -> "dae735bc-2f48-4919-ba7e-b2138814ac48"], + 3.82386363811887*^9, 3.8239331273219624`*^9, 3.832654027342184*^9, + 3.8326543054149427`*^9, 3.832655256789167*^9, 3.832682642925626*^9, + 3.8328418549165897`*^9}, + CellLabel-> + "Out[383]//MatrixForm=",ExpressionUUID->"0349e82c-3244-45ba-b424-\ +2f8f61462637"], Cell[BoxData[ TagBox[ @@ -1410,15 +1474,17 @@ Cell[BoxData[ RowBox[{"2", " ", RowBox[{"(", RowBox[{ - FractionBox[ - RowBox[{ - RowBox[{"x0", "[", "2", "]"}], "-", - RowBox[{"x1", "[", "2", "]"}]}], "Dt01"], "+", - FractionBox[ - RowBox[{ - RowBox[{"-", - RowBox[{"x1", "[", "2", "]"}]}], "+", - RowBox[{"x2", "[", "2", "]"}]}], "Dt12"]}], ")"}], " ", + RowBox[{"Dt12", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"x0", "[", "2", "]"}], "-", + RowBox[{"x1", "[", "2", "]"}]}], ")"}]}], "+", + RowBox[{"Dt01", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"-", + RowBox[{"x1", "[", "2", "]"}]}], "+", + RowBox[{"x2", "[", "2", "]"}]}], ")"}]}]}], ")"}], " ", RowBox[{"(", RowBox[{ RowBox[{ @@ -1426,54 +1492,55 @@ Cell[BoxData[ RowBox[{"x2", "[", "6", "]"}]}], "+", RowBox[{ RowBox[{"x2", "[", "4", "]"}], " ", - RowBox[{"x2", "[", "7", "]"}]}]}], ")"}]}], "+", + RowBox[{"x2", "[", "7", "]"}]}]}], ")"}]}], "-", RowBox[{"2", " ", RowBox[{"(", RowBox[{ - FractionBox[ - RowBox[{ - RowBox[{"x0", "[", "3", "]"}], "-", - RowBox[{"x1", "[", "3", "]"}]}], "Dt01"], "+", - FractionBox[ - RowBox[{ - RowBox[{"-", - RowBox[{"x1", "[", "3", "]"}]}], "+", - RowBox[{"x2", "[", "3", "]"}]}], "Dt12"]}], ")"}], " ", + RowBox[{"Dt12", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"x0", "[", "3", "]"}], "-", + RowBox[{"x1", "[", "3", "]"}]}], ")"}]}], "+", + RowBox[{"Dt01", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"-", + RowBox[{"x1", "[", "3", "]"}]}], "+", + RowBox[{"x2", "[", "3", "]"}]}], ")"}]}]}], ")"}], " ", RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", - RowBox[{"x2", "[", "4", "]"}]}], " ", - RowBox[{"x2", "[", "6", "]"}]}], "+", + RowBox[{"x2", "[", "4", "]"}], " ", + RowBox[{"x2", "[", "6", "]"}]}], "-", RowBox[{ RowBox[{"x2", "[", "5", "]"}], " ", - RowBox[{"x2", "[", "7", "]"}]}]}], ")"}]}], "+", - FractionBox[ - RowBox[{ - RowBox[{"(", - RowBox[{ - RowBox[{"Dt12", " ", - RowBox[{"(", - RowBox[{ - RowBox[{"-", - RowBox[{"x0", "[", "1", "]"}]}], "+", - RowBox[{"x1", "[", "1", "]"}]}], ")"}]}], "+", - RowBox[{"Dt01", " ", - RowBox[{"(", - RowBox[{ - RowBox[{"x1", "[", "1", "]"}], "-", - RowBox[{"x2", "[", "1", "]"}]}], ")"}]}]}], ")"}], " ", - RowBox[{"(", - RowBox[{ - RowBox[{"-", "1"}], "+", - RowBox[{"2", " ", - SuperscriptBox[ - RowBox[{"x2", "[", "6", "]"}], "2"]}], "+", - RowBox[{"2", " ", - SuperscriptBox[ - RowBox[{"x2", "[", "7", "]"}], "2"]}]}], ")"}]}], - RowBox[{"Dt01", " ", "Dt12"}]]}], ")"}]}], - RowBox[{"Dt01", "+", "Dt12"}]]}, + RowBox[{"x2", "[", "7", "]"}]}]}], ")"}]}], "-", + RowBox[{ + RowBox[{"(", + RowBox[{ + RowBox[{"Dt12", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"x0", "[", "1", "]"}], "-", + RowBox[{"x1", "[", "1", "]"}]}], ")"}]}], "+", + RowBox[{"Dt01", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"-", + RowBox[{"x1", "[", "1", "]"}]}], "+", + RowBox[{"x2", "[", "1", "]"}]}], ")"}]}]}], ")"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"-", "1"}], "+", + RowBox[{"2", " ", + SuperscriptBox[ + RowBox[{"x2", "[", "6", "]"}], "2"]}], "+", + RowBox[{"2", " ", + SuperscriptBox[ + RowBox[{"x2", "[", "7", "]"}], "2"]}]}], ")"}]}]}], ")"}]}], + RowBox[{"Dt01", " ", "Dt12", " ", + RowBox[{"(", + RowBox[{"Dt01", "+", "Dt12"}], ")"}]}]]}, { FractionBox[ RowBox[{"2", " ", @@ -1482,15 +1549,17 @@ Cell[BoxData[ RowBox[{"2", " ", RowBox[{"(", RowBox[{ - FractionBox[ - RowBox[{ - RowBox[{"x0", "[", "1", "]"}], "-", - RowBox[{"x1", "[", "1", "]"}]}], "Dt01"], "+", - FractionBox[ - RowBox[{ - RowBox[{"-", - RowBox[{"x1", "[", "1", "]"}]}], "+", - RowBox[{"x2", "[", "1", "]"}]}], "Dt12"]}], ")"}], " ", + RowBox[{"Dt12", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"x0", "[", "1", "]"}], "-", + RowBox[{"x1", "[", "1", "]"}]}], ")"}]}], "+", + RowBox[{"Dt01", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"-", + RowBox[{"x1", "[", "1", "]"}]}], "+", + RowBox[{"x2", "[", "1", "]"}]}], ")"}]}]}], ")"}], " ", RowBox[{"(", RowBox[{ RowBox[{ @@ -1502,15 +1571,17 @@ Cell[BoxData[ RowBox[{"2", " ", RowBox[{"(", RowBox[{ - FractionBox[ - RowBox[{ - RowBox[{"x0", "[", "3", "]"}], "-", - RowBox[{"x1", "[", "3", "]"}]}], "Dt01"], "+", - FractionBox[ - RowBox[{ - RowBox[{"-", - RowBox[{"x1", "[", "3", "]"}]}], "+", - RowBox[{"x2", "[", "3", "]"}]}], "Dt12"]}], ")"}], " ", + RowBox[{"Dt12", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"x0", "[", "3", "]"}], "-", + RowBox[{"x1", "[", "3", "]"}]}], ")"}]}], "+", + RowBox[{"Dt01", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"-", + RowBox[{"x1", "[", "3", "]"}]}], "+", + RowBox[{"x2", "[", "3", "]"}]}], ")"}]}]}], ")"}], " ", RowBox[{"(", RowBox[{ RowBox[{ @@ -1518,75 +1589,78 @@ Cell[BoxData[ RowBox[{"x2", "[", "5", "]"}]}], "+", RowBox[{ RowBox[{"x2", "[", "6", "]"}], " ", - RowBox[{"x2", "[", "7", "]"}]}]}], ")"}]}], "+", - FractionBox[ - RowBox[{ - RowBox[{"(", - RowBox[{ - RowBox[{"Dt12", " ", - RowBox[{"(", - RowBox[{ - RowBox[{"-", - RowBox[{"x0", "[", "2", "]"}]}], "+", - RowBox[{"x1", "[", "2", "]"}]}], ")"}]}], "+", - RowBox[{"Dt01", " ", - RowBox[{"(", - RowBox[{ - RowBox[{"x1", "[", "2", "]"}], "-", - RowBox[{"x2", "[", "2", "]"}]}], ")"}]}]}], ")"}], " ", - RowBox[{"(", - RowBox[{ - RowBox[{"-", "1"}], "+", - RowBox[{"2", " ", - SuperscriptBox[ - RowBox[{"x2", "[", "5", "]"}], "2"]}], "+", - RowBox[{"2", " ", - SuperscriptBox[ - RowBox[{"x2", "[", "7", "]"}], "2"]}]}], ")"}]}], - RowBox[{"Dt01", " ", "Dt12"}]]}], ")"}]}], - RowBox[{"Dt01", "+", "Dt12"}]]}, + RowBox[{"x2", "[", "7", "]"}]}]}], ")"}]}], "-", + RowBox[{ + RowBox[{"(", + RowBox[{ + RowBox[{"Dt12", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"x0", "[", "2", "]"}], "-", + RowBox[{"x1", "[", "2", "]"}]}], ")"}]}], "+", + RowBox[{"Dt01", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"-", + RowBox[{"x1", "[", "2", "]"}]}], "+", + RowBox[{"x2", "[", "2", "]"}]}], ")"}]}]}], ")"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"-", "1"}], "+", + RowBox[{"2", " ", + SuperscriptBox[ + RowBox[{"x2", "[", "5", "]"}], "2"]}], "+", + RowBox[{"2", " ", + SuperscriptBox[ + RowBox[{"x2", "[", "7", "]"}], "2"]}]}], ")"}]}]}], ")"}]}], + RowBox[{"Dt01", " ", "Dt12", " ", + RowBox[{"(", + RowBox[{"Dt01", "+", "Dt12"}], ")"}]}]]}, { FractionBox[ RowBox[{"2", " ", RowBox[{"(", RowBox[{ - FractionBox[ - RowBox[{ - RowBox[{"(", - RowBox[{ - RowBox[{"Dt12", " ", - RowBox[{"(", - RowBox[{ - RowBox[{"-", - RowBox[{"x0", "[", "3", "]"}]}], "+", - RowBox[{"x1", "[", "3", "]"}]}], ")"}]}], "+", - RowBox[{"Dt01", " ", - RowBox[{"(", - RowBox[{ - RowBox[{"x1", "[", "3", "]"}], "-", - RowBox[{"x2", "[", "3", "]"}]}], ")"}]}]}], ")"}], " ", - RowBox[{"(", - RowBox[{ - RowBox[{"-", "1"}], "+", - RowBox[{"2", " ", - SuperscriptBox[ - RowBox[{"x2", "[", "5", "]"}], "2"]}], "+", - RowBox[{"2", " ", - SuperscriptBox[ - RowBox[{"x2", "[", "6", "]"}], "2"]}]}], ")"}]}], - RowBox[{"Dt01", " ", "Dt12"}]], "+", - RowBox[{"2", " ", + RowBox[{"-", RowBox[{"(", RowBox[{ - FractionBox[ + RowBox[{"(", RowBox[{ - RowBox[{"x0", "[", "1", "]"}], "-", - RowBox[{"x1", "[", "1", "]"}]}], "Dt01"], "+", - FractionBox[ + RowBox[{"Dt12", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"x0", "[", "3", "]"}], "-", + RowBox[{"x1", "[", "3", "]"}]}], ")"}]}], "+", + RowBox[{"Dt01", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"-", + RowBox[{"x1", "[", "3", "]"}]}], "+", + RowBox[{"x2", "[", "3", "]"}]}], ")"}]}]}], ")"}], " ", + RowBox[{"(", RowBox[{ - RowBox[{"-", - RowBox[{"x1", "[", "1", "]"}]}], "+", - RowBox[{"x2", "[", "1", "]"}]}], "Dt12"]}], ")"}], " ", + RowBox[{"-", "1"}], "+", + RowBox[{"2", " ", + SuperscriptBox[ + RowBox[{"x2", "[", "5", "]"}], "2"]}], "+", + RowBox[{"2", " ", + SuperscriptBox[ + RowBox[{"x2", "[", "6", "]"}], "2"]}]}], ")"}]}], ")"}]}], + "+", + RowBox[{"2", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"Dt12", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"x0", "[", "1", "]"}], "-", + RowBox[{"x1", "[", "1", "]"}]}], ")"}]}], "+", + RowBox[{"Dt01", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"-", + RowBox[{"x1", "[", "1", "]"}]}], "+", + RowBox[{"x2", "[", "1", "]"}]}], ")"}]}]}], ")"}], " ", RowBox[{"(", RowBox[{ RowBox[{ @@ -1594,40 +1668,41 @@ Cell[BoxData[ RowBox[{"x2", "[", "6", "]"}]}], "+", RowBox[{ RowBox[{"x2", "[", "5", "]"}], " ", - RowBox[{"x2", "[", "7", "]"}]}]}], ")"}]}], "+", + RowBox[{"x2", "[", "7", "]"}]}]}], ")"}]}], "-", RowBox[{"2", " ", RowBox[{"(", RowBox[{ - FractionBox[ - RowBox[{ - RowBox[{"x0", "[", "2", "]"}], "-", - RowBox[{"x1", "[", "2", "]"}]}], "Dt01"], "+", - FractionBox[ - RowBox[{ - RowBox[{"-", - RowBox[{"x1", "[", "2", "]"}]}], "+", - RowBox[{"x2", "[", "2", "]"}]}], "Dt12"]}], ")"}], " ", + RowBox[{"Dt12", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"x0", "[", "2", "]"}], "-", + RowBox[{"x1", "[", "2", "]"}]}], ")"}]}], "+", + RowBox[{"Dt01", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"-", + RowBox[{"x1", "[", "2", "]"}]}], "+", + RowBox[{"x2", "[", "2", "]"}]}], ")"}]}]}], ")"}], " ", RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", - RowBox[{"x2", "[", "4", "]"}]}], " ", - RowBox[{"x2", "[", "5", "]"}]}], "+", + RowBox[{"x2", "[", "4", "]"}], " ", + RowBox[{"x2", "[", "5", "]"}]}], "-", RowBox[{ RowBox[{"x2", "[", "6", "]"}], " ", RowBox[{"x2", "[", "7", "]"}]}]}], ")"}]}]}], ")"}]}], - RowBox[{"Dt01", "+", "Dt12"}]]} - }, - GridBoxAlignment->{ - "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, - "RowsIndexed" -> {}}, + RowBox[{"Dt01", " ", "Dt12", " ", + RowBox[{"(", + RowBox[{"Dt01", "+", "Dt12"}], ")"}]}]]} + }, + GridBoxAlignment->{"Columns" -> {{Center}}, "Rows" -> {{Baseline}}}, GridBoxSpacings->{"Columns" -> { Offset[0.27999999999999997`], { Offset[0.5599999999999999]}, - Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> { + Offset[0.27999999999999997`]}, "Rows" -> { Offset[0.2], { Offset[0.4]}, - Offset[0.2]}, "RowsIndexed" -> {}}], + Offset[0.2]}}], Column], "\[NoBreak]", ")"}], Function[BoxForm`e$, MatrixForm[BoxForm`e$]]]], "Output", @@ -1641,8 +1716,12 @@ Cell[BoxData[ 3.697261407909176*^9, 3.6972615984816628`*^9, 3.697263591188946*^9, 3.697263627987894*^9, 3.752577464840166*^9, 3.7528134449103947`*^9, 3.7624940447139874`*^9, 3.763877884663149*^9, 3.823863459000699*^9, - 3.82386363811887*^9, 3.8239331273238783`*^9}, - ExpressionUUID -> "dae735bc-2f48-4919-ba7e-b2138814ac48"], + 3.82386363811887*^9, 3.8239331273219624`*^9, 3.832654027342184*^9, + 3.8326543054149427`*^9, 3.832655256789167*^9, 3.832682642925626*^9, + 3.832841854919306*^9}, + CellLabel-> + "Out[384]//MatrixForm=",ExpressionUUID->"84b5a5c0-0a05-4dbd-914c-\ +31d40e9a2a80"], Cell[BoxData[ TagBox[ @@ -1743,22 +1822,23 @@ Cell[BoxData[ RowBox[{"(", RowBox[{ RowBox[{"2", " ", + RowBox[{"x1", "[", "7", "]"}], " ", RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"x1", "[", "6", "]"}], " ", - RowBox[{"x2", "[", "4", "]"}], " ", - RowBox[{"x2", "[", "6", "]"}]}], "+", - RowBox[{ - RowBox[{"x1", "[", "7", "]"}], " ", RowBox[{"x2", "[", "5", "]"}], " ", RowBox[{"x2", "[", "6", "]"}]}], "+", RowBox[{ - RowBox[{"x1", "[", "7", "]"}], " ", RowBox[{"x2", "[", "4", "]"}], " ", - RowBox[{"x2", "[", "7", "]"}]}], "-", + RowBox[{"x2", "[", "7", "]"}]}]}], ")"}]}], "+", + RowBox[{"2", " ", + RowBox[{"x1", "[", "6", "]"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"x2", "[", "4", "]"}], " ", + RowBox[{"x2", "[", "6", "]"}]}], "-", RowBox[{ - RowBox[{"x1", "[", "6", "]"}], " ", RowBox[{"x2", "[", "5", "]"}], " ", RowBox[{"x2", "[", "7", "]"}]}]}], ")"}]}], "+", RowBox[{ @@ -1777,37 +1857,40 @@ Cell[BoxData[ RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "2"}], " ", + RowBox[{"x1", "[", "5", "]"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"-", "1"}], "+", + RowBox[{"2", " ", + SuperscriptBox[ + RowBox[{"x2", "[", "6", "]"}], "2"]}], "+", + RowBox[{"2", " ", + SuperscriptBox[ + RowBox[{"x2", "[", "7", "]"}], "2"]}]}], ")"}]}], "-", + RowBox[{"2", " ", RowBox[{"(", RowBox[{ - RowBox[{ - RowBox[{"-", - RowBox[{"x1", "[", "7", "]"}]}], " ", - RowBox[{"x2", "[", "4", "]"}], " ", - RowBox[{"x2", "[", "6", "]"}]}], "+", RowBox[{ RowBox[{"x1", "[", "6", "]"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{ RowBox[{"x2", "[", "5", "]"}], " ", RowBox[{"x2", "[", "6", "]"}]}], "+", RowBox[{ - RowBox[{"x1", "[", "6", "]"}], " ", RowBox[{"x2", "[", "4", "]"}], " ", - RowBox[{"x2", "[", "7", "]"}]}], "+", + RowBox[{"x2", "[", "7", "]"}]}]}], ")"}]}], "+", RowBox[{ RowBox[{"x1", "[", "7", "]"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"-", + RowBox[{"x2", "[", "4", "]"}]}], " ", + RowBox[{"x2", "[", "6", "]"}]}], "+", + RowBox[{ RowBox[{"x2", "[", "5", "]"}], " ", - RowBox[{"x2", "[", "7", "]"}]}]}], ")"}]}], "+", - RowBox[{ - RowBox[{"x1", "[", "5", "]"}], " ", - RowBox[{"(", - RowBox[{ - RowBox[{"-", "1"}], "+", - RowBox[{"2", " ", - SuperscriptBox[ - RowBox[{"x2", "[", "6", "]"}], "2"]}], "+", - RowBox[{"2", " ", - SuperscriptBox[ - RowBox[{"x2", "[", "7", "]"}], "2"]}]}], ")"}]}]}], + RowBox[{"x2", "[", "7", "]"}]}]}], ")"}]}]}], ")"}]}]}], ")"}]}]}], ")"}]}]}], ")"}]}], RowBox[{"Dt01", " ", "Dt12", " ", RowBox[{"(", @@ -1908,22 +1991,24 @@ Cell[BoxData[ RowBox[{"(", RowBox[{ RowBox[{"2", " ", + RowBox[{"x1", "[", "7", "]"}], " ", RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"x1", "[", "5", "]"}], " ", - RowBox[{"x2", "[", "4", "]"}], " ", - RowBox[{"x2", "[", "5", "]"}]}], "-", - RowBox[{ - RowBox[{"x1", "[", "7", "]"}], " ", - RowBox[{"x2", "[", "5", "]"}], " ", + RowBox[{"-", + RowBox[{"x2", "[", "5", "]"}]}], " ", RowBox[{"x2", "[", "6", "]"}]}], "+", RowBox[{ - RowBox[{"x1", "[", "7", "]"}], " ", RowBox[{"x2", "[", "4", "]"}], " ", - RowBox[{"x2", "[", "7", "]"}]}], "+", + RowBox[{"x2", "[", "7", "]"}]}]}], ")"}]}], "+", + RowBox[{"2", " ", + RowBox[{"x1", "[", "5", "]"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"x2", "[", "4", "]"}], " ", + RowBox[{"x2", "[", "5", "]"}]}], "+", RowBox[{ - RowBox[{"x1", "[", "5", "]"}], " ", RowBox[{"x2", "[", "6", "]"}], " ", RowBox[{"x2", "[", "7", "]"}]}]}], ")"}]}], "+", RowBox[{ @@ -1942,22 +2027,23 @@ Cell[BoxData[ RowBox[{"(", RowBox[{ RowBox[{"2", " ", + RowBox[{"x1", "[", "6", "]"}], " ", RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"x1", "[", "4", "]"}], " ", - RowBox[{"x2", "[", "4", "]"}], " ", - RowBox[{"x2", "[", "5", "]"}]}], "+", - RowBox[{ - RowBox[{"x1", "[", "6", "]"}], " ", RowBox[{"x2", "[", "5", "]"}], " ", RowBox[{"x2", "[", "6", "]"}]}], "-", RowBox[{ - RowBox[{"x1", "[", "6", "]"}], " ", RowBox[{"x2", "[", "4", "]"}], " ", - RowBox[{"x2", "[", "7", "]"}]}], "+", + RowBox[{"x2", "[", "7", "]"}]}]}], ")"}]}], "+", + RowBox[{"2", " ", + RowBox[{"x1", "[", "4", "]"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"x2", "[", "4", "]"}], " ", + RowBox[{"x2", "[", "5", "]"}]}], "+", RowBox[{ - RowBox[{"x1", "[", "4", "]"}], " ", RowBox[{"x2", "[", "6", "]"}], " ", RowBox[{"x2", "[", "7", "]"}]}]}], ")"}]}], "+", RowBox[{ @@ -2067,11 +2153,11 @@ Cell[BoxData[ RowBox[{"x2", "[", "6", "]"}], " ", RowBox[{"x2", "[", "7", "]"}]}], "+", RowBox[{ - RowBox[{"x0", "[", "6", "]"}], " ", + RowBox[{"x0", "[", "7", "]"}], " ", RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"x1", "[", "5", "]"}], " ", + RowBox[{"x1", "[", "4", "]"}], " ", RowBox[{"(", RowBox[{"1", "-", RowBox[{"2", " ", @@ -2079,33 +2165,34 @@ Cell[BoxData[ RowBox[{"x2", "[", "5", "]"}], "2"]}], "-", RowBox[{"2", " ", SuperscriptBox[ - RowBox[{"x2", "[", "6", "]"}], "2"]}]}], ")"}]}], "-", + RowBox[{"x2", "[", "6", "]"}], "2"]}]}], ")"}]}], "+", RowBox[{"2", " ", + RowBox[{"x1", "[", "6", "]"}], " ", RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"x1", "[", "4", "]"}], " ", - RowBox[{"x2", "[", "4", "]"}], " ", - RowBox[{"x2", "[", "5", "]"}]}], "+", - RowBox[{ - RowBox[{"x1", "[", "7", "]"}], " ", RowBox[{"x2", "[", "4", "]"}], " ", RowBox[{"x2", "[", "6", "]"}]}], "+", RowBox[{ - RowBox[{"x1", "[", "7", "]"}], " ", RowBox[{"x2", "[", "5", "]"}], " ", - RowBox[{"x2", "[", "7", "]"}]}], "-", + RowBox[{"x2", "[", "7", "]"}]}]}], ")"}]}], "+", + RowBox[{"2", " ", + RowBox[{"x1", "[", "5", "]"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"x2", "[", "4", "]"}], " ", + RowBox[{"x2", "[", "5", "]"}]}], "-", RowBox[{ - RowBox[{"x1", "[", "4", "]"}], " ", RowBox[{"x2", "[", "6", "]"}], " ", RowBox[{"x2", "[", "7", "]"}]}]}], ")"}]}]}], ")"}]}], "+", RowBox[{ - RowBox[{"x0", "[", "7", "]"}], " ", + RowBox[{"x0", "[", "6", "]"}], " ", RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"x1", "[", "4", "]"}], " ", + RowBox[{"x1", "[", "5", "]"}], " ", RowBox[{"(", RowBox[{"1", "-", RowBox[{"2", " ", @@ -2113,41 +2200,43 @@ Cell[BoxData[ RowBox[{"x2", "[", "5", "]"}], "2"]}], "-", RowBox[{"2", " ", SuperscriptBox[ - RowBox[{"x2", "[", "6", "]"}], "2"]}]}], ")"}]}], "+", + RowBox[{"x2", "[", "6", "]"}], "2"]}]}], ")"}]}], "-", RowBox[{"2", " ", RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"x1", "[", "5", "]"}], " ", - RowBox[{"x2", "[", "4", "]"}], " ", - RowBox[{"x2", "[", "5", "]"}]}], "+", + RowBox[{"x1", "[", "7", "]"}], " ", + RowBox[{"(", + RowBox[{ RowBox[{ - RowBox[{"x1", "[", "6", "]"}], " ", RowBox[{"x2", "[", "4", "]"}], " ", RowBox[{"x2", "[", "6", "]"}]}], "+", RowBox[{ - RowBox[{"x1", "[", "6", "]"}], " ", RowBox[{"x2", "[", "5", "]"}], " ", - RowBox[{"x2", "[", "7", "]"}]}], "-", + RowBox[{"x2", "[", "7", "]"}]}]}], ")"}]}], "+", + RowBox[{ + RowBox[{"x1", "[", "4", "]"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"x2", "[", "4", "]"}], " ", + RowBox[{"x2", "[", "5", "]"}]}], "-", RowBox[{ - RowBox[{"x1", "[", "5", "]"}], " ", RowBox[{"x2", "[", "6", "]"}], " ", RowBox[{"x2", "[", "7", "]"}]}]}], ")"}]}]}], ")"}]}]}], - ")"}]}]}], ")"}]}], + ")"}]}]}], ")"}]}]}], ")"}]}], RowBox[{"Dt01", " ", "Dt12", " ", RowBox[{"(", RowBox[{"Dt01", "+", "Dt12"}], ")"}]}]]} }, - GridBoxAlignment->{ - "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, - "RowsIndexed" -> {}}, + GridBoxAlignment->{"Columns" -> {{Center}}, "Rows" -> {{Baseline}}}, GridBoxSpacings->{"Columns" -> { Offset[0.27999999999999997`], { Offset[0.5599999999999999]}, - Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> { + Offset[0.27999999999999997`]}, "Rows" -> { Offset[0.2], { Offset[0.4]}, - Offset[0.2]}, "RowsIndexed" -> {}}], + Offset[0.2]}}], Column], "\[NoBreak]", ")"}], Function[BoxForm`e$, MatrixForm[BoxForm`e$]]]], "Output", @@ -2161,8 +2250,12 @@ Cell[BoxData[ 3.697261407909176*^9, 3.6972615984816628`*^9, 3.697263591188946*^9, 3.697263627987894*^9, 3.752577464840166*^9, 3.7528134449103947`*^9, 3.7624940447139874`*^9, 3.763877884663149*^9, 3.823863459000699*^9, - 3.82386363811887*^9, 3.823933127326055*^9}, - ExpressionUUID -> "dae735bc-2f48-4919-ba7e-b2138814ac48"] + 3.82386363811887*^9, 3.8239331273219624`*^9, 3.832654027342184*^9, + 3.8326543054149427`*^9, 3.832655256789167*^9, 3.832682642925626*^9, + 3.832841854923986*^9}, + CellLabel-> + "Out[385]//MatrixForm=",ExpressionUUID->"8baeb93f-a586-4dcd-902f-\ +79087ac00b31"] }, Open ]], Cell[BoxData[{ @@ -2214,15 +2307,40 @@ Cell[BoxData[{ RowBox[{ RowBox[{"QuatToRot", "[", RowBox[{"QuatInv", "[", "QOS", "]"}], "]"}], ".", "AlphaO"}]}], - ";"}]}], "Input", + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"QSprevhat", " ", "=", " ", + RowBox[{"QuatProd", "[", + RowBox[{"Q1", ",", "QOS"}], "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"VSprevhat", " ", "=", " ", + RowBox[{ + RowBox[{"QuatToRot", "[", + RowBox[{"QuatInv", "[", "QOS", "]"}], "]"}], ".", + RowBox[{"(", + RowBox[{"VOprev", "+", + RowBox[{"Cross", "[", + RowBox[{"WO", ",", "SO"}], "]"}]}], ")"}]}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"WSprevhat", " ", "=", " ", + RowBox[{ + RowBox[{"QuatToRot", "[", + RowBox[{"QuatInv", "[", + RowBox[{"QuatProd", "[", + RowBox[{"Q1", ",", "QOS"}], "]"}], "]"}], "]"}], ".", "OmegaWprev"}]}], + ";"}]}], "Input", CellChangeTimes->{{3.5740764224235477`*^9, 3.574076596848971*^9}, { 3.574076648844907*^9, 3.574076804422895*^9}, {3.581398654339856*^9, 3.5813986718598843`*^9}, {3.587366073779521*^9, 3.587366100913567*^9}, { 3.590473249925441*^9, 3.590473317128042*^9}, 3.590474059962389*^9, { 3.606027531795397*^9, 3.606027549937264*^9}, {3.608357474413703*^9, 3.608357485110188*^9}, {3.608358751877076*^9, 3.608358754764776*^9}, { - 3.6083587873295116`*^9, 3.6083588056207733`*^9}}, - ExpressionUUID -> "dc9c74e4-b7fd-479d-96ad-df3b425945ed"], + 3.6083587873295116`*^9, 3.6083588056207733`*^9}, {3.83284137482229*^9, + 3.832841464923024*^9}, {3.8328414969150763`*^9, 3.832841516754943*^9}, + 3.832841610116189*^9}, + CellLabel-> + "In[386]:=",ExpressionUUID->"dc9c74e4-b7fd-479d-96ad-df3b425945ed"], Cell[BoxData[{ RowBox[{ @@ -2260,15 +2378,17 @@ Cell[BoxData[{ RowBox[{ RowBox[{"Dt12", ">", "0"}], ",", RowBox[{"Dt01", ">", "0"}]}], "}"}]}], "]"}]}], "]"}]}], - ";"}]}], "Input", + ";"}], "\[IndentingNewLine]"}], "Input", CellChangeTimes->{{3.606028279837741*^9, 3.606028333718595*^9}, 3.606134590488863*^9, {3.606134646415031*^9, 3.606134657844077*^9}, { 3.608357704476698*^9, 3.608357706597742*^9}, {3.608357976545706*^9, 3.6083579841259317`*^9}, {3.608358432133675*^9, 3.608358441604765*^9}, { 3.60835889020942*^9, 3.608358898686665*^9}, {3.6084391448678007`*^9, 3.608439150176745*^9}, {3.608439785205418*^9, 3.608439792356657*^9}, { - 3.6175130392986794`*^9, 3.617513060807501*^9}}, - ExpressionUUID -> "af6ea6ed-8d84-4364-ad72-d9b03ce382e8"], + 3.6175130392986794`*^9, 3.617513060807501*^9}, {3.8326538358424*^9, + 3.832653867738306*^9}, 3.832653929586211*^9}, + CellLabel-> + "In[395]:=",ExpressionUUID->"af6ea6ed-8d84-4364-ad72-d9b03ce382e8"], Cell[BoxData[ RowBox[{"(*", @@ -2280,7 +2400,8 @@ Cell[BoxData[ 3.608357712743537*^9}, {3.6083579879636307`*^9, 3.608357989033553*^9}, { 3.608358443868833*^9, 3.608358446265375*^9}, {3.608439151397099*^9, 3.608439152520494*^9}, {3.608439794577024*^9, 3.608439797002676*^9}}, - ExpressionUUID -> "cbbf40a5-ffc1-449d-b645-dc4426abe1db"], + CellLabel-> + "In[397]:=",ExpressionUUID->"cbbf40a5-ffc1-449d-b645-dc4426abe1db"], Cell[CellGroupData[{ @@ -2288,7 +2409,8 @@ Cell[BoxData[{ RowBox[{"VShat", "//", "MatrixForm"}], "\[IndentingNewLine]", RowBox[{"AShat", "//", "MatrixForm"}]}], "Input", CellChangeTimes->{{3.617513143330982*^9, 3.61751315686552*^9}}, - ExpressionUUID -> "a4fa1226-db4f-4846-841d-81952fe05a97"], + CellLabel-> + "In[398]:=",ExpressionUUID->"a4fa1226-db4f-4846-841d-81952fe05a97"], Cell[BoxData[ TagBox[ @@ -2297,8 +2419,7 @@ Cell[BoxData[ { FractionBox[ RowBox[{ - RowBox[{ - RowBox[{"-", "2"}], " ", + RowBox[{"2", " ", RowBox[{"(", RowBox[{ RowBox[{ @@ -2309,291 +2430,253 @@ Cell[BoxData[ RowBox[{"qos", "[", "4", "]"}]}]}], ")"}], " ", RowBox[{"(", RowBox[{ - RowBox[{"-", - RowBox[{"x1", "[", "3", "]"}]}], "+", - RowBox[{"x2", "[", "3", "]"}], "-", RowBox[{"2", " ", RowBox[{"so", "[", "2", "]"}], " ", RowBox[{"x1", "[", "5", "]"}], " ", - RowBox[{"x2", "[", "4", "]"}]}], "+", + RowBox[{"x2", "[", "4", "]"}]}], "-", RowBox[{"2", " ", RowBox[{"so", "[", "1", "]"}], " ", RowBox[{"x1", "[", "6", "]"}], " ", - RowBox[{"x2", "[", "4", "]"}]}], "+", + RowBox[{"x2", "[", "4", "]"}]}], "-", RowBox[{"2", " ", RowBox[{"so", "[", "2", "]"}], " ", RowBox[{"x1", "[", "4", "]"}], " ", - RowBox[{"x2", "[", "5", "]"}]}], "+", + RowBox[{"x2", "[", "5", "]"}]}], "-", RowBox[{"2", " ", RowBox[{"so", "[", "1", "]"}], " ", RowBox[{"x1", "[", "7", "]"}], " ", RowBox[{"x2", "[", "5", "]"}]}], "+", - RowBox[{"2", " ", - RowBox[{"(", - RowBox[{ - RowBox[{"x1", "[", "2", "]"}], "-", - RowBox[{"x2", "[", "2", "]"}]}], ")"}], " ", - RowBox[{"x2", "[", "4", "]"}], " ", - RowBox[{"x2", "[", "5", "]"}]}], "+", - RowBox[{"2", " ", - RowBox[{"(", - RowBox[{ - RowBox[{"x1", "[", "3", "]"}], "-", - RowBox[{"x2", "[", "3", "]"}]}], ")"}], " ", - SuperscriptBox[ - RowBox[{"x2", "[", "5", "]"}], "2"]}], "-", RowBox[{"2", " ", RowBox[{"so", "[", "1", "]"}], " ", RowBox[{"x1", "[", "4", "]"}], " ", - RowBox[{"x2", "[", "6", "]"}]}], "+", + RowBox[{"x2", "[", "6", "]"}]}], "-", RowBox[{"2", " ", RowBox[{"so", "[", "2", "]"}], " ", RowBox[{"x1", "[", "7", "]"}], " ", - RowBox[{"x2", "[", "6", "]"}]}], "+", + RowBox[{"x2", "[", "6", "]"}]}], "-", RowBox[{ - RowBox[{"(", - RowBox[{ - RowBox[{ - RowBox[{"-", "2"}], " ", - RowBox[{"x1", "[", "1", "]"}]}], "+", - RowBox[{"2", " ", - RowBox[{"x2", "[", "1", "]"}]}]}], ")"}], " ", - RowBox[{"x2", "[", "4", "]"}], " ", - RowBox[{"x2", "[", "6", "]"}]}], "+", - RowBox[{"2", " ", RowBox[{"(", RowBox[{ RowBox[{"x1", "[", "3", "]"}], "-", RowBox[{"x2", "[", "3", "]"}]}], ")"}], " ", - SuperscriptBox[ - RowBox[{"x2", "[", "6", "]"}], "2"]}], "-", + RowBox[{"(", + RowBox[{ + RowBox[{"-", "1"}], "+", + RowBox[{"2", " ", + SuperscriptBox[ + RowBox[{"x2", "[", "5", "]"}], "2"]}], "+", + RowBox[{"2", " ", + SuperscriptBox[ + RowBox[{"x2", "[", "6", "]"}], "2"]}]}], ")"}]}], "+", RowBox[{"2", " ", RowBox[{"so", "[", "1", "]"}], " ", RowBox[{"x1", "[", "5", "]"}], " ", - RowBox[{"x2", "[", "7", "]"}]}], "-", + RowBox[{"x2", "[", "7", "]"}]}], "+", RowBox[{"2", " ", RowBox[{"so", "[", "2", "]"}], " ", RowBox[{"x1", "[", "6", "]"}], " ", RowBox[{"x2", "[", "7", "]"}]}], "+", - RowBox[{ + RowBox[{"2", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"x1", "[", "1", "]"}], "-", + RowBox[{"x2", "[", "1", "]"}]}], ")"}], " ", RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "2"}], " ", - RowBox[{"x1", "[", "1", "]"}]}], "+", - RowBox[{"2", " ", - RowBox[{"x2", "[", "1", "]"}]}]}], ")"}], " ", - RowBox[{"x2", "[", "5", "]"}], " ", - RowBox[{"x2", "[", "7", "]"}]}], "+", - RowBox[{ + RowBox[{"x2", "[", "4", "]"}], " ", + RowBox[{"x2", "[", "6", "]"}]}], "+", + RowBox[{ + RowBox[{"x2", "[", "5", "]"}], " ", + RowBox[{"x2", "[", "7", "]"}]}]}], ")"}]}], "-", + RowBox[{"2", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"x1", "[", "2", "]"}], "-", + RowBox[{"x2", "[", "2", "]"}]}], ")"}], " ", RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "2"}], " ", - RowBox[{"x1", "[", "2", "]"}]}], "+", - RowBox[{"2", " ", - RowBox[{"x2", "[", "2", "]"}]}]}], ")"}], " ", - RowBox[{"x2", "[", "6", "]"}], " ", - RowBox[{"x2", "[", "7", "]"}]}]}], ")"}]}], "-", - RowBox[{ + RowBox[{"x2", "[", "4", "]"}], " ", + RowBox[{"x2", "[", "5", "]"}]}], "-", + RowBox[{ + RowBox[{"x2", "[", "6", "]"}], " ", + RowBox[{"x2", "[", "7", "]"}]}]}], ")"}]}]}], ")"}]}], "+", + RowBox[{"2", " ", RowBox[{"(", RowBox[{ - RowBox[{"-", "1"}], "+", - RowBox[{"2", " ", - SuperscriptBox[ - RowBox[{"qos", "[", "3", "]"}], "2"]}], "+", - RowBox[{"2", " ", - SuperscriptBox[ - RowBox[{"qos", "[", "4", "]"}], "2"]}]}], ")"}], " ", + RowBox[{ + RowBox[{"qos", "[", "2", "]"}], " ", + RowBox[{"qos", "[", "3", "]"}]}], "+", + RowBox[{ + RowBox[{"qos", "[", "1", "]"}], " ", + RowBox[{"qos", "[", "4", "]"}]}]}], ")"}], " ", RowBox[{"(", RowBox[{ - RowBox[{"-", - RowBox[{"x1", "[", "1", "]"}]}], "+", - RowBox[{"x2", "[", "1", "]"}], "-", RowBox[{"2", " ", RowBox[{"so", "[", "3", "]"}], " ", - RowBox[{"x1", "[", "6", "]"}], " ", - RowBox[{"x2", "[", "4", "]"}]}], "+", + RowBox[{"x1", "[", "5", "]"}], " ", + RowBox[{"x2", "[", "4", "]"}]}], "-", RowBox[{"2", " ", - RowBox[{"so", "[", "2", "]"}], " ", + RowBox[{"so", "[", "1", "]"}], " ", RowBox[{"x1", "[", "7", "]"}], " ", RowBox[{"x2", "[", "4", "]"}]}], "-", - RowBox[{"2", " ", - RowBox[{"so", "[", "2", "]"}], " ", - RowBox[{"x1", "[", "6", "]"}], " ", - RowBox[{"x2", "[", "5", "]"}]}], "-", RowBox[{"2", " ", RowBox[{"so", "[", "3", "]"}], " ", - RowBox[{"x1", "[", "7", "]"}], " ", + RowBox[{"x1", "[", "4", "]"}], " ", RowBox[{"x2", "[", "5", "]"}]}], "+", RowBox[{"2", " ", - RowBox[{"so", "[", "3", "]"}], " ", - RowBox[{"x1", "[", "4", "]"}], " ", - RowBox[{"x2", "[", "6", "]"}]}], "+", + RowBox[{"so", "[", "1", "]"}], " ", + RowBox[{"x1", "[", "6", "]"}], " ", + RowBox[{"x2", "[", "5", "]"}]}], "-", RowBox[{"2", " ", - RowBox[{"so", "[", "2", "]"}], " ", + RowBox[{"so", "[", "1", "]"}], " ", RowBox[{"x1", "[", "5", "]"}], " ", + RowBox[{"x2", "[", "6", "]"}]}], "-", + RowBox[{"2", " ", + RowBox[{"so", "[", "3", "]"}], " ", + RowBox[{"x1", "[", "7", "]"}], " ", RowBox[{"x2", "[", "6", "]"}]}], "+", + RowBox[{"2", " ", + RowBox[{"so", "[", "1", "]"}], " ", + RowBox[{"x1", "[", "4", "]"}], " ", + RowBox[{"x2", "[", "7", "]"}]}], "+", + RowBox[{"2", " ", + RowBox[{"so", "[", "3", "]"}], " ", + RowBox[{"x1", "[", "6", "]"}], " ", + RowBox[{"x2", "[", "7", "]"}]}], "+", RowBox[{"2", " ", RowBox[{"(", RowBox[{ - RowBox[{"x1", "[", "3", "]"}], "-", - RowBox[{"x2", "[", "3", "]"}]}], ")"}], " ", - RowBox[{"x2", "[", "4", "]"}], " ", - RowBox[{"x2", "[", "6", "]"}]}], "+", - RowBox[{ + RowBox[{"-", + RowBox[{"x1", "[", "1", "]"}]}], "+", + RowBox[{"x2", "[", "1", "]"}]}], ")"}], " ", RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "2"}], " ", - RowBox[{"x1", "[", "2", "]"}]}], "+", - RowBox[{"2", " ", - RowBox[{"x2", "[", "2", "]"}]}]}], ")"}], " ", - RowBox[{"x2", "[", "5", "]"}], " ", - RowBox[{"x2", "[", "6", "]"}]}], "+", + RowBox[{"x2", "[", "5", "]"}], " ", + RowBox[{"x2", "[", "6", "]"}]}], "-", + RowBox[{ + RowBox[{"x2", "[", "4", "]"}], " ", + RowBox[{"x2", "[", "7", "]"}]}]}], ")"}]}], "+", RowBox[{"2", " ", RowBox[{"(", RowBox[{ - RowBox[{"x1", "[", "1", "]"}], "-", - RowBox[{"x2", "[", "1", "]"}]}], ")"}], " ", - SuperscriptBox[ - RowBox[{"x2", "[", "6", "]"}], "2"]}], "-", - RowBox[{"2", " ", - RowBox[{"so", "[", "2", "]"}], " ", - RowBox[{"x1", "[", "4", "]"}], " ", - RowBox[{"x2", "[", "7", "]"}]}], "+", - RowBox[{"2", " ", - RowBox[{"so", "[", "3", "]"}], " ", - RowBox[{"x1", "[", "5", "]"}], " ", - RowBox[{"x2", "[", "7", "]"}]}], "+", - RowBox[{ + RowBox[{"-", + RowBox[{"x1", "[", "3", "]"}]}], "+", + RowBox[{"x2", "[", "3", "]"}]}], ")"}], " ", RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "2"}], " ", - RowBox[{"x1", "[", "2", "]"}]}], "+", - RowBox[{"2", " ", - RowBox[{"x2", "[", "2", "]"}]}]}], ")"}], " ", - RowBox[{"x2", "[", "4", "]"}], " ", - RowBox[{"x2", "[", "7", "]"}]}], "+", + RowBox[{"x2", "[", "4", "]"}], " ", + RowBox[{"x2", "[", "5", "]"}]}], "+", + RowBox[{ + RowBox[{"x2", "[", "6", "]"}], " ", + RowBox[{"x2", "[", "7", "]"}]}]}], ")"}]}], "+", RowBox[{ RowBox[{"(", RowBox[{ - RowBox[{ - RowBox[{"-", "2"}], " ", - RowBox[{"x1", "[", "3", "]"}]}], "+", - RowBox[{"2", " ", - RowBox[{"x2", "[", "3", "]"}]}]}], ")"}], " ", - RowBox[{"x2", "[", "5", "]"}], " ", - RowBox[{"x2", "[", "7", "]"}]}], "+", - RowBox[{"2", " ", + RowBox[{"x1", "[", "2", "]"}], "-", + RowBox[{"x2", "[", "2", "]"}]}], ")"}], " ", RowBox[{"(", RowBox[{ - RowBox[{"x1", "[", "1", "]"}], "-", - RowBox[{"x2", "[", "1", "]"}]}], ")"}], " ", - SuperscriptBox[ - RowBox[{"x2", "[", "7", "]"}], "2"]}]}], ")"}]}], "+", - RowBox[{"2", " ", + RowBox[{"-", "1"}], "+", + RowBox[{"2", " ", + SuperscriptBox[ + RowBox[{"x2", "[", "5", "]"}], "2"]}], "+", + RowBox[{"2", " ", + SuperscriptBox[ + RowBox[{"x2", "[", "7", "]"}], "2"]}]}], ")"}]}]}], ")"}]}], + "-", + RowBox[{ RowBox[{"(", - RowBox[{ - RowBox[{ - RowBox[{"qos", "[", "2", "]"}], " ", - RowBox[{"qos", "[", "3", "]"}]}], "+", - RowBox[{ - RowBox[{"qos", "[", "1", "]"}], " ", - RowBox[{"qos", "[", "4", "]"}]}]}], ")"}], " ", + RowBox[{"1", "-", + RowBox[{"2", " ", + SuperscriptBox[ + RowBox[{"qos", "[", "3", "]"}], "2"]}], "-", + RowBox[{"2", " ", + SuperscriptBox[ + RowBox[{"qos", "[", "4", "]"}], "2"]}]}], ")"}], " ", RowBox[{"(", RowBox[{ - RowBox[{"-", - RowBox[{"x1", "[", "2", "]"}]}], "+", - RowBox[{"x2", "[", "2", "]"}], "+", RowBox[{"2", " ", RowBox[{"so", "[", "3", "]"}], " ", - RowBox[{"x1", "[", "5", "]"}], " ", + RowBox[{"x1", "[", "6", "]"}], " ", RowBox[{"x2", "[", "4", "]"}]}], "-", RowBox[{"2", " ", - RowBox[{"so", "[", "1", "]"}], " ", + RowBox[{"so", "[", "2", "]"}], " ", RowBox[{"x1", "[", "7", "]"}], " ", - RowBox[{"x2", "[", "4", "]"}]}], "-", - RowBox[{"2", " ", - RowBox[{"so", "[", "3", "]"}], " ", - RowBox[{"x1", "[", "4", "]"}], " ", - RowBox[{"x2", "[", "5", "]"}]}], "+", + RowBox[{"x2", "[", "4", "]"}]}], "+", RowBox[{"2", " ", - RowBox[{"so", "[", "1", "]"}], " ", + RowBox[{"so", "[", "2", "]"}], " ", RowBox[{"x1", "[", "6", "]"}], " ", RowBox[{"x2", "[", "5", "]"}]}], "+", - RowBox[{ - RowBox[{"(", - RowBox[{ - RowBox[{ - RowBox[{"-", "2"}], " ", - RowBox[{"x1", "[", "3", "]"}]}], "+", - RowBox[{"2", " ", - RowBox[{"x2", "[", "3", "]"}]}]}], ")"}], " ", - RowBox[{"x2", "[", "4", "]"}], " ", - RowBox[{"x2", "[", "5", "]"}]}], "+", RowBox[{"2", " ", - RowBox[{"(", - RowBox[{ - RowBox[{"x1", "[", "2", "]"}], "-", - RowBox[{"x2", "[", "2", "]"}]}], ")"}], " ", - SuperscriptBox[ - RowBox[{"x2", "[", "5", "]"}], "2"]}], "-", + RowBox[{"so", "[", "3", "]"}], " ", + RowBox[{"x1", "[", "7", "]"}], " ", + RowBox[{"x2", "[", "5", "]"}]}], "-", RowBox[{"2", " ", - RowBox[{"so", "[", "1", "]"}], " ", - RowBox[{"x1", "[", "5", "]"}], " ", + RowBox[{"so", "[", "3", "]"}], " ", + RowBox[{"x1", "[", "4", "]"}], " ", RowBox[{"x2", "[", "6", "]"}]}], "-", RowBox[{"2", " ", - RowBox[{"so", "[", "3", "]"}], " ", - RowBox[{"x1", "[", "7", "]"}], " ", - RowBox[{"x2", "[", "6", "]"}]}], "+", - RowBox[{ - RowBox[{"(", - RowBox[{ - RowBox[{ - RowBox[{"-", "2"}], " ", - RowBox[{"x1", "[", "1", "]"}]}], "+", - RowBox[{"2", " ", - RowBox[{"x2", "[", "1", "]"}]}]}], ")"}], " ", - RowBox[{"x2", "[", "5", "]"}], " ", + RowBox[{"so", "[", "2", "]"}], " ", + RowBox[{"x1", "[", "5", "]"}], " ", RowBox[{"x2", "[", "6", "]"}]}], "+", RowBox[{"2", " ", - RowBox[{"so", "[", "1", "]"}], " ", + RowBox[{"so", "[", "2", "]"}], " ", RowBox[{"x1", "[", "4", "]"}], " ", - RowBox[{"x2", "[", "7", "]"}]}], "+", + RowBox[{"x2", "[", "7", "]"}]}], "-", RowBox[{"2", " ", RowBox[{"so", "[", "3", "]"}], " ", - RowBox[{"x1", "[", "6", "]"}], " ", + RowBox[{"x1", "[", "5", "]"}], " ", RowBox[{"x2", "[", "7", "]"}]}], "+", RowBox[{"2", " ", RowBox[{"(", RowBox[{ - RowBox[{"x1", "[", "1", "]"}], "-", - RowBox[{"x2", "[", "1", "]"}]}], ")"}], " ", - RowBox[{"x2", "[", "4", "]"}], " ", - RowBox[{"x2", "[", "7", "]"}]}], "+", - RowBox[{ + RowBox[{"x1", "[", "2", "]"}], "-", + RowBox[{"x2", "[", "2", "]"}]}], ")"}], " ", RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "2"}], " ", - RowBox[{"x1", "[", "3", "]"}]}], "+", - RowBox[{"2", " ", - RowBox[{"x2", "[", "3", "]"}]}]}], ")"}], " ", - RowBox[{"x2", "[", "6", "]"}], " ", - RowBox[{"x2", "[", "7", "]"}]}], "+", + RowBox[{"x2", "[", "5", "]"}], " ", + RowBox[{"x2", "[", "6", "]"}]}], "+", + RowBox[{ + RowBox[{"x2", "[", "4", "]"}], " ", + RowBox[{"x2", "[", "7", "]"}]}]}], ")"}]}], "-", RowBox[{"2", " ", RowBox[{"(", RowBox[{ - RowBox[{"x1", "[", "2", "]"}], "-", - RowBox[{"x2", "[", "2", "]"}]}], ")"}], " ", - SuperscriptBox[ - RowBox[{"x2", "[", "7", "]"}], "2"]}]}], ")"}]}]}], "Dt12"]}, + RowBox[{"x1", "[", "3", "]"}], "-", + RowBox[{"x2", "[", "3", "]"}]}], ")"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"x2", "[", "4", "]"}], " ", + RowBox[{"x2", "[", "6", "]"}]}], "-", + RowBox[{ + RowBox[{"x2", "[", "5", "]"}], " ", + RowBox[{"x2", "[", "7", "]"}]}]}], ")"}]}], "-", + RowBox[{ + RowBox[{"(", + RowBox[{ + RowBox[{"x1", "[", "1", "]"}], "-", + RowBox[{"x2", "[", "1", "]"}]}], ")"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"-", "1"}], "+", + RowBox[{"2", " ", + SuperscriptBox[ + RowBox[{"x2", "[", "6", "]"}], "2"]}], "+", + RowBox[{"2", " ", + SuperscriptBox[ + RowBox[{"x2", "[", "7", "]"}], "2"]}]}], ")"}]}]}], + ")"}]}]}], "Dt12"]}, { FractionBox[ RowBox[{ - RowBox[{"2", " ", + RowBox[{ + RowBox[{"-", "2"}], " ", RowBox[{"(", RowBox[{ RowBox[{ @@ -2604,204 +2687,89 @@ Cell[BoxData[ RowBox[{"qos", "[", "4", "]"}]}]}], ")"}], " ", RowBox[{"(", RowBox[{ - RowBox[{"-", - RowBox[{"x1", "[", "3", "]"}]}], "+", - RowBox[{"x2", "[", "3", "]"}], "-", RowBox[{"2", " ", RowBox[{"so", "[", "2", "]"}], " ", RowBox[{"x1", "[", "5", "]"}], " ", - RowBox[{"x2", "[", "4", "]"}]}], "+", + RowBox[{"x2", "[", "4", "]"}]}], "-", RowBox[{"2", " ", RowBox[{"so", "[", "1", "]"}], " ", RowBox[{"x1", "[", "6", "]"}], " ", - RowBox[{"x2", "[", "4", "]"}]}], "+", + RowBox[{"x2", "[", "4", "]"}]}], "-", RowBox[{"2", " ", RowBox[{"so", "[", "2", "]"}], " ", RowBox[{"x1", "[", "4", "]"}], " ", - RowBox[{"x2", "[", "5", "]"}]}], "+", + RowBox[{"x2", "[", "5", "]"}]}], "-", RowBox[{"2", " ", RowBox[{"so", "[", "1", "]"}], " ", RowBox[{"x1", "[", "7", "]"}], " ", RowBox[{"x2", "[", "5", "]"}]}], "+", - RowBox[{"2", " ", - RowBox[{"(", - RowBox[{ - RowBox[{"x1", "[", "2", "]"}], "-", - RowBox[{"x2", "[", "2", "]"}]}], ")"}], " ", - RowBox[{"x2", "[", "4", "]"}], " ", - RowBox[{"x2", "[", "5", "]"}]}], "+", - RowBox[{"2", " ", - RowBox[{"(", - RowBox[{ - RowBox[{"x1", "[", "3", "]"}], "-", - RowBox[{"x2", "[", "3", "]"}]}], ")"}], " ", - SuperscriptBox[ - RowBox[{"x2", "[", "5", "]"}], "2"]}], "-", RowBox[{"2", " ", RowBox[{"so", "[", "1", "]"}], " ", RowBox[{"x1", "[", "4", "]"}], " ", - RowBox[{"x2", "[", "6", "]"}]}], "+", + RowBox[{"x2", "[", "6", "]"}]}], "-", RowBox[{"2", " ", RowBox[{"so", "[", "2", "]"}], " ", RowBox[{"x1", "[", "7", "]"}], " ", - RowBox[{"x2", "[", "6", "]"}]}], "+", + RowBox[{"x2", "[", "6", "]"}]}], "-", RowBox[{ - RowBox[{"(", - RowBox[{ - RowBox[{ - RowBox[{"-", "2"}], " ", - RowBox[{"x1", "[", "1", "]"}]}], "+", - RowBox[{"2", " ", - RowBox[{"x2", "[", "1", "]"}]}]}], ")"}], " ", - RowBox[{"x2", "[", "4", "]"}], " ", - RowBox[{"x2", "[", "6", "]"}]}], "+", - RowBox[{"2", " ", RowBox[{"(", RowBox[{ RowBox[{"x1", "[", "3", "]"}], "-", RowBox[{"x2", "[", "3", "]"}]}], ")"}], " ", - SuperscriptBox[ - RowBox[{"x2", "[", "6", "]"}], "2"]}], "-", + RowBox[{"(", + RowBox[{ + RowBox[{"-", "1"}], "+", + RowBox[{"2", " ", + SuperscriptBox[ + RowBox[{"x2", "[", "5", "]"}], "2"]}], "+", + RowBox[{"2", " ", + SuperscriptBox[ + RowBox[{"x2", "[", "6", "]"}], "2"]}]}], ")"}]}], "+", RowBox[{"2", " ", RowBox[{"so", "[", "1", "]"}], " ", RowBox[{"x1", "[", "5", "]"}], " ", - RowBox[{"x2", "[", "7", "]"}]}], "-", + RowBox[{"x2", "[", "7", "]"}]}], "+", RowBox[{"2", " ", RowBox[{"so", "[", "2", "]"}], " ", RowBox[{"x1", "[", "6", "]"}], " ", RowBox[{"x2", "[", "7", "]"}]}], "+", - RowBox[{ + RowBox[{"2", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"x1", "[", "1", "]"}], "-", + RowBox[{"x2", "[", "1", "]"}]}], ")"}], " ", RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "2"}], " ", - RowBox[{"x1", "[", "1", "]"}]}], "+", - RowBox[{"2", " ", - RowBox[{"x2", "[", "1", "]"}]}]}], ")"}], " ", - RowBox[{"x2", "[", "5", "]"}], " ", - RowBox[{"x2", "[", "7", "]"}]}], "+", - RowBox[{ + RowBox[{"x2", "[", "4", "]"}], " ", + RowBox[{"x2", "[", "6", "]"}]}], "+", + RowBox[{ + RowBox[{"x2", "[", "5", "]"}], " ", + RowBox[{"x2", "[", "7", "]"}]}]}], ")"}]}], "-", + RowBox[{"2", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"x1", "[", "2", "]"}], "-", + RowBox[{"x2", "[", "2", "]"}]}], ")"}], " ", RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "2"}], " ", - RowBox[{"x1", "[", "2", "]"}]}], "+", - RowBox[{"2", " ", - RowBox[{"x2", "[", "2", "]"}]}]}], ")"}], " ", - RowBox[{"x2", "[", "6", "]"}], " ", - RowBox[{"x2", "[", "7", "]"}]}]}], ")"}]}], "+", - RowBox[{"2", " ", + RowBox[{"x2", "[", "4", "]"}], " ", + RowBox[{"x2", "[", "5", "]"}]}], "-", + RowBox[{ + RowBox[{"x2", "[", "6", "]"}], " ", + RowBox[{"x2", "[", "7", "]"}]}]}], ")"}]}]}], ")"}]}], "+", + RowBox[{ RowBox[{"(", - RowBox[{ - RowBox[{ - RowBox[{"qos", "[", "2", "]"}], " ", - RowBox[{"qos", "[", "3", "]"}]}], "-", - RowBox[{ - RowBox[{"qos", "[", "1", "]"}], " ", - RowBox[{"qos", "[", "4", "]"}]}]}], ")"}], " ", + RowBox[{"1", "-", + RowBox[{"2", " ", + SuperscriptBox[ + RowBox[{"qos", "[", "2", "]"}], "2"]}], "-", + RowBox[{"2", " ", + SuperscriptBox[ + RowBox[{"qos", "[", "4", "]"}], "2"]}]}], ")"}], " ", RowBox[{"(", RowBox[{ - RowBox[{"-", - RowBox[{"x1", "[", "1", "]"}]}], "+", - RowBox[{"x2", "[", "1", "]"}], "-", - RowBox[{"2", " ", - RowBox[{"so", "[", "3", "]"}], " ", - RowBox[{"x1", "[", "6", "]"}], " ", - RowBox[{"x2", "[", "4", "]"}]}], "+", - RowBox[{"2", " ", - RowBox[{"so", "[", "2", "]"}], " ", - RowBox[{"x1", "[", "7", "]"}], " ", - RowBox[{"x2", "[", "4", "]"}]}], "-", - RowBox[{"2", " ", - RowBox[{"so", "[", "2", "]"}], " ", - RowBox[{"x1", "[", "6", "]"}], " ", - RowBox[{"x2", "[", "5", "]"}]}], "-", - RowBox[{"2", " ", - RowBox[{"so", "[", "3", "]"}], " ", - RowBox[{"x1", "[", "7", "]"}], " ", - RowBox[{"x2", "[", "5", "]"}]}], "+", - RowBox[{"2", " ", - RowBox[{"so", "[", "3", "]"}], " ", - RowBox[{"x1", "[", "4", "]"}], " ", - RowBox[{"x2", "[", "6", "]"}]}], "+", - RowBox[{"2", " ", - RowBox[{"so", "[", "2", "]"}], " ", - RowBox[{"x1", "[", "5", "]"}], " ", - RowBox[{"x2", "[", "6", "]"}]}], "+", - RowBox[{"2", " ", - RowBox[{"(", - RowBox[{ - RowBox[{"x1", "[", "3", "]"}], "-", - RowBox[{"x2", "[", "3", "]"}]}], ")"}], " ", - RowBox[{"x2", "[", "4", "]"}], " ", - RowBox[{"x2", "[", "6", "]"}]}], "+", - RowBox[{ - RowBox[{"(", - RowBox[{ - RowBox[{ - RowBox[{"-", "2"}], " ", - RowBox[{"x1", "[", "2", "]"}]}], "+", - RowBox[{"2", " ", - RowBox[{"x2", "[", "2", "]"}]}]}], ")"}], " ", - RowBox[{"x2", "[", "5", "]"}], " ", - RowBox[{"x2", "[", "6", "]"}]}], "+", - RowBox[{"2", " ", - RowBox[{"(", - RowBox[{ - RowBox[{"x1", "[", "1", "]"}], "-", - RowBox[{"x2", "[", "1", "]"}]}], ")"}], " ", - SuperscriptBox[ - RowBox[{"x2", "[", "6", "]"}], "2"]}], "-", - RowBox[{"2", " ", - RowBox[{"so", "[", "2", "]"}], " ", - RowBox[{"x1", "[", "4", "]"}], " ", - RowBox[{"x2", "[", "7", "]"}]}], "+", - RowBox[{"2", " ", - RowBox[{"so", "[", "3", "]"}], " ", - RowBox[{"x1", "[", "5", "]"}], " ", - RowBox[{"x2", "[", "7", "]"}]}], "+", - RowBox[{ - RowBox[{"(", - RowBox[{ - RowBox[{ - RowBox[{"-", "2"}], " ", - RowBox[{"x1", "[", "2", "]"}]}], "+", - RowBox[{"2", " ", - RowBox[{"x2", "[", "2", "]"}]}]}], ")"}], " ", - RowBox[{"x2", "[", "4", "]"}], " ", - RowBox[{"x2", "[", "7", "]"}]}], "+", - RowBox[{ - RowBox[{"(", - RowBox[{ - RowBox[{ - RowBox[{"-", "2"}], " ", - RowBox[{"x1", "[", "3", "]"}]}], "+", - RowBox[{"2", " ", - RowBox[{"x2", "[", "3", "]"}]}]}], ")"}], " ", - RowBox[{"x2", "[", "5", "]"}], " ", - RowBox[{"x2", "[", "7", "]"}]}], "+", - RowBox[{"2", " ", - RowBox[{"(", - RowBox[{ - RowBox[{"x1", "[", "1", "]"}], "-", - RowBox[{"x2", "[", "1", "]"}]}], ")"}], " ", - SuperscriptBox[ - RowBox[{"x2", "[", "7", "]"}], "2"]}]}], ")"}]}], "-", - RowBox[{ - RowBox[{"(", - RowBox[{ - RowBox[{"-", "1"}], "+", - RowBox[{"2", " ", - SuperscriptBox[ - RowBox[{"qos", "[", "2", "]"}], "2"]}], "+", - RowBox[{"2", " ", - SuperscriptBox[ - RowBox[{"qos", "[", "4", "]"}], "2"]}]}], ")"}], " ", - RowBox[{"(", - RowBox[{ - RowBox[{"-", - RowBox[{"x1", "[", "2", "]"}]}], "+", - RowBox[{"x2", "[", "2", "]"}], "+", RowBox[{"2", " ", RowBox[{"so", "[", "3", "]"}], " ", RowBox[{"x1", "[", "5", "]"}], " ", @@ -2817,24 +2785,7 @@ Cell[BoxData[ RowBox[{"2", " ", RowBox[{"so", "[", "1", "]"}], " ", RowBox[{"x1", "[", "6", "]"}], " ", - RowBox[{"x2", "[", "5", "]"}]}], "+", - RowBox[{ - RowBox[{"(", - RowBox[{ - RowBox[{ - RowBox[{"-", "2"}], " ", - RowBox[{"x1", "[", "3", "]"}]}], "+", - RowBox[{"2", " ", - RowBox[{"x2", "[", "3", "]"}]}]}], ")"}], " ", - RowBox[{"x2", "[", "4", "]"}], " ", - RowBox[{"x2", "[", "5", "]"}]}], "+", - RowBox[{"2", " ", - RowBox[{"(", - RowBox[{ - RowBox[{"x1", "[", "2", "]"}], "-", - RowBox[{"x2", "[", "2", "]"}]}], ")"}], " ", - SuperscriptBox[ - RowBox[{"x2", "[", "5", "]"}], "2"]}], "-", + RowBox[{"x2", "[", "5", "]"}]}], "-", RowBox[{"2", " ", RowBox[{"so", "[", "1", "]"}], " ", RowBox[{"x1", "[", "5", "]"}], " ", @@ -2843,16 +2794,6 @@ Cell[BoxData[ RowBox[{"so", "[", "3", "]"}], " ", RowBox[{"x1", "[", "7", "]"}], " ", RowBox[{"x2", "[", "6", "]"}]}], "+", - RowBox[{ - RowBox[{"(", - RowBox[{ - RowBox[{ - RowBox[{"-", "2"}], " ", - RowBox[{"x1", "[", "1", "]"}]}], "+", - RowBox[{"2", " ", - RowBox[{"x2", "[", "1", "]"}]}]}], ")"}], " ", - RowBox[{"x2", "[", "5", "]"}], " ", - RowBox[{"x2", "[", "6", "]"}]}], "+", RowBox[{"2", " ", RowBox[{"so", "[", "1", "]"}], " ", RowBox[{"x1", "[", "4", "]"}], " ", @@ -2864,334 +2805,396 @@ Cell[BoxData[ RowBox[{"2", " ", RowBox[{"(", RowBox[{ - RowBox[{"x1", "[", "1", "]"}], "-", + RowBox[{"-", + RowBox[{"x1", "[", "1", "]"}]}], "+", RowBox[{"x2", "[", "1", "]"}]}], ")"}], " ", - RowBox[{"x2", "[", "4", "]"}], " ", - RowBox[{"x2", "[", "7", "]"}]}], "+", - RowBox[{ RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "2"}], " ", - RowBox[{"x1", "[", "3", "]"}]}], "+", - RowBox[{"2", " ", - RowBox[{"x2", "[", "3", "]"}]}]}], ")"}], " ", - RowBox[{"x2", "[", "6", "]"}], " ", - RowBox[{"x2", "[", "7", "]"}]}], "+", - RowBox[{"2", " ", - RowBox[{"(", - RowBox[{ - RowBox[{"x1", "[", "2", "]"}], "-", - RowBox[{"x2", "[", "2", "]"}]}], ")"}], " ", - SuperscriptBox[ - RowBox[{"x2", "[", "7", "]"}], "2"]}]}], ")"}]}]}], "Dt12"]}, - { - FractionBox[ - RowBox[{ - RowBox[{ - RowBox[{"-", - RowBox[{"(", - RowBox[{ - RowBox[{"-", "1"}], "+", - RowBox[{"2", " ", - SuperscriptBox[ - RowBox[{"qos", "[", "2", "]"}], "2"]}], "+", - RowBox[{"2", " ", - SuperscriptBox[ - RowBox[{"qos", "[", "3", "]"}], "2"]}]}], ")"}]}], " ", - RowBox[{"(", - RowBox[{ - RowBox[{"-", - RowBox[{"x1", "[", "3", "]"}]}], "+", - RowBox[{"x2", "[", "3", "]"}], "-", - RowBox[{"2", " ", - RowBox[{"so", "[", "2", "]"}], " ", - RowBox[{"x1", "[", "5", "]"}], " ", - RowBox[{"x2", "[", "4", "]"}]}], "+", - RowBox[{"2", " ", - RowBox[{"so", "[", "1", "]"}], " ", - RowBox[{"x1", "[", "6", "]"}], " ", - RowBox[{"x2", "[", "4", "]"}]}], "+", - RowBox[{"2", " ", - RowBox[{"so", "[", "2", "]"}], " ", - RowBox[{"x1", "[", "4", "]"}], " ", - RowBox[{"x2", "[", "5", "]"}]}], "+", - RowBox[{"2", " ", - RowBox[{"so", "[", "1", "]"}], " ", - RowBox[{"x1", "[", "7", "]"}], " ", - RowBox[{"x2", "[", "5", "]"}]}], "+", - RowBox[{"2", " ", - RowBox[{"(", - RowBox[{ - RowBox[{"x1", "[", "2", "]"}], "-", - RowBox[{"x2", "[", "2", "]"}]}], ")"}], " ", - RowBox[{"x2", "[", "4", "]"}], " ", - RowBox[{"x2", "[", "5", "]"}]}], "+", + RowBox[{"x2", "[", "5", "]"}], " ", + RowBox[{"x2", "[", "6", "]"}]}], "-", + RowBox[{ + RowBox[{"x2", "[", "4", "]"}], " ", + RowBox[{"x2", "[", "7", "]"}]}]}], ")"}]}], "+", RowBox[{"2", " ", RowBox[{"(", RowBox[{ - RowBox[{"x1", "[", "3", "]"}], "-", + RowBox[{"-", + RowBox[{"x1", "[", "3", "]"}]}], "+", RowBox[{"x2", "[", "3", "]"}]}], ")"}], " ", - SuperscriptBox[ - RowBox[{"x2", "[", "5", "]"}], "2"]}], "-", - RowBox[{"2", " ", - RowBox[{"so", "[", "1", "]"}], " ", - RowBox[{"x1", "[", "4", "]"}], " ", - RowBox[{"x2", "[", "6", "]"}]}], "+", - RowBox[{"2", " ", - RowBox[{"so", "[", "2", "]"}], " ", - RowBox[{"x1", "[", "7", "]"}], " ", - RowBox[{"x2", "[", "6", "]"}]}], "+", - RowBox[{ RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "2"}], " ", - RowBox[{"x1", "[", "1", "]"}]}], "+", - RowBox[{"2", " ", - RowBox[{"x2", "[", "1", "]"}]}]}], ")"}], " ", - RowBox[{"x2", "[", "4", "]"}], " ", - RowBox[{"x2", "[", "6", "]"}]}], "+", - RowBox[{"2", " ", - RowBox[{"(", - RowBox[{ - RowBox[{"x1", "[", "3", "]"}], "-", - RowBox[{"x2", "[", "3", "]"}]}], ")"}], " ", - SuperscriptBox[ - RowBox[{"x2", "[", "6", "]"}], "2"]}], "-", - RowBox[{"2", " ", - RowBox[{"so", "[", "1", "]"}], " ", - RowBox[{"x1", "[", "5", "]"}], " ", - RowBox[{"x2", "[", "7", "]"}]}], "-", - RowBox[{"2", " ", - RowBox[{"so", "[", "2", "]"}], " ", - RowBox[{"x1", "[", "6", "]"}], " ", - RowBox[{"x2", "[", "7", "]"}]}], "+", + RowBox[{"x2", "[", "4", "]"}], " ", + RowBox[{"x2", "[", "5", "]"}]}], "+", + RowBox[{ + RowBox[{"x2", "[", "6", "]"}], " ", + RowBox[{"x2", "[", "7", "]"}]}]}], ")"}]}], "+", RowBox[{ RowBox[{"(", RowBox[{ - RowBox[{ - RowBox[{"-", "2"}], " ", - RowBox[{"x1", "[", "1", "]"}]}], "+", - RowBox[{"2", " ", - RowBox[{"x2", "[", "1", "]"}]}]}], ")"}], " ", - RowBox[{"x2", "[", "5", "]"}], " ", - RowBox[{"x2", "[", "7", "]"}]}], "+", - RowBox[{ + RowBox[{"x1", "[", "2", "]"}], "-", + RowBox[{"x2", "[", "2", "]"}]}], ")"}], " ", RowBox[{"(", RowBox[{ - RowBox[{ - RowBox[{"-", "2"}], " ", - RowBox[{"x1", "[", "2", "]"}]}], "+", + RowBox[{"-", "1"}], "+", RowBox[{"2", " ", - RowBox[{"x2", "[", "2", "]"}]}]}], ")"}], " ", - RowBox[{"x2", "[", "6", "]"}], " ", - RowBox[{"x2", "[", "7", "]"}]}]}], ")"}]}], "+", + SuperscriptBox[ + RowBox[{"x2", "[", "5", "]"}], "2"]}], "+", + RowBox[{"2", " ", + SuperscriptBox[ + RowBox[{"x2", "[", "7", "]"}], "2"]}]}], ")"}]}]}], ")"}]}], + "-", RowBox[{"2", " ", RowBox[{"(", RowBox[{ - RowBox[{ - RowBox[{"qos", "[", "1", "]"}], " ", - RowBox[{"qos", "[", "3", "]"}]}], "+", RowBox[{ RowBox[{"qos", "[", "2", "]"}], " ", + RowBox[{"qos", "[", "3", "]"}]}], "-", + RowBox[{ + RowBox[{"qos", "[", "1", "]"}], " ", RowBox[{"qos", "[", "4", "]"}]}]}], ")"}], " ", RowBox[{"(", RowBox[{ - RowBox[{"-", - RowBox[{"x1", "[", "1", "]"}]}], "+", - RowBox[{"x2", "[", "1", "]"}], "-", RowBox[{"2", " ", RowBox[{"so", "[", "3", "]"}], " ", RowBox[{"x1", "[", "6", "]"}], " ", - RowBox[{"x2", "[", "4", "]"}]}], "+", + RowBox[{"x2", "[", "4", "]"}]}], "-", RowBox[{"2", " ", RowBox[{"so", "[", "2", "]"}], " ", RowBox[{"x1", "[", "7", "]"}], " ", - RowBox[{"x2", "[", "4", "]"}]}], "-", + RowBox[{"x2", "[", "4", "]"}]}], "+", RowBox[{"2", " ", RowBox[{"so", "[", "2", "]"}], " ", RowBox[{"x1", "[", "6", "]"}], " ", - RowBox[{"x2", "[", "5", "]"}]}], "-", + RowBox[{"x2", "[", "5", "]"}]}], "+", RowBox[{"2", " ", RowBox[{"so", "[", "3", "]"}], " ", RowBox[{"x1", "[", "7", "]"}], " ", - RowBox[{"x2", "[", "5", "]"}]}], "+", + RowBox[{"x2", "[", "5", "]"}]}], "-", RowBox[{"2", " ", RowBox[{"so", "[", "3", "]"}], " ", RowBox[{"x1", "[", "4", "]"}], " ", - RowBox[{"x2", "[", "6", "]"}]}], "+", + RowBox[{"x2", "[", "6", "]"}]}], "-", RowBox[{"2", " ", RowBox[{"so", "[", "2", "]"}], " ", RowBox[{"x1", "[", "5", "]"}], " ", RowBox[{"x2", "[", "6", "]"}]}], "+", - RowBox[{"2", " ", - RowBox[{"(", - RowBox[{ - RowBox[{"x1", "[", "3", "]"}], "-", - RowBox[{"x2", "[", "3", "]"}]}], ")"}], " ", - RowBox[{"x2", "[", "4", "]"}], " ", - RowBox[{"x2", "[", "6", "]"}]}], "+", - RowBox[{ - RowBox[{"(", - RowBox[{ - RowBox[{ - RowBox[{"-", "2"}], " ", - RowBox[{"x1", "[", "2", "]"}]}], "+", - RowBox[{"2", " ", - RowBox[{"x2", "[", "2", "]"}]}]}], ")"}], " ", - RowBox[{"x2", "[", "5", "]"}], " ", - RowBox[{"x2", "[", "6", "]"}]}], "+", - RowBox[{"2", " ", - RowBox[{"(", - RowBox[{ - RowBox[{"x1", "[", "1", "]"}], "-", - RowBox[{"x2", "[", "1", "]"}]}], ")"}], " ", - SuperscriptBox[ - RowBox[{"x2", "[", "6", "]"}], "2"]}], "-", RowBox[{"2", " ", RowBox[{"so", "[", "2", "]"}], " ", RowBox[{"x1", "[", "4", "]"}], " ", - RowBox[{"x2", "[", "7", "]"}]}], "+", + RowBox[{"x2", "[", "7", "]"}]}], "-", RowBox[{"2", " ", RowBox[{"so", "[", "3", "]"}], " ", RowBox[{"x1", "[", "5", "]"}], " ", RowBox[{"x2", "[", "7", "]"}]}], "+", - RowBox[{ - RowBox[{"(", - RowBox[{ - RowBox[{ - RowBox[{"-", "2"}], " ", - RowBox[{"x1", "[", "2", "]"}]}], "+", - RowBox[{"2", " ", - RowBox[{"x2", "[", "2", "]"}]}]}], ")"}], " ", - RowBox[{"x2", "[", "4", "]"}], " ", - RowBox[{"x2", "[", "7", "]"}]}], "+", - RowBox[{ - RowBox[{"(", - RowBox[{ - RowBox[{ - RowBox[{"-", "2"}], " ", - RowBox[{"x1", "[", "3", "]"}]}], "+", - RowBox[{"2", " ", - RowBox[{"x2", "[", "3", "]"}]}]}], ")"}], " ", - RowBox[{"x2", "[", "5", "]"}], " ", - RowBox[{"x2", "[", "7", "]"}]}], "+", RowBox[{"2", " ", RowBox[{"(", RowBox[{ - RowBox[{"x1", "[", "1", "]"}], "-", - RowBox[{"x2", "[", "1", "]"}]}], ")"}], " ", - SuperscriptBox[ - RowBox[{"x2", "[", "7", "]"}], "2"]}]}], ")"}]}], "-", - RowBox[{"2", " ", - RowBox[{"(", - RowBox[{ - RowBox[{ - RowBox[{"qos", "[", "1", "]"}], " ", - RowBox[{"qos", "[", "2", "]"}]}], "-", - RowBox[{ - RowBox[{"qos", "[", "3", "]"}], " ", - RowBox[{"qos", "[", "4", "]"}]}]}], ")"}], " ", - RowBox[{"(", - RowBox[{ - RowBox[{"-", - RowBox[{"x1", "[", "2", "]"}]}], "+", - RowBox[{"x2", "[", "2", "]"}], "+", - RowBox[{"2", " ", - RowBox[{"so", "[", "3", "]"}], " ", - RowBox[{"x1", "[", "5", "]"}], " ", - RowBox[{"x2", "[", "4", "]"}]}], "-", - RowBox[{"2", " ", - RowBox[{"so", "[", "1", "]"}], " ", - RowBox[{"x1", "[", "7", "]"}], " ", - RowBox[{"x2", "[", "4", "]"}]}], "-", - RowBox[{"2", " ", - RowBox[{"so", "[", "3", "]"}], " ", - RowBox[{"x1", "[", "4", "]"}], " ", - RowBox[{"x2", "[", "5", "]"}]}], "+", - RowBox[{"2", " ", - RowBox[{"so", "[", "1", "]"}], " ", - RowBox[{"x1", "[", "6", "]"}], " ", - RowBox[{"x2", "[", "5", "]"}]}], "+", - RowBox[{ + RowBox[{"x1", "[", "2", "]"}], "-", + RowBox[{"x2", "[", "2", "]"}]}], ")"}], " ", RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "2"}], " ", - RowBox[{"x1", "[", "3", "]"}]}], "+", - RowBox[{"2", " ", - RowBox[{"x2", "[", "3", "]"}]}]}], ")"}], " ", - RowBox[{"x2", "[", "4", "]"}], " ", - RowBox[{"x2", "[", "5", "]"}]}], "+", + RowBox[{"x2", "[", "5", "]"}], " ", + RowBox[{"x2", "[", "6", "]"}]}], "+", + RowBox[{ + RowBox[{"x2", "[", "4", "]"}], " ", + RowBox[{"x2", "[", "7", "]"}]}]}], ")"}]}], "-", RowBox[{"2", " ", RowBox[{"(", RowBox[{ - RowBox[{"x1", "[", "2", "]"}], "-", - RowBox[{"x2", "[", "2", "]"}]}], ")"}], " ", - SuperscriptBox[ - RowBox[{"x2", "[", "5", "]"}], "2"]}], "-", - RowBox[{"2", " ", - RowBox[{"so", "[", "1", "]"}], " ", - RowBox[{"x1", "[", "5", "]"}], " ", - RowBox[{"x2", "[", "6", "]"}]}], "-", - RowBox[{"2", " ", - RowBox[{"so", "[", "3", "]"}], " ", - RowBox[{"x1", "[", "7", "]"}], " ", - RowBox[{"x2", "[", "6", "]"}]}], "+", - RowBox[{ + RowBox[{"x1", "[", "3", "]"}], "-", + RowBox[{"x2", "[", "3", "]"}]}], ")"}], " ", RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"-", "2"}], " ", - RowBox[{"x1", "[", "1", "]"}]}], "+", - RowBox[{"2", " ", - RowBox[{"x2", "[", "1", "]"}]}]}], ")"}], " ", - RowBox[{"x2", "[", "5", "]"}], " ", - RowBox[{"x2", "[", "6", "]"}]}], "+", - RowBox[{"2", " ", - RowBox[{"so", "[", "1", "]"}], " ", - RowBox[{"x1", "[", "4", "]"}], " ", - RowBox[{"x2", "[", "7", "]"}]}], "+", - RowBox[{"2", " ", - RowBox[{"so", "[", "3", "]"}], " ", - RowBox[{"x1", "[", "6", "]"}], " ", - RowBox[{"x2", "[", "7", "]"}]}], "+", - RowBox[{"2", " ", + RowBox[{"x2", "[", "4", "]"}], " ", + RowBox[{"x2", "[", "6", "]"}]}], "-", + RowBox[{ + RowBox[{"x2", "[", "5", "]"}], " ", + RowBox[{"x2", "[", "7", "]"}]}]}], ")"}]}], "-", + RowBox[{ RowBox[{"(", RowBox[{ RowBox[{"x1", "[", "1", "]"}], "-", RowBox[{"x2", "[", "1", "]"}]}], ")"}], " ", - RowBox[{"x2", "[", "4", "]"}], " ", - RowBox[{"x2", "[", "7", "]"}]}], "+", - RowBox[{ RowBox[{"(", RowBox[{ - RowBox[{ - RowBox[{"-", "2"}], " ", - RowBox[{"x1", "[", "3", "]"}]}], "+", + RowBox[{"-", "1"}], "+", RowBox[{"2", " ", - RowBox[{"x2", "[", "3", "]"}]}]}], ")"}], " ", - RowBox[{"x2", "[", "6", "]"}], " ", - RowBox[{"x2", "[", "7", "]"}]}], "+", - RowBox[{"2", " ", - RowBox[{"(", - RowBox[{ - RowBox[{"x1", "[", "2", "]"}], "-", - RowBox[{"x2", "[", "2", "]"}]}], ")"}], " ", - SuperscriptBox[ - RowBox[{"x2", "[", "7", "]"}], "2"]}]}], ")"}]}]}], "Dt12"]} + SuperscriptBox[ + RowBox[{"x2", "[", "6", "]"}], "2"]}], "+", + RowBox[{"2", " ", + SuperscriptBox[ + RowBox[{"x2", "[", "7", "]"}], "2"]}]}], ")"}]}]}], + ")"}]}]}], "Dt12"]}, + { + RowBox[{"-", + FractionBox[ + RowBox[{ + RowBox[{ + RowBox[{"(", + RowBox[{"1", "-", + RowBox[{"2", " ", + SuperscriptBox[ + RowBox[{"qos", "[", "2", "]"}], "2"]}], "-", + RowBox[{"2", " ", + SuperscriptBox[ + RowBox[{"qos", "[", "3", "]"}], "2"]}]}], ")"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"2", " ", + RowBox[{"so", "[", "2", "]"}], " ", + RowBox[{"x1", "[", "5", "]"}], " ", + RowBox[{"x2", "[", "4", "]"}]}], "-", + RowBox[{"2", " ", + RowBox[{"so", "[", "1", "]"}], " ", + RowBox[{"x1", "[", "6", "]"}], " ", + RowBox[{"x2", "[", "4", "]"}]}], "-", + RowBox[{"2", " ", + RowBox[{"so", "[", "2", "]"}], " ", + RowBox[{"x1", "[", "4", "]"}], " ", + RowBox[{"x2", "[", "5", "]"}]}], "-", + RowBox[{"2", " ", + RowBox[{"so", "[", "1", "]"}], " ", + RowBox[{"x1", "[", "7", "]"}], " ", + RowBox[{"x2", "[", "5", "]"}]}], "+", + RowBox[{"2", " ", + RowBox[{"so", "[", "1", "]"}], " ", + RowBox[{"x1", "[", "4", "]"}], " ", + RowBox[{"x2", "[", "6", "]"}]}], "-", + RowBox[{"2", " ", + RowBox[{"so", "[", "2", "]"}], " ", + RowBox[{"x1", "[", "7", "]"}], " ", + RowBox[{"x2", "[", "6", "]"}]}], "-", + RowBox[{ + RowBox[{"(", + RowBox[{ + RowBox[{"x1", "[", "3", "]"}], "-", + RowBox[{"x2", "[", "3", "]"}]}], ")"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"-", "1"}], "+", + RowBox[{"2", " ", + SuperscriptBox[ + RowBox[{"x2", "[", "5", "]"}], "2"]}], "+", + RowBox[{"2", " ", + SuperscriptBox[ + RowBox[{"x2", "[", "6", "]"}], "2"]}]}], ")"}]}], "+", + RowBox[{"2", " ", + RowBox[{"so", "[", "1", "]"}], " ", + RowBox[{"x1", "[", "5", "]"}], " ", + RowBox[{"x2", "[", "7", "]"}]}], "+", + RowBox[{"2", " ", + RowBox[{"so", "[", "2", "]"}], " ", + RowBox[{"x1", "[", "6", "]"}], " ", + RowBox[{"x2", "[", "7", "]"}]}], "+", + RowBox[{"2", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"x1", "[", "1", "]"}], "-", + RowBox[{"x2", "[", "1", "]"}]}], ")"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"x2", "[", "4", "]"}], " ", + RowBox[{"x2", "[", "6", "]"}]}], "+", + RowBox[{ + RowBox[{"x2", "[", "5", "]"}], " ", + RowBox[{"x2", "[", "7", "]"}]}]}], ")"}]}], "-", + RowBox[{"2", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"x1", "[", "2", "]"}], "-", + RowBox[{"x2", "[", "2", "]"}]}], ")"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"x2", "[", "4", "]"}], " ", + RowBox[{"x2", "[", "5", "]"}]}], "-", + RowBox[{ + RowBox[{"x2", "[", "6", "]"}], " ", + RowBox[{"x2", "[", "7", "]"}]}]}], ")"}]}]}], ")"}]}], "+", + RowBox[{"2", " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"qos", "[", "1", "]"}], " ", + RowBox[{"qos", "[", "2", "]"}]}], "-", + RowBox[{ + RowBox[{"qos", "[", "3", "]"}], " ", + RowBox[{"qos", "[", "4", "]"}]}]}], ")"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"2", " ", + RowBox[{"so", "[", "3", "]"}], " ", + RowBox[{"x1", "[", "5", "]"}], " ", + RowBox[{"x2", "[", "4", "]"}]}], "-", + RowBox[{"2", " ", + RowBox[{"so", "[", "1", "]"}], " ", + RowBox[{"x1", "[", "7", "]"}], " ", + RowBox[{"x2", "[", "4", "]"}]}], "-", + RowBox[{"2", " ", + RowBox[{"so", "[", "3", "]"}], " ", + RowBox[{"x1", "[", "4", "]"}], " ", + RowBox[{"x2", "[", "5", "]"}]}], "+", + RowBox[{"2", " ", + RowBox[{"so", "[", "1", "]"}], " ", + RowBox[{"x1", "[", "6", "]"}], " ", + RowBox[{"x2", "[", "5", "]"}]}], "-", + RowBox[{"2", " ", + RowBox[{"so", "[", "1", "]"}], " ", + RowBox[{"x1", "[", "5", "]"}], " ", + RowBox[{"x2", "[", "6", "]"}]}], "-", + RowBox[{"2", " ", + RowBox[{"so", "[", "3", "]"}], " ", + RowBox[{"x1", "[", "7", "]"}], " ", + RowBox[{"x2", "[", "6", "]"}]}], "+", + RowBox[{"2", " ", + RowBox[{"so", "[", "1", "]"}], " ", + RowBox[{"x1", "[", "4", "]"}], " ", + RowBox[{"x2", "[", "7", "]"}]}], "+", + RowBox[{"2", " ", + RowBox[{"so", "[", "3", "]"}], " ", + RowBox[{"x1", "[", "6", "]"}], " ", + RowBox[{"x2", "[", "7", "]"}]}], "+", + RowBox[{"2", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"-", + RowBox[{"x1", "[", "1", "]"}]}], "+", + RowBox[{"x2", "[", "1", "]"}]}], ")"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"x2", "[", "5", "]"}], " ", + RowBox[{"x2", "[", "6", "]"}]}], "-", + RowBox[{ + RowBox[{"x2", "[", "4", "]"}], " ", + RowBox[{"x2", "[", "7", "]"}]}]}], ")"}]}], "+", + RowBox[{"2", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"-", + RowBox[{"x1", "[", "3", "]"}]}], "+", + RowBox[{"x2", "[", "3", "]"}]}], ")"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"x2", "[", "4", "]"}], " ", + RowBox[{"x2", "[", "5", "]"}]}], "+", + RowBox[{ + RowBox[{"x2", "[", "6", "]"}], " ", + RowBox[{"x2", "[", "7", "]"}]}]}], ")"}]}], "+", + RowBox[{ + RowBox[{"(", + RowBox[{ + RowBox[{"x1", "[", "2", "]"}], "-", + RowBox[{"x2", "[", "2", "]"}]}], ")"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"-", "1"}], "+", + RowBox[{"2", " ", + SuperscriptBox[ + RowBox[{"x2", "[", "5", "]"}], "2"]}], "+", + RowBox[{"2", " ", + SuperscriptBox[ + RowBox[{"x2", "[", "7", "]"}], "2"]}]}], ")"}]}]}], ")"}]}], + "+", + RowBox[{"2", " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"qos", "[", "1", "]"}], " ", + RowBox[{"qos", "[", "3", "]"}]}], "+", + RowBox[{ + RowBox[{"qos", "[", "2", "]"}], " ", + RowBox[{"qos", "[", "4", "]"}]}]}], ")"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"2", " ", + RowBox[{"so", "[", "3", "]"}], " ", + RowBox[{"x1", "[", "6", "]"}], " ", + RowBox[{"x2", "[", "4", "]"}]}], "-", + RowBox[{"2", " ", + RowBox[{"so", "[", "2", "]"}], " ", + RowBox[{"x1", "[", "7", "]"}], " ", + RowBox[{"x2", "[", "4", "]"}]}], "+", + RowBox[{"2", " ", + RowBox[{"so", "[", "2", "]"}], " ", + RowBox[{"x1", "[", "6", "]"}], " ", + RowBox[{"x2", "[", "5", "]"}]}], "+", + RowBox[{"2", " ", + RowBox[{"so", "[", "3", "]"}], " ", + RowBox[{"x1", "[", "7", "]"}], " ", + RowBox[{"x2", "[", "5", "]"}]}], "-", + RowBox[{"2", " ", + RowBox[{"so", "[", "3", "]"}], " ", + RowBox[{"x1", "[", "4", "]"}], " ", + RowBox[{"x2", "[", "6", "]"}]}], "-", + RowBox[{"2", " ", + RowBox[{"so", "[", "2", "]"}], " ", + RowBox[{"x1", "[", "5", "]"}], " ", + RowBox[{"x2", "[", "6", "]"}]}], "+", + RowBox[{"2", " ", + RowBox[{"so", "[", "2", "]"}], " ", + RowBox[{"x1", "[", "4", "]"}], " ", + RowBox[{"x2", "[", "7", "]"}]}], "-", + RowBox[{"2", " ", + RowBox[{"so", "[", "3", "]"}], " ", + RowBox[{"x1", "[", "5", "]"}], " ", + RowBox[{"x2", "[", "7", "]"}]}], "+", + RowBox[{"2", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"x1", "[", "2", "]"}], "-", + RowBox[{"x2", "[", "2", "]"}]}], ")"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"x2", "[", "5", "]"}], " ", + RowBox[{"x2", "[", "6", "]"}]}], "+", + RowBox[{ + RowBox[{"x2", "[", "4", "]"}], " ", + RowBox[{"x2", "[", "7", "]"}]}]}], ")"}]}], "-", + RowBox[{"2", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"x1", "[", "3", "]"}], "-", + RowBox[{"x2", "[", "3", "]"}]}], ")"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"x2", "[", "4", "]"}], " ", + RowBox[{"x2", "[", "6", "]"}]}], "-", + RowBox[{ + RowBox[{"x2", "[", "5", "]"}], " ", + RowBox[{"x2", "[", "7", "]"}]}]}], ")"}]}], "-", + RowBox[{ + RowBox[{"(", + RowBox[{ + RowBox[{"x1", "[", "1", "]"}], "-", + RowBox[{"x2", "[", "1", "]"}]}], ")"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"-", "1"}], "+", + RowBox[{"2", " ", + SuperscriptBox[ + RowBox[{"x2", "[", "6", "]"}], "2"]}], "+", + RowBox[{"2", " ", + SuperscriptBox[ + RowBox[{"x2", "[", "7", "]"}], "2"]}]}], ")"}]}]}], + ")"}]}]}], "Dt12"]}]} }, - GridBoxAlignment->{ - "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, - "RowsIndexed" -> {}}, + GridBoxAlignment->{"Columns" -> {{Center}}, "Rows" -> {{Baseline}}}, GridBoxSpacings->{"Columns" -> { Offset[0.27999999999999997`], { Offset[0.5599999999999999]}, - Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> { + Offset[0.27999999999999997`]}, "Rows" -> { Offset[0.2], { Offset[0.4]}, - Offset[0.2]}, "RowsIndexed" -> {}}], + Offset[0.2]}}], Column], "\[NoBreak]", ")"}], Function[BoxForm`e$, MatrixForm[BoxForm`e$]]]], "Output", @@ -3202,24 +3205,16 @@ Cell[BoxData[ 3.697261543011924*^9, 3.697261721894086*^9, 3.697263751548497*^9, 3.752577595933364*^9, 3.752813571918913*^9, 3.7624941693843327`*^9, 3.763878009192663*^9, 3.823863577014786*^9, 3.823863700048583*^9, - 3.823933297191674*^9}, - ExpressionUUID -> "ed38f097-9b2e-4220-8549-bcbd0cd327da"], + 3.823933297191674*^9, 3.832654119186309*^9, 3.832654394639473*^9, + 3.832655338443295*^9, 3.832682723193851*^9, 3.832841944651578*^9}, + CellLabel-> + "Out[398]//MatrixForm=",ExpressionUUID->"03000688-f85f-4d04-9c50-\ +e0d07d99b5ec"], Cell[BoxData[ InterpretationBox[ TagBox[ - PanelBox[GridBox[{ - { - StyleBox[ - StyleBox[ - DynamicBox[ToBoxes[ - FEPrivate`FrontEndResource["FEStrings", "sizeExplanation"], - StandardForm], - ImageSizeCache->{674., {5., 18.}}], - StripOnInput->False, - DynamicUpdating->True], "Panel", - StripOnInput->False, - Background->None]}, + FrameBox[GridBox[{ { ItemBox[ TagBox[ @@ -3227,60 +3222,252 @@ Cell[BoxData[ RowBox[{"(", "\[NoBreak]", TagBox[GridBox[{ { - FractionBox[ - RowBox[{"2", " ", - RowBox[{"(", - RowBox[{"\[LeftSkeleton]", "1", "\[RightSkeleton]"}], - ")"}]}], - RowBox[{"Dt01", " ", - SuperscriptBox["Dt12", "2"], " ", - RowBox[{"(", - RowBox[{"Dt01", "+", "Dt12"}], ")"}]}]]}, - { - FractionBox[ - RowBox[{"2", " ", - RowBox[{"(", - RowBox[{ + RowBox[{"-", + FractionBox[ + RowBox[{"2", " ", + RowBox[{"(", RowBox[{ - RowBox[{"-", "2"}], " ", + RowBox[{"2", " ", RowBox[{"(", RowBox[{ RowBox[{ - RowBox[{"qos", "[", "1", "]"}], " ", - RowBox[{"qos", "[", "2", "]"}]}], "+", + RowBox[{"qos", "[", "2", "]"}], " ", + RowBox[{"qos", "[", "3", "]"}]}], "+", RowBox[{ - RowBox[{"qos", "[", "3", "]"}], " ", - RowBox[{"\[LeftSkeleton]", "1", "\[RightSkeleton]"}]}]}], - ")"}], " ", + RowBox[{"qos", "[", "1", "]"}], " ", + RowBox[{"qos", "[", "4", "]"}]}]}], ")"}], " ", RowBox[{"(", RowBox[{ - RowBox[{ - RowBox[{"-", - SuperscriptBox["Dt12", "2"]}], " ", - RowBox[{"x0", "[", "3", "]"}]}], "+", - RowBox[{"Dt01", " ", "Dt12", " ", - RowBox[{"x1", "[", "3", "]"}]}], "+", - RowBox[{"\[LeftSkeleton]", "181", "\[RightSkeleton]"}], - "+", + RowBox[{"2", " ", + SuperscriptBox["Dt12", "2"], " ", + RowBox[{"so", "[", "3", "]"}], " ", + RowBox[{"x0", "[", "5", "]"}], " ", + RowBox[{"x1", "[", "4", "]"}]}], "-", + RowBox[{"2", " ", + SuperscriptBox["Dt12", "2"], " ", + RowBox[{"so", "[", "1", "]"}], " ", + RowBox[{"x0", "[", "7", "]"}], " ", + RowBox[{"x1", "[", "4", "]"}]}], "-", + RowBox[{"2", " ", + SuperscriptBox["Dt12", "2"], " ", + RowBox[{"so", "[", "3", "]"}], " ", + RowBox[{"x0", "[", "4", "]"}], " ", + RowBox[{"x1", "[", "5", "]"}]}], "-", + RowBox[{"2", " ", + SuperscriptBox["Dt12", "2"], " ", + RowBox[{"so", "[", "1", "]"}], " ", + RowBox[{"x0", "[", "6", "]"}], " ", + RowBox[{"x1", "[", "5", "]"}]}], "+", + RowBox[{"2", " ", + SuperscriptBox["Dt12", "2"], " ", + RowBox[{"so", "[", "1", "]"}], " ", + RowBox[{"x0", "[", "5", "]"}], " ", + RowBox[{"x1", "[", "6", "]"}]}], "+", + RowBox[{"2", " ", + SuperscriptBox["Dt12", "2"], " ", + RowBox[{"so", "[", "3", "]"}], " ", + RowBox[{"x0", "[", "7", "]"}], " ", + RowBox[{"x1", "[", "6", "]"}]}], "+", + TemplateBox[{"154"}, + "OutputSizeLimit`Skeleton"], "+", RowBox[{"2", " ", "Dt01", " ", RowBox[{"(", RowBox[{"Dt01", "+", "Dt12"}], ")"}], " ", RowBox[{"so", "[", "1", "]"}], " ", RowBox[{"x1", "[", "5", "]"}], " ", - RowBox[{"x1", "[", "7", "]"}], " ", + RowBox[{"x1", "[", "6", "]"}], " ", SuperscriptBox[ RowBox[{"x2", "[", "7", "]"}], "2"]}], "+", RowBox[{"2", " ", "Dt01", " ", RowBox[{"(", RowBox[{"Dt01", "+", "Dt12"}], ")"}], " ", RowBox[{"so", "[", "2", "]"}], " ", - RowBox[{"x1", "[", "6", "]"}], " ", + SuperscriptBox[ + RowBox[{"x1", "[", "6", "]"}], "2"], " ", + SuperscriptBox[ + RowBox[{"x2", "[", "7", "]"}], "2"]}], "+", + RowBox[{"4", " ", + SuperscriptBox["Dt12", "2"], " ", + RowBox[{"so", "[", "3", "]"}], " ", + RowBox[{"x0", "[", "6", "]"}], " ", RowBox[{"x1", "[", "7", "]"}], " ", SuperscriptBox[ - RowBox[{"x2", "[", "7", "]"}], "2"]}]}], ")"}]}], "-", - RowBox[{"\[LeftSkeleton]", "1", "\[RightSkeleton]"}], "+", - - RowBox[{"\[LeftSkeleton]", "1", "\[RightSkeleton]"}]}], + RowBox[{"x2", "[", "7", "]"}], "2"]}], "-", + RowBox[{"2", " ", "Dt12", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"Dt12", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"x0", "[", "1", "]"}], "-", + RowBox[{"x1", "[", "1", "]"}]}], ")"}]}], "+", + RowBox[{"Dt01", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"-", + RowBox[{"x1", "[", "1", "]"}]}], "+", + RowBox[{"x2", "[", "1", "]"}]}], ")"}]}]}], ")"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"x2", "[", "5", "]"}], " ", + RowBox[{"x2", "[", "6", "]"}]}], "-", + RowBox[{ + RowBox[{"x2", "[", "4", "]"}], " ", + RowBox[{"x2", "[", "7", "]"}]}]}], ")"}]}], "-", + RowBox[{"2", " ", "Dt12", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"Dt12", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"x0", "[", "3", "]"}], "-", + RowBox[{"x1", "[", "3", "]"}]}], ")"}]}], "+", + RowBox[{"Dt01", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"-", + RowBox[{"x1", "[", "3", "]"}]}], "+", + RowBox[{"x2", "[", "3", "]"}]}], ")"}]}]}], ")"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"x2", "[", "4", "]"}], " ", + RowBox[{"x2", "[", "5", "]"}]}], "+", + RowBox[{ + RowBox[{"x2", "[", "6", "]"}], " ", + RowBox[{"x2", "[", "7", "]"}]}]}], ")"}]}], "+", + RowBox[{"Dt12", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"Dt12", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"x0", "[", "2", "]"}], "-", + RowBox[{"x1", "[", "2", "]"}]}], ")"}]}], "+", + RowBox[{"Dt01", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"-", + RowBox[{"x1", "[", "2", "]"}]}], "+", + RowBox[{"x2", "[", "2", "]"}]}], ")"}]}]}], ")"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"-", "1"}], "+", + RowBox[{"2", " ", + SuperscriptBox[ + RowBox[{"x2", "[", "5", "]"}], "2"]}], "+", + RowBox[{"2", " ", + SuperscriptBox[ + RowBox[{"x2", "[", "7", "]"}], "2"]}]}], ")"}]}]}], + ")"}]}], "+", + RowBox[{"2", " ", + RowBox[{"(", + TemplateBox[{"1"}, + "OutputSizeLimit`Skeleton"], ")"}], " ", + RowBox[{"(", + TemplateBox[{"1"}, + "OutputSizeLimit`Skeleton"], ")"}]}], "-", + RowBox[{ + RowBox[{"(", + RowBox[{"1", "-", + RowBox[{"2", " ", + SuperscriptBox[ + RowBox[{"qos", "[", "3", "]"}], "2"]}], "-", + RowBox[{"2", " ", + SuperscriptBox[ + RowBox[{"qos", "[", "4", "]"}], "2"]}]}], ")"}], " ", + RowBox[{"(", + TemplateBox[{"1"}, + "OutputSizeLimit`Skeleton"], ")"}]}]}], ")"}]}], + RowBox[{"Dt01", " ", + SuperscriptBox["Dt12", "2"], " ", + RowBox[{"(", + RowBox[{"Dt01", "+", "Dt12"}], ")"}]}]]}]}, + { + FractionBox[ + RowBox[{"2", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"-", + RowBox[{"(", + RowBox[{ + RowBox[{"(", + RowBox[{"1", "-", + RowBox[{"2", " ", + SuperscriptBox[ + RowBox[{"qos", "[", "2", "]"}], "2"]}], "-", + RowBox[{"2", " ", + SuperscriptBox[ + RowBox[{"qos", "[", "4", "]"}], "2"]}]}], ")"}], " ", + RowBox[{"(", + RowBox[{ + TemplateBox[{"170"}, + "OutputSizeLimit`Skeleton"], "+", + RowBox[{"Dt12", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"Dt12", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"x0", "[", "2", "]"}], "-", + RowBox[{"x1", "[", "2", "]"}]}], ")"}]}], "+", + RowBox[{"Dt01", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"-", + RowBox[{"x1", "[", "2", "]"}]}], "+", + RowBox[{"x2", "[", "2", "]"}]}], ")"}]}]}], ")"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"-", "1"}], "+", + RowBox[{"2", " ", + SuperscriptBox[ + RowBox[{"x2", "[", "5", "]"}], "2"]}], "+", + RowBox[{"2", " ", + SuperscriptBox[ + RowBox[{"x2", "[", "7", "]"}], "2"]}]}], ")"}]}]}], + ")"}]}], ")"}]}], "+", + RowBox[{"2", " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"qos", "[", "1", "]"}], " ", + RowBox[{"qos", "[", "2", "]"}]}], "+", + RowBox[{ + RowBox[{"qos", "[", "3", "]"}], " ", + RowBox[{"qos", "[", "4", "]"}]}]}], ")"}], " ", + RowBox[{"(", + RowBox[{ + TemplateBox[{"169"}, + "OutputSizeLimit`Skeleton"], "+", + RowBox[{"4", " ", + TemplateBox[{"4"}, + "OutputSizeLimit`Skeleton"], " ", + SuperscriptBox[ + TemplateBox[{"1"}, + "OutputSizeLimit`Skeleton"], "2"]}], "-", + RowBox[{"Dt12", " ", + RowBox[{"(", + TemplateBox[{"1"}, + "OutputSizeLimit`Skeleton"], ")"}]}]}], ")"}]}], "+", + RowBox[{"2", " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"qos", "[", "2", "]"}], " ", + RowBox[{"qos", "[", "3", "]"}]}], "-", + RowBox[{ + RowBox[{"qos", "[", "1", "]"}], " ", + RowBox[{"qos", "[", "4", "]"}]}]}], ")"}], " ", + RowBox[{"(", + RowBox[{ + TemplateBox[{"170"}, + "OutputSizeLimit`Skeleton"], "+", + RowBox[{"Dt12", " ", + RowBox[{"(", + TemplateBox[{"1"}, + "OutputSizeLimit`Skeleton"], ")"}]}]}], ")"}]}]}], ")"}]}], RowBox[{"Dt01", " ", SuperscriptBox["Dt12", "2"], " ", @@ -3290,8 +3477,51 @@ Cell[BoxData[ FractionBox[ RowBox[{"2", " ", RowBox[{"(", - RowBox[{"\[LeftSkeleton]", "1", "\[RightSkeleton]"}], - ")"}]}], + RowBox[{ + RowBox[{"2", " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"qos", "[", "1", "]"}], " ", + RowBox[{"qos", "[", "2", "]"}]}], "-", + RowBox[{ + RowBox[{"qos", "[", "3", "]"}], " ", + RowBox[{"qos", "[", "4", "]"}]}]}], ")"}], " ", + RowBox[{"(", + RowBox[{ + TemplateBox[{"170"}, + "OutputSizeLimit`Skeleton"], "+", + RowBox[{"Dt12", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"Dt12", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"x0", "[", "2", "]"}], "-", + RowBox[{"x1", "[", "2", "]"}]}], ")"}]}], "+", + RowBox[{"Dt01", " ", + RowBox[{"(", + TemplateBox[{"1"}, + "OutputSizeLimit`Skeleton"], ")"}]}]}], ")"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{"-", "1"}], "+", + RowBox[{"2", " ", + SuperscriptBox[ + RowBox[{"x2", "[", "5", "]"}], "2"]}], "+", + RowBox[{"2", " ", + SuperscriptBox[ + RowBox[{"x2", "[", "7", "]"}], "2"]}]}], ")"}]}]}], + ")"}]}], "+", + TemplateBox[{"1"}, + "OutputSizeLimit`Skeleton"], "+", + RowBox[{"2", " ", + RowBox[{"(", + TemplateBox[{"1"}, + "OutputSizeLimit`Skeleton"], ")"}], " ", + RowBox[{"(", + TemplateBox[{"1"}, + "OutputSizeLimit`Skeleton"], ")"}]}]}], ")"}]}], RowBox[{"Dt01", " ", SuperscriptBox["Dt12", "2"], " ", RowBox[{"(", @@ -3299,126 +3529,239 @@ Cell[BoxData[ }, GridBoxAlignment->{ - "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, - "Rows" -> {{Baseline}}, "RowsIndexed" -> {}}, + "Columns" -> {{Center}}, "Rows" -> {{Baseline}}}, GridBoxSpacings->{"Columns" -> { Offset[0.27999999999999997`], { Offset[0.5599999999999999]}, - Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, - "Rows" -> { + Offset[0.27999999999999997`]}, "Rows" -> { Offset[0.2], { Offset[0.4]}, - Offset[0.2]}, "RowsIndexed" -> {}}], + Offset[0.2]}}], Column], "\[NoBreak]", ")"}], Function[BoxForm`e$, MatrixForm[BoxForm`e$]]], Short[#, 5]& ], - Background->GrayLevel[1], BaseStyle->{Deployed -> False}, - Frame->True, - FrameStyle->GrayLevel[0, 0.2], StripOnInput->False]}, - { - RowBox[{ - ButtonBox[ - StyleBox[ - StyleBox[ - DynamicBox[ToBoxes[ - FEPrivate`FrontEndResource["FEStrings", "sizeShowLess"], - StandardForm], - ImageSizeCache->{120., {0., 18.}}], - StripOnInput->False, - DynamicUpdating->True], "Panel", - StripOnInput->False, - Background->None], - Appearance->Automatic, - ButtonFunction:>OutputSizeLimit`ButtonFunction[ - Function[{OutputSizeLimit`Dump`x$}, - MatrixForm[OutputSizeLimit`Dump`x$]], 87, 33305684090290960952, 5/ - 2], - Enabled->True, - Evaluator->Automatic, - Method->"Queued"], "\[ThinSpace]", - ButtonBox[ - StyleBox[ - StyleBox[ - DynamicBox[ToBoxes[ - FEPrivate`FrontEndResource["FEStrings", "sizeShowMore"], - StandardForm], - ImageSizeCache->{126., {0., 18.}}], - StripOnInput->False, - DynamicUpdating->True], "Panel", - StripOnInput->False, - Background->None], - Appearance->Automatic, - ButtonFunction:>OutputSizeLimit`ButtonFunction[ - Function[{OutputSizeLimit`Dump`x$}, - MatrixForm[OutputSizeLimit`Dump`x$]], 87, 33305684090290960952, 5 - 2], - Enabled->True, - Evaluator->Automatic, - Method->"Queued"], "\[ThinSpace]", - ButtonBox[ - StyleBox[ - StyleBox[ - DynamicBox[ToBoxes[ - FEPrivate`FrontEndResource["FEStrings", "sizeShowAll"], - StandardForm], - ImageSizeCache->{195., {5., 18.}}], - StripOnInput->False, - DynamicUpdating->True], "Panel", - StripOnInput->False, - Background->None], - Appearance->Automatic, - ButtonFunction:>OutputSizeLimit`ButtonFunction[ - Function[{OutputSizeLimit`Dump`x$}, - MatrixForm[OutputSizeLimit`Dump`x$]], 87, 33305684090290960952, - Infinity], - Enabled->True, - Evaluator->Automatic, - Method->"Queued"], "\[ThinSpace]", - ButtonBox[ - StyleBox[ - StyleBox[ - DynamicBox[ToBoxes[ - FEPrivate`FrontEndResource["FEStrings", "sizeChangeLimit"], - StandardForm], - ImageSizeCache->{178., {0., 18.}}], - StripOnInput->False, - DynamicUpdating->True], "Panel", - StripOnInput->False, - Background->None], - Appearance->Automatic, - ButtonFunction:>FrontEndExecute[{ - FrontEnd`SetOptions[ - FrontEnd`$FrontEnd, - FrontEnd`PreferencesSettings -> {"Page" -> "Evaluation"}], - FrontEnd`FrontEndToken["PreferencesDialog"]}], - Evaluator->None, - Method->"Preemptive"]}]} + {GridBox[{ + { + PaneBox[ + TagBox[ + TooltipBox[ + StyleBox[ + StyleBox[ + DynamicBox[ToBoxes[ + FEPrivate`FrontEndResource[ + "FEStrings", "sizeBriefExplanation"], StandardForm], + ImageSizeCache->{36.94921875, {2., 5.}}], + StripOnInput->False, + DynamicUpdating->True, + LineSpacing->{1, 2}, + LineIndent->0, + LinebreakAdjustments->{1., 100, 0, 0, 0}], "OSLText", + StripOnInput->False], + StyleBox[ + DynamicBox[ + ToBoxes[ + FEPrivate`FrontEndResource["FEStrings", "sizeExplanation"], + StandardForm]], DynamicUpdating -> True, LineIndent -> 0, + LinebreakAdjustments -> {1., 100, 0, 0, 0}, + LineSpacing -> {1, 2}, StripOnInput -> False]], + Annotation[#, + Style[ + Dynamic[ + FEPrivate`FrontEndResource["FEStrings", "sizeExplanation"]], + DynamicUpdating -> True, LineIndent -> 0, + LinebreakAdjustments -> {1., 100, 0, 0, 0}, + LineSpacing -> {1, 2}], "Tooltip"]& ], + Alignment->Center, + BaselinePosition->Baseline, + ImageSize->{Automatic, {25, Full}}], + ButtonBox[ + PaneSelectorBox[{False-> + StyleBox[ + StyleBox[ + DynamicBox[ToBoxes[ + FEPrivate`FrontEndResource["FEStrings", "sizeShowLess"], + StandardForm], + ImageSizeCache->{32.8125, {0., 5.}}], + StripOnInput->False, + DynamicUpdating->True, + LineSpacing->{1, 2}, + LineIndent->0, + LinebreakAdjustments->{1., 100, 0, 0, 0}], "OSLControl", + StripOnInput->False], True-> + StyleBox[ + StyleBox[ + DynamicBox[ToBoxes[ + FEPrivate`FrontEndResource["FEStrings", "sizeShowLess"], + StandardForm]], + StripOnInput->False, + DynamicUpdating->True, + LineSpacing->{1, 2}, + LineIndent->0, + LinebreakAdjustments->{1., 100, 0, 0, 0}], "OSLControlActive", + StripOnInput->False]}, Dynamic[ + CurrentValue["MouseOver"]], + Alignment->Center, + FrameMargins->0, + ImageSize->{Automatic, {25, Full}}], + Appearance->None, + BaselinePosition->Baseline, + ButtonFunction:>OutputSizeLimit`ButtonFunction[ + Composition[OutputSizeLimit`Defer, + Function[{OutputSizeLimit`Dump`x$}, + MatrixForm[OutputSizeLimit`Dump`x$]]], 399, + 21942660866036203118, 5/2], + Enabled->True, + Evaluator->Automatic, + Method->"Queued"], + ButtonBox[ + PaneSelectorBox[{False-> + StyleBox[ + StyleBox[ + DynamicBox[ToBoxes[ + FEPrivate`FrontEndResource["FEStrings", "sizeShowMore"], + StandardForm], + ImageSizeCache->{36.94921875, {0., 5.}}], + StripOnInput->False, + DynamicUpdating->True, + LineSpacing->{1, 2}, + LineIndent->0, + LinebreakAdjustments->{1., 100, 0, 0, 0}], "OSLControl", + StripOnInput->False], True-> + StyleBox[ + StyleBox[ + DynamicBox[ToBoxes[ + FEPrivate`FrontEndResource["FEStrings", "sizeShowMore"], + StandardForm]], + StripOnInput->False, + DynamicUpdating->True, + LineSpacing->{1, 2}, + LineIndent->0, + LinebreakAdjustments->{1., 100, 0, 0, 0}], "OSLControlActive", + StripOnInput->False]}, Dynamic[ + CurrentValue["MouseOver"]], + Alignment->Center, + FrameMargins->0, + ImageSize->{Automatic, {25, Full}}], + Appearance->None, + BaselinePosition->Baseline, + ButtonFunction:>OutputSizeLimit`ButtonFunction[ + Composition[OutputSizeLimit`Defer, + Function[{OutputSizeLimit`Dump`x$}, + MatrixForm[OutputSizeLimit`Dump`x$]]], 399, + 21942660866036203118, 5 2], + Enabled->True, + Evaluator->Automatic, + Method->"Queued"], + ButtonBox[ + PaneSelectorBox[{False-> + StyleBox[ + StyleBox[ + DynamicBox[ToBoxes[ + FEPrivate`FrontEndResource["FEStrings", "sizeShowAll"], + StandardForm], + ImageSizeCache->{27.71484375, {0., 5.}}], + StripOnInput->False, + DynamicUpdating->True, + LineSpacing->{1, 2}, + LineIndent->0, + LinebreakAdjustments->{1., 100, 0, 0, 0}], "OSLControl", + StripOnInput->False], True-> + StyleBox[ + StyleBox[ + DynamicBox[ToBoxes[ + FEPrivate`FrontEndResource["FEStrings", "sizeShowAll"], + StandardForm]], + StripOnInput->False, + DynamicUpdating->True, + LineSpacing->{1, 2}, + LineIndent->0, + LinebreakAdjustments->{1., 100, 0, 0, 0}], "OSLControlActive", + StripOnInput->False]}, Dynamic[ + CurrentValue["MouseOver"]], + Alignment->Center, + FrameMargins->0, + ImageSize->{Automatic, {25, Full}}], + Appearance->None, + BaselinePosition->Baseline, + ButtonFunction:>OutputSizeLimit`ButtonFunction[ + Composition[OutputSizeLimit`Defer, + Function[{OutputSizeLimit`Dump`x$}, + MatrixForm[OutputSizeLimit`Dump`x$]]], 399, + 21942660866036203118, Infinity], + Enabled->True, + Evaluator->Automatic, + Method->"Queued"], + ButtonBox[ + PaneSelectorBox[{False-> + StyleBox[ + StyleBox[ + DynamicBox[ToBoxes[ + FEPrivate`FrontEndResource["FEStrings", "sizeChangeLimit"], + StandardForm], + ImageSizeCache->{49.91015625, {0., 5.}}], + StripOnInput->False, + DynamicUpdating->True, + LineSpacing->{1, 2}, + LineIndent->0, + LinebreakAdjustments->{1., 100, 0, 0, 0}], "OSLControl", + StripOnInput->False], True-> + StyleBox[ + StyleBox[ + DynamicBox[ToBoxes[ + FEPrivate`FrontEndResource["FEStrings", "sizeChangeLimit"], + StandardForm]], + StripOnInput->False, + DynamicUpdating->True, + LineSpacing->{1, 2}, + LineIndent->0, + LinebreakAdjustments->{1., 100, 0, 0, 0}], "OSLControlActive", + StripOnInput->False]}, Dynamic[ + CurrentValue["MouseOver"]], + Alignment->Center, + FrameMargins->0, + ImageSize->{Automatic, {25, Full}}], + Appearance->None, + BaselinePosition->Baseline, + ButtonFunction:>FrontEndExecute[{ + FrontEnd`SetOptions[ + FrontEnd`$FrontEnd, + FrontEnd`PreferencesSettings -> {"Page" -> "Advanced"}], + FrontEnd`FrontEndToken["PreferencesDialog"]}], + Evaluator->None, + Method->"Preemptive"]} + }, + AutoDelete->False, + FrameStyle->GrayLevel[0.85], + GridBoxDividers->{"Columns" -> {False, {True}}}, + GridBoxItemSize->{"Columns" -> {{Automatic}}, "Rows" -> {{Automatic}}}, + GridBoxSpacings->{"Columns" -> {{2}}}]} }, DefaultBaseStyle->"Column", - GridBoxAlignment->{ - "Columns" -> {{Left}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, - "RowsIndexed" -> {}}, - GridBoxDividers->{ - "Columns" -> {{False}}, "ColumnsIndexed" -> {}, "Rows" -> {{False}}, - "RowsIndexed" -> {}}, - GridBoxItemSize->{ - "Columns" -> {{Automatic}}, "ColumnsIndexed" -> {}, "Rows" -> {{1.}}, - "RowsIndexed" -> {}}, + GridBoxAlignment->{"Columns" -> {{Left}}, "Rows" -> {{Baseline}}}, + GridBoxDividers->{"Columns" -> {{False}}, "Rows" -> {{False}}}, + GridBoxItemSize->{"Columns" -> {{Automatic}}, "Rows" -> {{1.}}}, GridBoxSpacings->{"Columns" -> { Offset[0.27999999999999997`], { Offset[0.5599999999999999]}, - Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> { - Offset[0.2], { - Offset[1.2]}, - Offset[0.2]}, "RowsIndexed" -> {}}], - DefaultBaseStyle->{}, - FrameMargins->5], + Offset[0.27999999999999997`]}, "Rows" -> { + Offset[0.2], + Offset[1.2], { + Offset[0.4]}, + Offset[0.2]}}], + BaseStyle->"OutputSizeLimit", + FrameMargins->{{12, 12}, {0, 15}}, + FrameStyle->GrayLevel[0.85], + RoundingRadius->5, + StripOnInput->False], Deploy, DefaultBaseStyle->"Deploy"], - Out[87]]], "Output", + If[21942660866036203118 === $SessionID, + Out[399], Message[ + MessageName[Syntax, "noinfoker"]]; Missing["NotAvailable"]; + Null]]], "Output", CellChangeTimes->{3.617513305415209*^9, 3.617534761468522*^9, 3.6176911569914*^9, 3.618898389901362*^9, 3.618900775322711*^9, 3.619341342784012*^9, 3.619343236963545*^9, 3.6242549803603773`*^9, @@ -3426,8 +3769,11 @@ Cell[BoxData[ 3.697261543011924*^9, 3.697261721894086*^9, 3.697263751548497*^9, 3.752577595933364*^9, 3.752813571918913*^9, 3.7624941693843327`*^9, 3.763878009192663*^9, 3.823863577014786*^9, 3.823863700048583*^9, - 3.823933297226288*^9}, - ExpressionUUID -> "ed38f097-9b2e-4220-8549-bcbd0cd327da"] + 3.823933297191674*^9, 3.832654119186309*^9, 3.832654394639473*^9, + 3.832655338443295*^9, 3.832682723193851*^9, 3.832841944700376*^9}, + CellLabel-> + "Out[399]//MatrixForm=",ExpressionUUID->"6ca3cee9-19b8-4348-879b-\ +e870a2d8e793"] }, Open ]], Cell[BoxData[ @@ -3477,7 +3823,8 @@ Cell[BoxData[ 3.606039513926032*^9}, {3.6060396772814074`*^9, 3.606039677688698*^9}, { 3.606133864037573*^9, 3.606133864095793*^9}, 3.6075970058206778`*^9, 3.6075970496067266`*^9, {3.6176907420690403`*^9, 3.617690760496502*^9}}, - ExpressionUUID -> "14f9d837-7081-472b-a26e-d681ac2cf1aa"], + CellLabel-> + "In[400]:=",ExpressionUUID->"14f9d837-7081-472b-a26e-d681ac2cf1aa"], Cell[BoxData[ RowBox[{"\[IndentingNewLine]", @@ -3513,7 +3860,8 @@ Cell[BoxData[ 3.608439168246879*^9}, {3.608439800561326*^9, 3.608439807240604*^9}, { 3.6175131032034197`*^9, 3.617513120549831*^9}, {3.617513863501154*^9, 3.61751386401295*^9}, {3.6176907463460417`*^9, 3.617690749068235*^9}}, - ExpressionUUID -> "9579ced9-13a8-447f-82d0-84c2516d7db7"], + CellLabel-> + "In[402]:=",ExpressionUUID->"9579ced9-13a8-447f-82d0-84c2516d7db7"], Cell[CellGroupData[{ @@ -3522,7 +3870,8 @@ Cell[BoxData[{ RowBox[{"DispQ12hat", " ", "//", "MatrixForm"}]}], "Input", CellChangeTimes->{{3.6175131945170603`*^9, 3.617513206387601*^9}, { 3.6176908690359*^9, 3.6176908690970984`*^9}}, - ExpressionUUID -> "0894dfd6-f474-46d3-b15b-56dc4f477026"], + CellLabel-> + "In[404]:=",ExpressionUUID->"0894dfd6-f474-46d3-b15b-56dc4f477026"], Cell[BoxData[ TagBox[ @@ -3766,37 +4115,38 @@ Cell[BoxData[ RowBox[{"2", " ", RowBox[{"(", RowBox[{ - RowBox[{ - RowBox[{"-", - RowBox[{"(", - RowBox[{ - RowBox[{ - RowBox[{"qos", "[", "3", "]"}], " ", - RowBox[{"x1", "[", "4", "]"}]}], "-", - RowBox[{ - RowBox[{"qos", "[", "4", "]"}], " ", - RowBox[{"x1", "[", "5", "]"}]}], "+", - RowBox[{ - RowBox[{"qos", "[", "1", "]"}], " ", - RowBox[{"x1", "[", "6", "]"}]}], "+", - RowBox[{ - RowBox[{"qos", "[", "2", "]"}], " ", - RowBox[{"x1", "[", "7", "]"}]}]}], ")"}]}], " ", + RowBox[{"-", RowBox[{"(", RowBox[{ - RowBox[{ - RowBox[{"-", - RowBox[{"qos", "[", "2", "]"}]}], " ", - RowBox[{"x1", "[", "4", "]"}]}], "-", - RowBox[{ - RowBox[{"qos", "[", "1", "]"}], " ", - RowBox[{"x1", "[", "5", "]"}]}], "-", - RowBox[{ - RowBox[{"qos", "[", "4", "]"}], " ", - RowBox[{"x1", "[", "6", "]"}]}], "+", - RowBox[{ - RowBox[{"qos", "[", "3", "]"}], " ", - RowBox[{"x1", "[", "7", "]"}]}]}], ")"}]}], "-", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"qos", "[", "3", "]"}], " ", + RowBox[{"x1", "[", "4", "]"}]}], "-", + RowBox[{ + RowBox[{"qos", "[", "4", "]"}], " ", + RowBox[{"x1", "[", "5", "]"}]}], "+", + RowBox[{ + RowBox[{"qos", "[", "1", "]"}], " ", + RowBox[{"x1", "[", "6", "]"}]}], "+", + RowBox[{ + RowBox[{"qos", "[", "2", "]"}], " ", + RowBox[{"x1", "[", "7", "]"}]}]}], ")"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"-", + RowBox[{"qos", "[", "2", "]"}]}], " ", + RowBox[{"x1", "[", "4", "]"}]}], "-", + RowBox[{ + RowBox[{"qos", "[", "1", "]"}], " ", + RowBox[{"x1", "[", "5", "]"}]}], "-", + RowBox[{ + RowBox[{"qos", "[", "4", "]"}], " ", + RowBox[{"x1", "[", "6", "]"}]}], "+", + RowBox[{ + RowBox[{"qos", "[", "3", "]"}], " ", + RowBox[{"x1", "[", "7", "]"}]}]}], ")"}]}], ")"}]}], "-", RowBox[{ RowBox[{"(", RowBox[{ @@ -3885,37 +4235,38 @@ Cell[BoxData[ RowBox[{"2", " ", RowBox[{"(", RowBox[{ - RowBox[{ - RowBox[{"-", - RowBox[{"(", - RowBox[{ - RowBox[{ - RowBox[{"qos", "[", "3", "]"}], " ", - RowBox[{"x1", "[", "4", "]"}]}], "-", - RowBox[{ - RowBox[{"qos", "[", "4", "]"}], " ", - RowBox[{"x1", "[", "5", "]"}]}], "+", - RowBox[{ - RowBox[{"qos", "[", "1", "]"}], " ", - RowBox[{"x1", "[", "6", "]"}]}], "+", - RowBox[{ - RowBox[{"qos", "[", "2", "]"}], " ", - RowBox[{"x1", "[", "7", "]"}]}]}], ")"}]}], " ", + RowBox[{"-", RowBox[{"(", RowBox[{ - RowBox[{ - RowBox[{"-", - RowBox[{"qos", "[", "2", "]"}]}], " ", - RowBox[{"x1", "[", "4", "]"}]}], "-", - RowBox[{ - RowBox[{"qos", "[", "1", "]"}], " ", - RowBox[{"x1", "[", "5", "]"}]}], "-", - RowBox[{ - RowBox[{"qos", "[", "4", "]"}], " ", - RowBox[{"x1", "[", "6", "]"}]}], "+", - RowBox[{ - RowBox[{"qos", "[", "3", "]"}], " ", - RowBox[{"x1", "[", "7", "]"}]}]}], ")"}]}], "+", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"qos", "[", "3", "]"}], " ", + RowBox[{"x1", "[", "4", "]"}]}], "-", + RowBox[{ + RowBox[{"qos", "[", "4", "]"}], " ", + RowBox[{"x1", "[", "5", "]"}]}], "+", + RowBox[{ + RowBox[{"qos", "[", "1", "]"}], " ", + RowBox[{"x1", "[", "6", "]"}]}], "+", + RowBox[{ + RowBox[{"qos", "[", "2", "]"}], " ", + RowBox[{"x1", "[", "7", "]"}]}]}], ")"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"-", + RowBox[{"qos", "[", "2", "]"}]}], " ", + RowBox[{"x1", "[", "4", "]"}]}], "-", + RowBox[{ + RowBox[{"qos", "[", "1", "]"}], " ", + RowBox[{"x1", "[", "5", "]"}]}], "-", + RowBox[{ + RowBox[{"qos", "[", "4", "]"}], " ", + RowBox[{"x1", "[", "6", "]"}]}], "+", + RowBox[{ + RowBox[{"qos", "[", "3", "]"}], " ", + RowBox[{"x1", "[", "7", "]"}]}]}], ")"}]}], ")"}]}], "+", RowBox[{ RowBox[{"(", RowBox[{ @@ -4582,16 +4933,14 @@ Cell[BoxData[ RowBox[{"x2", "[", "6", "]"}], " ", RowBox[{"x2", "[", "7", "]"}]}]}], ")"}]}]}], ")"}]}]}]} }, - GridBoxAlignment->{ - "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, - "RowsIndexed" -> {}}, + GridBoxAlignment->{"Columns" -> {{Center}}, "Rows" -> {{Baseline}}}, GridBoxSpacings->{"Columns" -> { Offset[0.27999999999999997`], { Offset[0.5599999999999999]}, - Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> { + Offset[0.27999999999999997`]}, "Rows" -> { Offset[0.2], { Offset[0.4]}, - Offset[0.2]}, "RowsIndexed" -> {}}], + Offset[0.2]}}], Column], "\[NoBreak]", ")"}], Function[BoxForm`e$, MatrixForm[BoxForm`e$]]]], "Output", @@ -4601,8 +4950,12 @@ Cell[BoxData[ 3.633438238164608*^9, 3.63366960805031*^9, 3.634452669104021*^9, 3.697261793020834*^9, 3.6972638219676743`*^9, 3.752577683693405*^9, 3.752813643538809*^9, 3.762494199058545*^9, 3.763878031432008*^9, - 3.823863582287848*^9, 3.8238637041128187`*^9, 3.823933301061132*^9}, - ExpressionUUID -> "399dca12-6f73-42f7-b943-69abcccf01ed"], + 3.823863582287848*^9, 3.8238637041128187`*^9, 3.823933301061132*^9, + 3.832654122718917*^9, 3.832654397885722*^9, 3.832655340537537*^9, + 3.832682726775811*^9, 3.832841947974286*^9}, + CellLabel-> + "Out[404]//MatrixForm=",ExpressionUUID->"879808b0-e9e6-4155-9f2c-\ +8ef66dd10160"], Cell[BoxData[ TagBox[ @@ -4622,38 +4975,39 @@ Cell[BoxData[ RowBox[{ RowBox[{"x1", "[", "7", "]"}], " ", RowBox[{"x2", "[", "7", "]"}]}]}]}, - { - RowBox[{ - RowBox[{ - RowBox[{"-", - RowBox[{"(", - RowBox[{ - RowBox[{ - RowBox[{"qos", "[", "3", "]"}], " ", - RowBox[{"x1", "[", "4", "]"}]}], "-", - RowBox[{ - RowBox[{"qos", "[", "4", "]"}], " ", - RowBox[{"x1", "[", "5", "]"}]}], "+", - RowBox[{ - RowBox[{"qos", "[", "1", "]"}], " ", - RowBox[{"x1", "[", "6", "]"}]}], "+", - RowBox[{ - RowBox[{"qos", "[", "2", "]"}], " ", - RowBox[{"x1", "[", "7", "]"}]}]}], ")"}]}], " ", + { + RowBox[{ + RowBox[{"-", RowBox[{"(", RowBox[{ - RowBox[{ - RowBox[{"qos", "[", "4", "]"}], " ", - RowBox[{"x2", "[", "4", "]"}]}], "+", - RowBox[{ - RowBox[{"qos", "[", "3", "]"}], " ", - RowBox[{"x2", "[", "5", "]"}]}], "-", - RowBox[{ - RowBox[{"qos", "[", "2", "]"}], " ", - RowBox[{"x2", "[", "6", "]"}]}], "+", - RowBox[{ - RowBox[{"qos", "[", "1", "]"}], " ", - RowBox[{"x2", "[", "7", "]"}]}]}], ")"}]}], "-", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"qos", "[", "3", "]"}], " ", + RowBox[{"x1", "[", "4", "]"}]}], "-", + RowBox[{ + RowBox[{"qos", "[", "4", "]"}], " ", + RowBox[{"x1", "[", "5", "]"}]}], "+", + RowBox[{ + RowBox[{"qos", "[", "1", "]"}], " ", + RowBox[{"x1", "[", "6", "]"}]}], "+", + RowBox[{ + RowBox[{"qos", "[", "2", "]"}], " ", + RowBox[{"x1", "[", "7", "]"}]}]}], ")"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"qos", "[", "4", "]"}], " ", + RowBox[{"x2", "[", "4", "]"}]}], "+", + RowBox[{ + RowBox[{"qos", "[", "3", "]"}], " ", + RowBox[{"x2", "[", "5", "]"}]}], "-", + RowBox[{ + RowBox[{"qos", "[", "2", "]"}], " ", + RowBox[{"x2", "[", "6", "]"}]}], "+", + RowBox[{ + RowBox[{"qos", "[", "1", "]"}], " ", + RowBox[{"x2", "[", "7", "]"}]}]}], ")"}]}], ")"}]}], "-", RowBox[{ RowBox[{"(", RowBox[{ @@ -4745,37 +5099,38 @@ Cell[BoxData[ RowBox[{"x2", "[", "7", "]"}]}]}], ")"}]}]}]}, { RowBox[{ - RowBox[{ - RowBox[{"-", - RowBox[{"(", - RowBox[{ - RowBox[{ - RowBox[{"qos", "[", "2", "]"}], " ", - RowBox[{"x1", "[", "4", "]"}]}], "+", - RowBox[{ - RowBox[{"qos", "[", "1", "]"}], " ", - RowBox[{"x1", "[", "5", "]"}]}], "+", - RowBox[{ - RowBox[{"qos", "[", "4", "]"}], " ", - RowBox[{"x1", "[", "6", "]"}]}], "-", - RowBox[{ - RowBox[{"qos", "[", "3", "]"}], " ", - RowBox[{"x1", "[", "7", "]"}]}]}], ")"}]}], " ", + RowBox[{"-", RowBox[{"(", RowBox[{ - RowBox[{ - RowBox[{"-", - RowBox[{"qos", "[", "4", "]"}]}], " ", - RowBox[{"x2", "[", "4", "]"}]}], "-", - RowBox[{ - RowBox[{"qos", "[", "3", "]"}], " ", - RowBox[{"x2", "[", "5", "]"}]}], "+", - RowBox[{ - RowBox[{"qos", "[", "2", "]"}], " ", - RowBox[{"x2", "[", "6", "]"}]}], "-", - RowBox[{ - RowBox[{"qos", "[", "1", "]"}], " ", - RowBox[{"x2", "[", "7", "]"}]}]}], ")"}]}], "+", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"qos", "[", "2", "]"}], " ", + RowBox[{"x1", "[", "4", "]"}]}], "+", + RowBox[{ + RowBox[{"qos", "[", "1", "]"}], " ", + RowBox[{"x1", "[", "5", "]"}]}], "+", + RowBox[{ + RowBox[{"qos", "[", "4", "]"}], " ", + RowBox[{"x1", "[", "6", "]"}]}], "-", + RowBox[{ + RowBox[{"qos", "[", "3", "]"}], " ", + RowBox[{"x1", "[", "7", "]"}]}]}], ")"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"-", + RowBox[{"qos", "[", "4", "]"}]}], " ", + RowBox[{"x2", "[", "4", "]"}]}], "-", + RowBox[{ + RowBox[{"qos", "[", "3", "]"}], " ", + RowBox[{"x2", "[", "5", "]"}]}], "+", + RowBox[{ + RowBox[{"qos", "[", "2", "]"}], " ", + RowBox[{"x2", "[", "6", "]"}]}], "-", + RowBox[{ + RowBox[{"qos", "[", "1", "]"}], " ", + RowBox[{"x2", "[", "7", "]"}]}]}], ")"}]}], ")"}]}], "+", RowBox[{ RowBox[{"(", RowBox[{ @@ -4985,16 +5340,14 @@ Cell[BoxData[ RowBox[{"qos", "[", "4", "]"}], " ", RowBox[{"x2", "[", "7", "]"}]}]}], ")"}]}]}]} }, - GridBoxAlignment->{ - "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, - "RowsIndexed" -> {}}, + GridBoxAlignment->{"Columns" -> {{Center}}, "Rows" -> {{Baseline}}}, GridBoxSpacings->{"Columns" -> { Offset[0.27999999999999997`], { Offset[0.5599999999999999]}, - Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> { + Offset[0.27999999999999997`]}, "Rows" -> { Offset[0.2], { Offset[0.4]}, - Offset[0.2]}, "RowsIndexed" -> {}}], + Offset[0.2]}}], Column], "\[NoBreak]", ")"}], Function[BoxForm`e$, MatrixForm[BoxForm`e$]]]], "Output", @@ -5004,8 +5357,12 @@ Cell[BoxData[ 3.633438238164608*^9, 3.63366960805031*^9, 3.634452669104021*^9, 3.697261793020834*^9, 3.6972638219676743`*^9, 3.752577683693405*^9, 3.752813643538809*^9, 3.762494199058545*^9, 3.763878031432008*^9, - 3.823863582287848*^9, 3.8238637041128187`*^9, 3.823933301065151*^9}, - ExpressionUUID -> "399dca12-6f73-42f7-b943-69abcccf01ed"] + 3.823863582287848*^9, 3.8238637041128187`*^9, 3.823933301061132*^9, + 3.832654122718917*^9, 3.832654397885722*^9, 3.832655340537537*^9, + 3.832682726775811*^9, 3.832841947981436*^9}, + CellLabel-> + "Out[405]//MatrixForm=",ExpressionUUID->"f38ff185-74d0-4ced-8a97-\ +d1853e3821c0"] }, Open ]], Cell[BoxData[ @@ -5059,7 +5416,8 @@ Cell[BoxData[ 3.618924798414143*^9}, {3.619340667629931*^9, 3.619340677655244*^9}, { 3.624254728300384*^9, 3.624254729034965*^9}, 3.763877871437956*^9, 3.823863425417804*^9}, - ExpressionUUID -> "9db810eb-0c78-44dc-aec4-e68718871d82"], + CellLabel-> + "In[406]:=",ExpressionUUID->"9db810eb-0c78-44dc-aec4-e68718871d82"], Cell[BoxData[ RowBox[{ @@ -5075,7 +5433,8 @@ Cell[BoxData[ CellChangeTimes->{{3.6175137878325453`*^9, 3.61751382339027*^9}, { 3.617690787143221*^9, 3.617690818111946*^9}, 3.617690877134933*^9, { 3.619340673584573*^9, 3.619340674479411*^9}}, - ExpressionUUID -> "6301058d-c421-4eaa-b9cc-557560239001"], + CellLabel-> + "In[410]:=",ExpressionUUID->"6301058d-c421-4eaa-b9cc-557560239001"], Cell[CellGroupData[{ @@ -5085,7 +5444,8 @@ Cell[BoxData[ "\[IndentingNewLine]"}]], "Input", CellChangeTimes->{{3.617513886909363*^9, 3.617513897518958*^9}, { 3.617691403463037*^9, 3.617691416480743*^9}, 3.619340682383349*^9}, - ExpressionUUID -> "a2f92fb8-7fd6-40b4-b3b6-f1895b04c740"], + CellLabel-> + "In[411]:=",ExpressionUUID->"a2f92fb8-7fd6-40b4-b3b6-f1895b04c740"], Cell[BoxData[ TagBox[ @@ -5383,37 +5743,38 @@ Cell[BoxData[ RowBox[{"2", " ", RowBox[{"(", RowBox[{ - RowBox[{ - RowBox[{"-", - RowBox[{"(", - RowBox[{ - RowBox[{ - RowBox[{"qos", "[", "3", "]"}], " ", - RowBox[{"x1", "[", "4", "]"}]}], "-", - RowBox[{ - RowBox[{"qos", "[", "4", "]"}], " ", - RowBox[{"x1", "[", "5", "]"}]}], "+", - RowBox[{ - RowBox[{"qos", "[", "1", "]"}], " ", - RowBox[{"x1", "[", "6", "]"}]}], "+", - RowBox[{ - RowBox[{"qos", "[", "2", "]"}], " ", - RowBox[{"x1", "[", "7", "]"}]}]}], ")"}]}], " ", + RowBox[{"-", RowBox[{"(", RowBox[{ - RowBox[{ - RowBox[{"-", - RowBox[{"qos", "[", "2", "]"}]}], " ", - RowBox[{"x1", "[", "4", "]"}]}], "-", - RowBox[{ - RowBox[{"qos", "[", "1", "]"}], " ", - RowBox[{"x1", "[", "5", "]"}]}], "-", - RowBox[{ - RowBox[{"qos", "[", "4", "]"}], " ", - RowBox[{"x1", "[", "6", "]"}]}], "+", - RowBox[{ - RowBox[{"qos", "[", "3", "]"}], " ", - RowBox[{"x1", "[", "7", "]"}]}]}], ")"}]}], "-", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"qos", "[", "3", "]"}], " ", + RowBox[{"x1", "[", "4", "]"}]}], "-", + RowBox[{ + RowBox[{"qos", "[", "4", "]"}], " ", + RowBox[{"x1", "[", "5", "]"}]}], "+", + RowBox[{ + RowBox[{"qos", "[", "1", "]"}], " ", + RowBox[{"x1", "[", "6", "]"}]}], "+", + RowBox[{ + RowBox[{"qos", "[", "2", "]"}], " ", + RowBox[{"x1", "[", "7", "]"}]}]}], ")"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"-", + RowBox[{"qos", "[", "2", "]"}]}], " ", + RowBox[{"x1", "[", "4", "]"}]}], "-", + RowBox[{ + RowBox[{"qos", "[", "1", "]"}], " ", + RowBox[{"x1", "[", "5", "]"}]}], "-", + RowBox[{ + RowBox[{"qos", "[", "4", "]"}], " ", + RowBox[{"x1", "[", "6", "]"}]}], "+", + RowBox[{ + RowBox[{"qos", "[", "3", "]"}], " ", + RowBox[{"x1", "[", "7", "]"}]}]}], ")"}]}], ")"}]}], "-", RowBox[{ RowBox[{"(", RowBox[{ @@ -5671,37 +6032,38 @@ Cell[BoxData[ RowBox[{"2", " ", RowBox[{"(", RowBox[{ - RowBox[{ - RowBox[{"-", - RowBox[{"(", - RowBox[{ - RowBox[{ - RowBox[{"qos", "[", "3", "]"}], " ", - RowBox[{"x1", "[", "4", "]"}]}], "-", - RowBox[{ - RowBox[{"qos", "[", "4", "]"}], " ", - RowBox[{"x1", "[", "5", "]"}]}], "+", - RowBox[{ - RowBox[{"qos", "[", "1", "]"}], " ", - RowBox[{"x1", "[", "6", "]"}]}], "+", - RowBox[{ - RowBox[{"qos", "[", "2", "]"}], " ", - RowBox[{"x1", "[", "7", "]"}]}]}], ")"}]}], " ", + RowBox[{"-", RowBox[{"(", RowBox[{ - RowBox[{ - RowBox[{"-", - RowBox[{"qos", "[", "2", "]"}]}], " ", - RowBox[{"x1", "[", "4", "]"}]}], "-", - RowBox[{ - RowBox[{"qos", "[", "1", "]"}], " ", - RowBox[{"x1", "[", "5", "]"}]}], "-", - RowBox[{ - RowBox[{"qos", "[", "4", "]"}], " ", - RowBox[{"x1", "[", "6", "]"}]}], "+", - RowBox[{ - RowBox[{"qos", "[", "3", "]"}], " ", - RowBox[{"x1", "[", "7", "]"}]}]}], ")"}]}], "+", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"qos", "[", "3", "]"}], " ", + RowBox[{"x1", "[", "4", "]"}]}], "-", + RowBox[{ + RowBox[{"qos", "[", "4", "]"}], " ", + RowBox[{"x1", "[", "5", "]"}]}], "+", + RowBox[{ + RowBox[{"qos", "[", "1", "]"}], " ", + RowBox[{"x1", "[", "6", "]"}]}], "+", + RowBox[{ + RowBox[{"qos", "[", "2", "]"}], " ", + RowBox[{"x1", "[", "7", "]"}]}]}], ")"}], " ", + RowBox[{"(", + RowBox[{ + RowBox[{ + RowBox[{"-", + RowBox[{"qos", "[", "2", "]"}]}], " ", + RowBox[{"x1", "[", "4", "]"}]}], "-", + RowBox[{ + RowBox[{"qos", "[", "1", "]"}], " ", + RowBox[{"x1", "[", "5", "]"}]}], "-", + RowBox[{ + RowBox[{"qos", "[", "4", "]"}], " ", + RowBox[{"x1", "[", "6", "]"}]}], "+", + RowBox[{ + RowBox[{"qos", "[", "3", "]"}], " ", + RowBox[{"x1", "[", "7", "]"}]}]}], ")"}]}], ")"}]}], "+", RowBox[{ RowBox[{"(", RowBox[{ @@ -6391,16 +6753,14 @@ Cell[BoxData[ RowBox[{"x2", "[", "7", "]"}]}]}], ")"}]}]}], ")"}]}]}], ")"}]}]}]} }, - GridBoxAlignment->{ - "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, - "RowsIndexed" -> {}}, + GridBoxAlignment->{"Columns" -> {{Center}}, "Rows" -> {{Baseline}}}, GridBoxSpacings->{"Columns" -> { Offset[0.27999999999999997`], { Offset[0.5599999999999999]}, - Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> { + Offset[0.27999999999999997`]}, "Rows" -> { Offset[0.2], { Offset[0.4]}, - Offset[0.2]}, "RowsIndexed" -> {}}], + Offset[0.2]}}], Column], "\[NoBreak]", ")"}], Function[BoxForm`e$, MatrixForm[BoxForm`e$]]]], "Output", @@ -6411,8 +6771,12 @@ Cell[BoxData[ 3.63366968448136*^9, 3.634452757344584*^9, 3.6972618932705383`*^9, 3.69726391548508*^9, 3.752577783747003*^9, 3.752813739644515*^9, 3.762494241756254*^9, 3.763878069441881*^9, 3.823863597136351*^9, - 3.823863712624889*^9, 3.823933309556869*^9}, - ExpressionUUID -> "345a98cd-0571-4bd2-8226-78346c8b4df0"] + 3.823863712624889*^9, 3.823933309556869*^9, 3.8326541303319387`*^9, + 3.8326544054209538`*^9, 3.832655346172598*^9, 3.8326827354340057`*^9, + 3.832841956154338*^9}, + CellLabel-> + "Out[411]//MatrixForm=",ExpressionUUID->"11acf96c-aee3-419f-927c-\ +5c74cd5ad370"] }, Open ]], Cell[BoxData[ @@ -6427,13 +6791,18 @@ Cell[BoxData[ RowBox[{"{", RowBox[{ "POSEShat", ",", "QShat", ",", "VShat", ",", "WShat", ",", "AShat", ",", - "AlphaShat", ",", "Disphat", ",", "DispQ12hat", ",", "IMUintdPhat"}], + "AlphaShat", ",", "Disphat", ",", "DispQ12hat", ",", "IMUintdPhat", + ",", " ", "QSprevhat", ",", " ", "VSprevhat", ",", " ", "WSprevhat"}], "}"}], "]"}]}], ";"}]}]], "Input", CellChangeTimes->{{3.606028123449987*^9, 3.6060281663918123`*^9}, { 3.606037951571478*^9, 3.6060379542304783`*^9}, {3.617514032714843*^9, 3.6175140431209106`*^9}, {3.617690851858078*^9, 3.617690854334939*^9}, - 3.617690889109269*^9, 3.7525781289581137`*^9}, - ExpressionUUID -> "772da980-73d4-4177-9a96-6c731ff9d20a"], + 3.617690889109269*^9, 3.7525781289581137`*^9, {3.832655005323621*^9, + 3.8326550095708027`*^9}, 3.8326552104539013`*^9, {3.8326822248723307`*^9, + 3.832682229431615*^9}, {3.832682286551252*^9, 3.8326822948313313`*^9}, { + 3.83284169788309*^9, 3.832841711922678*^9}}, + CellLabel-> + "In[412]:=",ExpressionUUID->"772da980-73d4-4177-9a96-6c731ff9d20a"], Cell["Constant speed predictor", "Subsubtitle", CellChangeTimes->{{3.571119620568287*^9, 3.571119644637387*^9}, { @@ -6449,7 +6818,8 @@ Cell[BoxData[ "from", " ", "x0", " ", "and", " ", "x1", " ", "and", " ", "predict", " ", "x2"}], " ", "*)"}]], "Input", CellChangeTimes->{{3.582622986495626*^9, 3.582623008115547*^9}}, - ExpressionUUID -> "2b65f9f5-73dc-4068-8969-212ca637e4c2"], + CellLabel-> + "In[413]:=",ExpressionUUID->"2b65f9f5-73dc-4068-8969-212ca637e4c2"], Cell[BoxData[{ RowBox[{ @@ -6476,7 +6846,8 @@ Cell[BoxData[{ "Simplify"}]}], ";"}]}], "Input", CellChangeTimes->{{3.582623016608653*^9, 3.5826230914040956`*^9}, { 3.608356558256617*^9, 3.608356558258312*^9}}, - ExpressionUUID -> "e462403b-c716-4eee-91c5-c7dbfdbd6e2d"], + CellLabel-> + "In[414]:=",ExpressionUUID->"e462403b-c716-4eee-91c5-c7dbfdbd6e2d"], Cell[BoxData[{ RowBox[{ @@ -6504,7 +6875,8 @@ Cell[BoxData[{ 3.582622199941121*^9, 3.5826222706516314`*^9}, {3.582622907688993*^9, 3.582622913927534*^9}, {3.582623109813613*^9, 3.582623125613282*^9}, { 3.6083565582643433`*^9, 3.608356558265799*^9}}, - ExpressionUUID -> "f49bbb81-1821-409e-9a7c-4648040b18ce"], + CellLabel-> + "In[416]:=",ExpressionUUID->"f49bbb81-1821-409e-9a7c-4648040b18ce"], Cell[BoxData[ RowBox[{ @@ -6516,7 +6888,8 @@ Cell[BoxData[ CellChangeTimes->{{3.58262229372995*^9, 3.582622309444421*^9}, { 3.582623149387439*^9, 3.582623159843779*^9}, {3.6206518445954037`*^9, 3.620651880174917*^9}}, - ExpressionUUID -> "ed7721db-b1a2-4a8e-ba0a-fd8e03d91e5a"], + CellLabel-> + "In[418]:=",ExpressionUUID->"ed7721db-b1a2-4a8e-ba0a-fd8e03d91e5a"], Cell[CellGroupData[{ @@ -6535,7 +6908,8 @@ Cell[BoxData[ RowBox[{"{", RowBox[{"i", ",", "1", ",", "6"}], "}"}]}], "]"}]}], ";"}]], "Input", CellChangeTimes->{{3.5731959836293488`*^9, 3.5731959990488377`*^9}}, - ExpressionUUID -> "22c85d01-74a2-4ab0-aadc-9f415f2e5625"] + CellLabel-> + "In[419]:=",ExpressionUUID->"22c85d01-74a2-4ab0-aadc-9f415f2e5625"] }, Open ]], Cell[CellGroupData[{ @@ -6559,7 +6933,8 @@ Cell[BoxData[{ 3.697264966198505*^9}, {3.697265465728429*^9, 3.697265476927476*^9}, { 3.6972664921623087`*^9, 3.69726649720623*^9}, {3.697268337243885*^9, 3.697268383994207*^9}, {3.697280139369082*^9, 3.697280143115209*^9}}, - ExpressionUUID -> "a77efe4d-2c92-466a-9ee5-09bee4330c9c"], + CellLabel-> + "In[420]:=",ExpressionUUID->"a77efe4d-2c92-466a-9ee5-09bee4330c9c"], Cell[BoxData[{ RowBox[{ @@ -6716,6 +7091,51 @@ Cell[BoxData[{ RowBox[{"27", ",", "28", ",", "29"}], "}"}], "]"}], "]"}], ",", "X0", ",", "X0pert"}], "]"}], ",", RowBox[{"{", "dX", "}"}]}], "]"}], ",", "dX"}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JAugQprevX0", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{ + RowBox[{"SubVec", "[", + RowBox[{ + RowBox[{"AugX", "[", + RowBox[{"[", + RowBox[{"{", + RowBox[{"30", ",", "31", ",", "32", ",", " ", "33"}], "}"}], + "]"}], "]"}], ",", "X0", ",", "X0pert"}], "]"}], ",", + RowBox[{"{", "dX", "}"}]}], "]"}], ",", "dX"}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JAugVprevX0", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{ + RowBox[{"SubVec", "[", + RowBox[{ + RowBox[{"AugX", "[", + RowBox[{"[", + RowBox[{"{", + RowBox[{"34", ",", "35", ",", "36"}], "}"}], "]"}], "]"}], ",", + "X0", ",", "X0pert"}], "]"}], ",", + RowBox[{"{", "dX", "}"}]}], "]"}], ",", "dX"}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JAugWprevX0", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{ + RowBox[{"SubVec", "[", + RowBox[{ + RowBox[{"AugX", "[", + RowBox[{"[", + RowBox[{"{", + RowBox[{"37", ",", "38", ",", "39"}], "}"}], "]"}], "]"}], ",", + "X0", ",", "X0pert"}], "]"}], ",", + RowBox[{"{", "dX", "}"}]}], "]"}], ",", "dX"}], "]"}]}], ";"}]}], "Input", CellChangeTimes->{{3.573195979383194*^9, 3.573196203434111*^9}, { 3.573196262229154*^9, 3.5731962781558523`*^9}, {3.573196311076292*^9, @@ -6726,8 +7146,9 @@ Cell[BoxData[{ 3.60603953780797*^9}, {3.6175142107055597`*^9, 3.6175142678093843`*^9}, { 3.619340701411696*^9, 3.619340702356639*^9}, {3.697260742190899*^9, 3.6972607931361847`*^9}, {3.6972613326433773`*^9, 3.6972613738053293`*^9}, - 3.697266561644383*^9}, - ExpressionUUID -> "b23ded5d-856b-4152-b45f-9afd0cab6abb"], + 3.697266561644383*^9, {3.832681807491168*^9, 3.832681911825362*^9}}, + CellLabel-> + "In[422]:=",ExpressionUUID->"b23ded5d-856b-4152-b45f-9afd0cab6abb"], Cell[BoxData[{ RowBox[{ @@ -6884,6 +7305,51 @@ Cell[BoxData[{ RowBox[{"27", ",", "28", ",", "29"}], "}"}], "]"}], "]"}], ",", "X1", ",", "X1pert"}], "]"}], ",", RowBox[{"{", "dX", "}"}]}], "]"}], ",", "dX"}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JAugQprevX1", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{ + RowBox[{"SubVec", "[", + RowBox[{ + RowBox[{"AugX", "[", + RowBox[{"[", + RowBox[{"{", + RowBox[{"30", ",", "31", ",", "32", ",", " ", "33"}], "}"}], + "]"}], "]"}], ",", "X1", ",", "X1pert"}], "]"}], ",", + RowBox[{"{", "dX", "}"}]}], "]"}], ",", "dX"}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JAugVprevX1", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{ + RowBox[{"SubVec", "[", + RowBox[{ + RowBox[{"AugX", "[", + RowBox[{"[", + RowBox[{"{", + RowBox[{"34", ",", "35", ",", "36"}], "}"}], "]"}], "]"}], ",", + "X1", ",", "X1pert"}], "]"}], ",", + RowBox[{"{", "dX", "}"}]}], "]"}], ",", "dX"}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JAugWprevX1", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{ + RowBox[{"SubVec", "[", + RowBox[{ + RowBox[{"AugX", "[", + RowBox[{"[", + RowBox[{"{", + RowBox[{"37", ",", "38", ",", "39"}], "}"}], "]"}], "]"}], ",", + "X1", ",", "X1pert"}], "]"}], ",", + RowBox[{"{", "dX", "}"}]}], "]"}], ",", "dX"}], "]"}]}], ";"}]}], "Input", CellChangeTimes->{{3.5731963813541117`*^9, 3.573196513444121*^9}, { 3.573196547924096*^9, 3.5731965498503838`*^9}, 3.573197904244328*^9, { @@ -6894,8 +7360,10 @@ Cell[BoxData[{ 3.606039786659706*^9, 3.606039795752778*^9}, {3.6175142844142*^9, 3.617514297683299*^9}, {3.617604720960475*^9, 3.617604722154723*^9}, { 3.6193407052367983`*^9, 3.61934070603651*^9}, 3.697260744444146*^9, { - 3.6972613765325403`*^9, 3.69726139078487*^9}}, - ExpressionUUID -> "afea1643-327e-49c5-8654-688a95d46acb"], + 3.6972613765325403`*^9, 3.69726139078487*^9}, {3.8326819247212973`*^9, + 3.8326819503286657`*^9}}, + CellLabel-> + "In[435]:=",ExpressionUUID->"afea1643-327e-49c5-8654-688a95d46acb"], Cell[BoxData[{ RowBox[{ @@ -7034,12 +7502,57 @@ Cell[BoxData[{ RowBox[{"AugX", "[", RowBox[{"[", RowBox[{"{", - RowBox[{"23", ",", "24", ",", "25", ",", "26"}], "}"}], "]"}], - "]"}], ",", "X2", ",", "X2pert"}], "]"}], ",", + RowBox[{"23", ",", "24", ",", "25", ",", "26"}], "}"}], "]"}], + "]"}], ",", "X2", ",", "X2pert"}], "]"}], ",", + RowBox[{"{", "dX", "}"}]}], "]"}], ",", "dX"}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JAugIMUintdPX2", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{ + RowBox[{"SubVec", "[", + RowBox[{ + RowBox[{"AugX", "[", + RowBox[{"[", + RowBox[{"{", + RowBox[{"27", ",", "28", ",", "29"}], "}"}], "]"}], "]"}], ",", + "X2", ",", "X2pert"}], "]"}], ",", + RowBox[{"{", "dX", "}"}]}], "]"}], ",", "dX"}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JAugQprevX2", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{ + RowBox[{"SubVec", "[", + RowBox[{ + RowBox[{"AugX", "[", + RowBox[{"[", + RowBox[{"{", + RowBox[{"30", ",", "31", ",", "32", ",", " ", "33"}], "}"}], + "]"}], "]"}], ",", "X2", ",", "X2pert"}], "]"}], ",", + RowBox[{"{", "dX", "}"}]}], "]"}], ",", "dX"}], "]"}]}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JAugVprevX2", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{ + RowBox[{"SubVec", "[", + RowBox[{ + RowBox[{"AugX", "[", + RowBox[{"[", + RowBox[{"{", + RowBox[{"34", ",", "35", ",", "36"}], "}"}], "]"}], "]"}], ",", + "X2", ",", "X2pert"}], "]"}], ",", RowBox[{"{", "dX", "}"}]}], "]"}], ",", "dX"}], "]"}]}], ";"}], "\[IndentingNewLine]", RowBox[{ - RowBox[{"JAugIMUintdPX2", "=", + RowBox[{"JAugWprevX2", "=", RowBox[{"VecToZero", "[", RowBox[{ RowBox[{"D", "[", @@ -7049,7 +7562,7 @@ Cell[BoxData[{ RowBox[{"AugX", "[", RowBox[{"[", RowBox[{"{", - RowBox[{"27", ",", "28", ",", "29"}], "}"}], "]"}], "]"}], ",", + RowBox[{"37", ",", "38", ",", "39"}], "}"}], "]"}], "]"}], ",", "X2", ",", "X2pert"}], "]"}], ",", RowBox[{"{", "dX", "}"}]}], "]"}], ",", "dX"}], "]"}]}], ";"}]}], "Input", @@ -7061,8 +7574,10 @@ Cell[BoxData[{ 3.606039635988667*^9, 3.606039642750998*^9}, {3.6060397982602367`*^9, 3.606039803419691*^9}, {3.617514302054162*^9, 3.617514312395774*^9}, { 3.619340708972601*^9, 3.6193407098153*^9}, 3.697260746470517*^9, { - 3.6972613830605783`*^9, 3.6972613955072813`*^9}}, - ExpressionUUID -> "fef7a11e-1dd8-4c10-885e-c6924832ad33"] + 3.6972613830605783`*^9, 3.6972613955072813`*^9}, {3.83268195912046*^9, + 3.832681973481064*^9}}, + CellLabel-> + "In[448]:=",ExpressionUUID->"fef7a11e-1dd8-4c10-885e-c6924832ad33"] }, Open ]], Cell[CellGroupData[{ @@ -7099,7 +7614,8 @@ Cell[BoxData[{ 3.573205806544126*^9, 3.573205814453236*^9}, {3.7624932188665953`*^9, 3.7624932277510653`*^9}, {3.7624933067141523`*^9, 3.7624933578854218`*^9}, { 3.762493388152514*^9, 3.762493394246661*^9}}, - ExpressionUUID -> "bddae2f3-5491-4f6b-a59f-80093e439436"], + CellLabel-> + "In[461]:=",ExpressionUUID->"bddae2f3-5491-4f6b-a59f-80093e439436"], Cell[BoxData[{ RowBox[{ @@ -7263,9 +7779,65 @@ Cell[BoxData[{ RowBox[{"dX", "[", RowBox[{"[", RowBox[{"1", ";;", "3"}], "]"}], "]"}], "}"}]}], "]"}], ",", "dX"}], + "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JAugQprevSO", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{ + RowBox[{"SubVec", "[", + RowBox[{ + RowBox[{"AugX", "[", + RowBox[{"[", + RowBox[{"{", + RowBox[{"30", ",", "31", ",", "32", ",", " ", "33"}], "}"}], + "]"}], "]"}], ",", "S", ",", "Spert"}], "]"}], ",", + RowBox[{"{", + RowBox[{"dX", "[", + RowBox[{"[", + RowBox[{"1", ";;", "3"}], "]"}], "]"}], "}"}]}], "]"}], ",", "dX"}], + "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JAugVprevSO", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{ + RowBox[{"SubVec", "[", + RowBox[{ + RowBox[{"AugX", "[", + RowBox[{"[", + RowBox[{"{", + RowBox[{"34", ",", "35", ",", "36"}], "}"}], "]"}], "]"}], ",", + "S", ",", "Spert"}], "]"}], ",", + RowBox[{"{", + RowBox[{"dX", "[", + RowBox[{"[", + RowBox[{"1", ";;", "3"}], "]"}], "]"}], "}"}]}], "]"}], ",", "dX"}], + "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JAugWprevSO", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{ + RowBox[{"SubVec", "[", + RowBox[{ + RowBox[{"AugX", "[", + RowBox[{"[", + RowBox[{"{", + RowBox[{"37", ",", "38", ",", "39"}], "}"}], "]"}], "]"}], ",", + "S", ",", "Spert"}], "]"}], ",", + RowBox[{"{", + RowBox[{"dX", "[", + RowBox[{"[", + RowBox[{"1", ";;", "3"}], "]"}], "]"}], "}"}]}], "]"}], ",", "dX"}], "]"}]}], ";"}]}], "Input", - CellChangeTimes->{{3.762494328667454*^9, 3.7624943286687183`*^9}}, - ExpressionUUID -> "6c12f5eb-34c9-4c10-be38-8da6426dc669"], + CellChangeTimes->{{3.762494328667454*^9, 3.7624943286687183`*^9}, { + 3.832681979994532*^9, 3.832682029162084*^9}}, + CellLabel-> + "In[463]:=",ExpressionUUID->"6c12f5eb-34c9-4c10-be38-8da6426dc669"], Cell[BoxData[{ RowBox[{ @@ -7429,9 +8001,66 @@ Cell[BoxData[{ RowBox[{"dX", "[", RowBox[{"[", RowBox[{"4", ";;", "6"}], "]"}], "]"}], "}"}]}], "]"}], ",", "dX"}], + "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JAugQprevqOS", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{ + RowBox[{"SubVec", "[", + RowBox[{ + RowBox[{"AugX", "[", + RowBox[{"[", + RowBox[{"{", + RowBox[{"30", ",", "31", ",", "32", ",", " ", "33"}], "}"}], + "]"}], "]"}], ",", "S", ",", "Spert"}], "]"}], ",", + RowBox[{"{", + RowBox[{"dX", "[", + RowBox[{"[", + RowBox[{"4", ";;", "6"}], "]"}], "]"}], "}"}]}], "]"}], ",", "dX"}], + "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JAugVprevqOS", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{ + RowBox[{"SubVec", "[", + RowBox[{ + RowBox[{"AugX", "[", + RowBox[{"[", + RowBox[{"{", + RowBox[{"34", ",", "35", ",", "36"}], "}"}], "]"}], "]"}], ",", + "S", ",", "Spert"}], "]"}], ",", + RowBox[{"{", + RowBox[{"dX", "[", + RowBox[{"[", + RowBox[{"4", ";;", "6"}], "]"}], "]"}], "}"}]}], "]"}], ",", "dX"}], + "]"}]}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"JAugWprevqOS", "=", + RowBox[{"VecToZero", "[", + RowBox[{ + RowBox[{"D", "[", + RowBox[{ + RowBox[{"SubVec", "[", + RowBox[{ + RowBox[{"AugX", "[", + RowBox[{"[", + RowBox[{"{", + RowBox[{"37", ",", "38", ",", "39"}], "}"}], "]"}], "]"}], ",", + "S", ",", "Spert"}], "]"}], ",", + RowBox[{"{", + RowBox[{"dX", "[", + RowBox[{"[", + RowBox[{"4", ";;", "6"}], "]"}], "]"}], "}"}]}], "]"}], ",", "dX"}], "]"}]}], ";"}]}], "Input", - CellChangeTimes->{{3.7624943593708887`*^9, 3.762494359371443*^9}}, - ExpressionUUID -> "ca659b6d-dc91-461a-943e-607d3f607ebf"], + CellChangeTimes->{{3.7624943593708887`*^9, 3.762494359371443*^9}, { + 3.8326820431400223`*^9, 3.832682043660741*^9}, {3.83268214488135*^9, + 3.832682172880458*^9}}, + CellLabel-> + "In[475]:=",ExpressionUUID->"ca659b6d-dc91-461a-943e-607d3f607ebf"], Cell[BoxData[{ RowBox[{ @@ -7442,7 +8071,8 @@ Cell[BoxData[{ RowBox[{"Run", "[", "\"\\"", "]"}], ";"}]}], "Input", CellChangeTimes->{{3.606038736553583*^9, 3.606038756025543*^9}, 3.6193407949456587`*^9}, - ExpressionUUID -> "01e81ac5-1851-49ba-a288-48dc7163f39d"], + CellLabel-> + "In[487]:=",ExpressionUUID->"01e81ac5-1851-49ba-a288-48dc7163f39d"], Cell[BoxData[ RowBox[{"(*", @@ -7468,7 +8098,8 @@ BackwardAugmentedStateEstimator_v7.mthout xhat 1 1\>\"", "]"}], ";"}], 3.7525802166767282`*^9, 3.752580217601409*^9}, {3.7624936980552588`*^9, 3.762493724388318*^9}, {3.762493999543098*^9, 3.762494001523481*^9}, { 3.762494696180825*^9, 3.762494701645299*^9}, 3.8238634386088963`*^9}, - ExpressionUUID -> "660027f3-dbe7-4552-8f1f-be06da3c7f7d"], + CellLabel-> + "In[489]:=",ExpressionUUID->"660027f3-dbe7-4552-8f1f-be06da3c7f7d"], Cell[BoxData[ RowBox[{ @@ -7531,6 +8162,18 @@ Cell[BoxData[ RowBox[{"ToGoodC", "[", "IMUintdPhat", "]"}], ",", "\"\\""}], "]"}], ";"}], "\[IndentingNewLine]", "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "VSprevhat", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "WSprevhat", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", "\[IndentingNewLine]", RowBox[{ RowBox[{ "Run", "[", @@ -7584,6 +8227,18 @@ BackwardAugmentedStateEstimator_v7_DispQ.mthout part 0 1\>\"", "]"}], ";"}], "Run", "[", "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], ";"}], + "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], ";"}]}]}]], "Input", CellChangeTimes->{{3.5731955350800858`*^9, 3.57319554397292*^9}, { 3.5731999514722853`*^9, 3.5731999515541143`*^9}, {3.573213491032804*^9, @@ -7601,8 +8256,11 @@ BackwardAugmentedStateEstimator_v7_IMUintdP.mthout part 0 1\>\"", "]"}], 3.617514592406671*^9, 3.617514683455076*^9}, {3.6176914396881533`*^9, 3.617691439838238*^9}, 3.617691472327279*^9, {3.619340802512971*^9, 3.619340809453024*^9}, {3.76249370460083*^9, 3.7624937151094427`*^9}, { - 3.823863438609713*^9, 3.823863438613782*^9}}, - ExpressionUUID -> "a6871df0-153c-4955-b454-6dec0f8aac6a"], + 3.823863438609713*^9, 3.823863438613782*^9}, {3.832653978058915*^9, + 3.832653989539254*^9}, {3.832654234500708*^9, 3.832654279867391*^9}, + 3.832654796494247*^9, {3.832841802884437*^9, 3.832841812763397*^9}}, + CellLabel-> + "In[490]:=",ExpressionUUID->"a6871df0-153c-4955-b454-6dec0f8aac6a"], Cell[BoxData[ RowBox[{ @@ -7673,6 +8331,24 @@ Cell[BoxData[ RowBox[{ RowBox[{"ToGoodC", "[", "JAugIMUintdPX2", "]"}], ",", "\"\\""}], + "]"}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JAugQprevX2", "]"}], ",", + "\"\\""}], + "]"}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JAugVprevX2", "]"}], ",", + "\"\\""}], + "]"}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JAugWprevX2", "]"}], ",", + "\"\\""}], "]"}], ";"}], "\[IndentingNewLine]", "\[IndentingNewLine]", RowBox[{ RowBox[{ @@ -7723,27 +8399,47 @@ BackwardAugmentedStateEstimator_v7_JAugDispX2.mthout J 0 1\>\"", "]"}], ";"}], BackwardAugmentedStateEstimator_v7_JAugDispQX2.mthout J 0 1\>\"", "]"}], ";"}]}]}]], "Input", CellChangeTimes->CompressedData[" -1:eJxTTMoPSmViYGAQBWIQzabg2hQ2/5XjkZiwCWD6vsouEL1DRHgPiJ7y3eYa +1:eJxTTMoPSmViYGAQA2IQzabg2hQ2/5XjkZiwCWD6vsouEL1DRHgPiJ7y3eYa iNYLywfT5y7lK8YB6YuTy8H0oqc/zH8teOUoWWJiAaLNJrh3SVx95ch8qB5M v6sMOPfj7ivHQJ9AMP3I1+YeiP7zMAZMM6RYPwLRd+9dA9MKbCIfQLR4b+ln EM0bfEv0J5COyfovBqJvlX7RBtHXCiV0QHSHj/4/sfuvHK/wG4DpI64PQlMf vXJUzXgIptk8XqlPBdICvkwaIHr6aqVJdz6/cmRdqgym8y0VZ4Lof/81wLSe heZcEP17785FIPrszWqniO+vHJsEEp1BdM/uW0rJHG8cBb66KYPoO+svXyyo -feOYdANCAwBSj7/U +feOYdANCr7gg9tap742jnqz+JxANAJdlx7Y= "], - ExpressionUUID -> "b71d9672-279a-4123-ab53-1e3075234465"], + CellLabel-> + "In[512]:=",ExpressionUUID->"b71d9672-279a-4123-ab53-1e3075234465"], -Cell[BoxData[ +Cell[BoxData[{ RowBox[{ RowBox[{ - RowBox[{ - "Run", "[", - "\"\\"", "]"}], - ";"}], "\[IndentingNewLine]"}]], "Input", + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]"}], "Input", CellChangeTimes->{{3.61751531187344*^9, 3.617515327677194*^9}, - 3.619340832565092*^9, 3.762493722412959*^9, 3.823863438621318*^9}, - ExpressionUUID -> "6498545b-7a32-4bdd-bcba-16b669221638"], + 3.619340832565092*^9, 3.762493722412959*^9, 3.823863438621318*^9, { + 3.832682427823539*^9, 3.8326824439032307`*^9}}, + CellLabel-> + "In[534]:=",ExpressionUUID->"6498545b-7a32-4bdd-bcba-16b669221638"], Cell[BoxData[{ RowBox[{ @@ -7794,13 +8490,31 @@ Cell[BoxData[{ RowBox[{"ToGoodC", "[", "JAugDISPQX1", "]"}], ",", "\"\\""}], "]"}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JAugIMUintdPX1", "]"}], ",", + "\"\\""}], + "]"}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JAugQprevX1", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JAugVprevX1", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", RowBox[{ RowBox[{ RowBox[{"MyStringWrite", "[", RowBox[{ - RowBox[{"ToGoodC", "[", "JAugIMUintdPX1", "]"}], ",", - "\"\\""}], - "]"}], ";"}], "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{"ToGoodC", "[", "JAugWprevX1", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]"}], "\[IndentingNewLine]", RowBox[{ RowBox[{ "Run", "[", @@ -7854,16 +8568,36 @@ BackwardAugmentedStateEstimator_v7_JAugDispQX1.mthout J 0 1\>\"", "]"}], "Run", "[", "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], ";"}], "\[IndentingNewLine]"}], "Input", CellChangeTimes->CompressedData[" -1:eJxTTMoPSmViYGAQBmIQzabg2hQ2/5XjkZiwCWD6vsouEL1DRHgPiJ7y3eYa +1:eJxTTMoPSmViYGAQBWIQzabg2hQ2/5XjkZiwCWD6vsouEL1DRHgPiJ7y3eYa iNYLywfTp8QfakcCaSubl2DaxqBCMQ5Ih4TUgemoHZYWvxa8cnylVASma/62 dElcfeX4zHAvmBaLCDz34+4rR5XfEDpB5N5zED3roP8bEH1pjtZHEN14x/Yb iDZYL6vzE0ibb3EE09I6Bv/E7r9ybHsBoa/UPwxNffTK8YfsIzA955DypDuf XznKzlMB018kniwG0cGiD5eAaBMLj5cg2ob5NZhur97kEvH9leM25t2uIPrD -z2DlZI43jnLTOsD0t/eXLxbUvnF04L4CpgGDl6s2 +z2DlZI43jnLTOsD0t/eXLxbUvnF04L4Cphlj8z479b1xrEje8AVE8zBd+w2i +wy4t+gOiAfZ7vA8= "], - ExpressionUUID -> "905004a8-9759-44c4-afdc-b3ff9d6b889c"], + CellLabel-> + "In[538]:=",ExpressionUUID->"905004a8-9759-44c4-afdc-b3ff9d6b889c"], Cell[BoxData[{ RowBox[{ @@ -7914,13 +8648,31 @@ Cell[BoxData[{ RowBox[{"ToGoodC", "[", "JAugDISPQX0", "]"}], ",", "\"\\""}], "]"}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JAugIMUintdPX0", "]"}], ",", + "\"\\""}], + "]"}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JAugQprevX0", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JAugVprevX0", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", RowBox[{ RowBox[{ RowBox[{"MyStringWrite", "[", RowBox[{ - RowBox[{"ToGoodC", "[", "JAugIMUintdPX0", "]"}], ",", - "\"\\""}], - "]"}], ";"}], "\[IndentingNewLine]"}], "\[IndentingNewLine]", + RowBox[{"ToGoodC", "[", "JAugWprevX0", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]"}], "\[IndentingNewLine]", RowBox[{ RowBox[{ "Run", "[", @@ -7974,16 +8726,36 @@ BackwardAugmentedStateEstimator_v7_JAugDispQX0.mthout J 0 1\>\"", "]"}], "Run", "[", "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], ";"}]}], "Input", CellChangeTimes->CompressedData[" -1:eJxTTMoPSmViYGAQBmIQzabg2hQ2/5XjkZiwCWD6vsouEL1DRHgPiJ7y3eYa +1:eJxTTMoPSmViYGAQAWIQzabg2hQ2/5XjkZiwCWD6vsouEL1DRHgPiJ7y3eYa iNYLywfTJgyz9COB9BLNRWDaXrpeMQ5It3i2gukqz3KLXwuA8r3rwfSH/ENd EldfOYas/AWmn7AFnftx95UjzxoIvcjizncQLRSz5CeI3rSKjfMnkH7i2Aym Of656YDoqj9ZYHoah+E/sfuvHL9th9AWRo9CUx+9cmx4C6GbdqtMuvP5lWNO qSqYbts3aymIvty+fRmI1rE8/ApEz3zi8xpEG4X4uUV8f+VYc/CIO4jWuNGh -nMzxxvGYUyeYfiNx5WJB7RvHZyYQGgD4x6s8 +nMzxxvGYUyeYfiNx5WJB7RvHZyYQ+tGU7f+c+t44Lps/hckZSAMAAsS0Pg== + "], - ExpressionUUID -> "e547bf6c-b217-4f33-bf06-e45bb055fb6c"], + CellLabel-> + "In[562]:=",ExpressionUUID->"e547bf6c-b217-4f33-bf06-e45bb055fb6c"], Cell[BoxData[{ RowBox[{ @@ -8034,13 +8806,31 @@ Cell[BoxData[{ RowBox[{"ToGoodC", "[", "JAugDISPQSO", "]"}], ",", "\"\\""}], "]"}], ";"}], "\n", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JAugIMUintdPSO", "]"}], ",", + "\"\\""}], + "]"}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JAugQprevSO", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JAugVprevSO", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", RowBox[{ RowBox[{ RowBox[{"MyStringWrite", "[", RowBox[{ - RowBox[{"ToGoodC", "[", "JAugIMUintdPSO", "]"}], ",", - "\"\\""}], - "]"}], ";"}], "\n"}], "\[IndentingNewLine]", + RowBox[{"ToGoodC", "[", "JAugWprevSO", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\n"}], "\[IndentingNewLine]", RowBox[{ RowBox[{ "Run", "[", @@ -8094,6 +8884,24 @@ BackwardAugmentedStateEstimator_v7_JAugDispQSO.mthout J 0 1\>\"", "]"}], "Run", "[", "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], ";"}], "\[IndentingNewLine]"}], "Input", CellChangeTimes->{{3.573199890160159*^9, 3.573200002698824*^9}, { 3.573200337152315*^9, 3.573200352596279*^9}, {3.573200561905222*^9, @@ -8108,8 +8916,10 @@ BackwardAugmentedStateEstimator_v7_JAugIMUintdPSO.mthout J 0 1\>\"", "]"}], 3.617514641204034*^9}, {3.6175148126871643`*^9, 3.617514869303466*^9}, { 3.6175153588028593`*^9, 3.617515369792218*^9}, {3.619340866322624*^9, 3.619340873295295*^9}, {3.7624937242975063`*^9, 3.762493724308778*^9}, - 3.762493897568228*^9, {3.823863438633144*^9, 3.823863438636421*^9}}, - ExpressionUUID -> "d033a6a6-10ec-4c48-8037-a3d0629121ca"], + 3.762493897568228*^9, {3.823863438633144*^9, 3.823863438636421*^9}, { + 3.832682524449785*^9, 3.832682561743813*^9}}, + CellLabel-> + "In[586]:=",ExpressionUUID->"d033a6a6-10ec-4c48-8037-a3d0629121ca"], Cell[BoxData[{ RowBox[{ @@ -8160,12 +8970,30 @@ Cell[BoxData[{ RowBox[{"ToGoodC", "[", "JAugDISPQqOS", "]"}], ",", "\"\\""}], "]"}], ";"}], "\n", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JAugIMUintdPqOS", "]"}], ",", + "\"\\""}], + "]"}], ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JAugQprevqOS", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"MyStringWrite", "[", + RowBox[{ + RowBox[{"ToGoodC", "[", "JAugVprevqOS", "]"}], ",", + "\"\\""}], "]"}], + ";"}], "\[IndentingNewLine]", RowBox[{ RowBox[{ RowBox[{"MyStringWrite", "[", RowBox[{ - RowBox[{"ToGoodC", "[", "JAugIMUintdPqOS", "]"}], ",", - "\"\\""}], + RowBox[{"ToGoodC", "[", "JAugWprevqOS", "]"}], ",", + "\"\\""}], "]"}], ";"}], "\n"}], "\[IndentingNewLine]", RowBox[{ RowBox[{ @@ -8220,7 +9048,26 @@ BackwardAugmentedStateEstimator_v7_JAugDispQqOS.mthout J 0 1\>\"", "]"}], "Run", "[", "\"\\"", "]"}], - ";"}], "\[IndentingNewLine]"}], "Input", + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{ + RowBox[{ + "Run", "[", + "\"\\"", "]"}], + ";"}], "\[IndentingNewLine]"}], "\[IndentingNewLine]"}], "Input", CellChangeTimes->{{3.573199890160159*^9, 3.573200002698824*^9}, { 3.573200337152315*^9, 3.573200352596279*^9}, {3.573200561905222*^9, 3.57320056347927*^9}, {3.573205372812525*^9, 3.573205373067583*^9}, { @@ -8235,8 +9082,10 @@ BackwardAugmentedStateEstimator_v7_JAugIMUintdPqOS.mthout J 0 1\>\"", "]"}], 3.617514906459271*^9, 3.617514933746583*^9}, {3.617515398742161*^9, 3.6175154386716948`*^9}, {3.619340907272347*^9, 3.619340913173324*^9}, { 3.762493724345517*^9, 3.762493724356784*^9}, 3.762493971689711*^9, { - 3.823863438638115*^9, 3.823863438641457*^9}}, - ExpressionUUID -> "1860b2d7-7e7c-4063-aa98-3baa3acca060"], + 3.823863438638115*^9, 3.823863438641457*^9}, {3.83268257088909*^9, + 3.832682627160026*^9}}, + CellLabel-> + "In[610]:=",ExpressionUUID->"1860b2d7-7e7c-4063-aa98-3baa3acca060"], Cell[BoxData[{ RowBox[{ @@ -8253,16 +9102,20 @@ Cell[BoxData[{ CellChangeTimes->{{3.606038796963069*^9, 3.606038806188902*^9}, { 3.606039984602006*^9, 3.6060399865032663`*^9}, {3.633438018484614*^9, 3.633438019213787*^9}, 3.7624939774750013`*^9}, - ExpressionUUID -> "162827f2-4a00-4183-9dfd-8807fbdfad2f"] + CellLabel-> + "In[634]:=",ExpressionUUID->"162827f2-4a00-4183-9dfd-8807fbdfad2f"] }, Open ]] }, AutoGeneratedPackage->None, CellGrouping->Manual, -WindowSize->{3696, 2032}, -WindowMargins->{{-2, Automatic}, {Automatic, -2}}, -Magnification:>2. Inherited, -FrontEndVersion->"9.0 for Linux x86 (64-bit) (November 20, 2012)", -StyleDefinitions->"Default.nb" +WindowSize->{1386, 852}, +WindowMargins->{{Automatic, 0}, {0, Automatic}}, +TaggingRules->{ + "WelcomeScreenSettings" -> {"FEStarting" -> False}, "TryRealOnly" -> False}, +Magnification:>0.7 Inherited, +FrontEndVersion->"12.2 for Linux x86 (64-bit) (December 12, 2020)", +StyleDefinitions->"Default.nb", +ExpressionUUID->"f2ed00e3-a9a7-4e18-94d6-6e605e6c0c77" ] (* End of Notebook Content *) @@ -8275,109 +9128,109 @@ CellTagsIndex->{} *) (*NotebookFileOutline Notebook[{ -Cell[545, 20, 525, 8, 88, "Input", +Cell[545, 20, 549, 9, 32, "Input",ExpressionUUID->"e0db13eb-9582-4c6d-b546-c853c2642b35", InitializationCell->True], -Cell[1073, 30, 883, 20, 257, "Input", +Cell[1097, 31, 907, 21, 93, "Input",ExpressionUUID->"3c600d9d-2e79-41d3-8083-e1831361c722", InitializationCell->True], -Cell[1959, 52, 242, 5, 103, "Subtitle", +Cell[2007, 54, 242, 5, 37, "Subtitle",ExpressionUUID->"9f7cf2b0-85b8-42e4-a83e-a0efc9887daa", InitializationCell->True], -Cell[2204, 59, 944, 30, 88, "Input", +Cell[2252, 61, 968, 31, 32, "Input",ExpressionUUID->"f86ff61e-6031-4947-996f-c15c46271dbb", InitializationCell->True], -Cell[3151, 91, 1634, 47, 88, "Input", +Cell[3223, 94, 1658, 48, 32, "Input",ExpressionUUID->"c6408360-b2e8-4480-9616-8c693b01c013", InitializationCell->True], -Cell[4788, 140, 924, 29, 131, "Input", +Cell[4884, 144, 948, 30, 49, "Input",ExpressionUUID->"277e1463-1c11-4b65-b1bc-b28d2b0e5524", InitializationCell->True], -Cell[5715, 171, 1632, 49, 88, "Input", +Cell[5835, 176, 1656, 50, 32, "Input",ExpressionUUID->"2d39ecd2-7dd3-4880-9ef7-4ddd41eaff19", InitializationCell->True], -Cell[7350, 222, 953, 25, 88, "Input", +Cell[7494, 228, 977, 26, 33, "Input",ExpressionUUID->"a8b63d28-8415-46db-898f-c535129b02a0", InitializationCell->True], -Cell[8306, 249, 1325, 40, 131, "Input", +Cell[8474, 256, 1349, 41, 48, "Input",ExpressionUUID->"13e8fd29-a86a-4514-9ef2-bbe37133ffab", InitializationCell->True], -Cell[9634, 291, 2490, 65, 257, "Input", +Cell[9826, 299, 2514, 66, 123, "Input",ExpressionUUID->"e2231016-d80a-43fc-841d-ee590b1c2a49", InitializationCell->True], -Cell[12127, 358, 236, 5, 61, "Subsubtitle", +Cell[12343, 367, 236, 5, 21, "Subsubtitle",ExpressionUUID->"cfada3cd-ac54-48b6-8b43-295a46b2c850", InitializationCell->True], -Cell[12366, 365, 2335, 75, 509, "Input", +Cell[12582, 374, 2359, 76, 183, "Input",ExpressionUUID->"03f4c2ce-f63a-48b8-8e96-2ef2f3664780", InitializationCell->True], -Cell[14704, 442, 603, 18, 131, "Input", +Cell[14944, 452, 627, 19, 48, "Input",ExpressionUUID->"8356b1fb-37a1-4b3c-a6a1-002dc40c7ee1", InitializationCell->True], -Cell[15310, 462, 1284, 27, 131, "Input", +Cell[15574, 473, 1308, 28, 48, "Input",ExpressionUUID->"3cd926d4-767b-409d-b5ee-f0b985910568", InitializationCell->True], -Cell[16597, 491, 327, 5, 61, "Subsubtitle"], +Cell[16885, 503, 327, 5, 21, "Subsubtitle",ExpressionUUID->"26064550-11f1-4bb5-a8bc-e7641f6b440d"], Cell[CellGroupData[{ -Cell[16949, 500, 686, 18, 215, "Input"], -Cell[17638, 520, 1798, 45, 209, "Output"], -Cell[19439, 567, 4348, 114, 239, "Output"] +Cell[17237, 512, 1151, 32, 98, "Input",ExpressionUUID->"2618f75a-15b8-4eab-8502-5b0faafa4be4"], +Cell[18391, 546, 1847, 47, 75, "Output",ExpressionUUID->"18271bfb-de94-406b-bcae-51c0d0b6c135"], +Cell[20241, 595, 4439, 116, 87, "Output",ExpressionUUID->"5845398f-f75f-49e2-bd78-296d43853c95"] }, Open ]], Cell[CellGroupData[{ -Cell[23824, 686, 1463, 40, 215, "Input"], -Cell[25290, 728, 2743, 72, 266, "Output"], -Cell[28036, 802, 8386, 210, 338, "Output"] +Cell[24717, 716, 1487, 41, 67, "Input",ExpressionUUID->"a4331214-3c74-4555-af4c-a6061694ee5d"], +Cell[26207, 759, 2793, 74, 96, "Output",ExpressionUUID->"51d66e53-6b4f-452c-9a84-7eaa720e67e6"], +Cell[29003, 835, 8434, 212, 126, "Output",ExpressionUUID->"6c35c2d1-9c9d-464c-be6d-256d48d18b3d"] }, Open ]], Cell[CellGroupData[{ -Cell[36459, 1017, 2030, 51, 299, "Input"], -Cell[38492, 1070, 2272, 66, 257, "Input"] +Cell[37474, 1052, 2311, 59, 112, "Input",ExpressionUUID->"471e0847-624c-413e-9a45-53b7ac630836"], +Cell[39788, 1113, 2805, 82, 97, "Input",ExpressionUUID->"f44ac9b5-4f66-4593-b64a-dac5fee848ae"] }, Open ]], Cell[CellGroupData[{ -Cell[40801, 1141, 470, 8, 215, "Input"], -Cell[41274, 1151, 6026, 162, 245, "Output"], -Cell[47303, 1315, 3238, 83, 209, "Output"], -Cell[50544, 1400, 9421, 244, 338, "Output"], -Cell[59968, 1646, 22873, 518, 257, "Output"] +Cell[42630, 1200, 494, 9, 67, "Input",ExpressionUUID->"0a6da266-07fb-4595-9bfa-54e81dfa376e"], +Cell[43127, 1211, 6079, 164, 90, "Output",ExpressionUUID->"3c1d34a7-4c5d-4cd7-8048-277376e7ede9"], +Cell[49209, 1377, 3295, 85, 75, "Output",ExpressionUUID->"0349e82c-3244-45ba-b424-2f8f61462637"], +Cell[52507, 1464, 9981, 259, 93, "Output",ExpressionUUID->"84b5a5c0-0a05-4dbd-914c-31d40e9a2a80"], +Cell[62491, 1725, 23073, 532, 93, "Output",ExpressionUUID->"8baeb93f-a586-4dcd-902f-79087ac00b31"] }, Open ]], -Cell[82856, 2167, 2076, 57, 299, "Input"], -Cell[84935, 2226, 1750, 44, 131, "Input"], -Cell[86688, 2272, 593, 10, 131, "Input"], +Cell[85579, 2260, 2952, 82, 142, "Input",ExpressionUUID->"dc9c74e4-b7fd-479d-96ad-df3b425945ed"], +Cell[88534, 2344, 1867, 46, 52, "Input",ExpressionUUID->"af6ea6ed-8d84-4364-ad72-d9b03ce382e8"], +Cell[90404, 2392, 617, 11, 37, "Input",ExpressionUUID->"cbbf40a5-ffc1-449d-b645-dc4426abe1db"], Cell[CellGroupData[{ -Cell[87306, 2286, 253, 4, 131, "Input"], -Cell[87562, 2292, 37550, 913, 245, "Output"], -Cell[125115, 3207, 8730, 222, 393, "Output"] +Cell[91046, 2407, 277, 5, 37, "Input",ExpressionUUID->"a4fa1226-db4f-4846-841d-81952fe05a97"], +Cell[91326, 2414, 32766, 797, 90, "Output",ExpressionUUID->"03000688-f85f-4d04-9c50-e0d07d99b5ec"], +Cell[124095, 3213, 23846, 562, 143, "Output",ExpressionUUID->"6ca3cee9-19b8-4348-879b-e870a2d8e793"] }, Open ]], -Cell[133860, 3432, 2040, 47, 173, "Input"], -Cell[135903, 3481, 1587, 34, 341, "Input"], +Cell[147956, 3778, 2064, 48, 52, "Input",ExpressionUUID->"14f9d837-7081-472b-a26e-d681ac2cf1aa"], +Cell[150023, 3828, 1611, 35, 112, "Input",ExpressionUUID->"9579ced9-13a8-447f-82d0-84c2516d7db7"], Cell[CellGroupData[{ -Cell[137515, 3519, 317, 5, 131, "Input"], -Cell[137835, 3526, 43691, 1078, 188, "Output"], -Cell[181529, 4606, 14989, 401, 183, "Output"] +Cell[151659, 3867, 341, 6, 37, "Input",ExpressionUUID->"0894dfd6-f474-46d3-b15b-56dc4f477026"], +Cell[152003, 3875, 43896, 1082, 66, "Output",ExpressionUUID->"879808b0-e9e6-4155-9f2c-8ef66dd10160"], +Cell[195902, 4959, 15186, 405, 69, "Output",ExpressionUUID->"f38ff185-74d0-4ced-8a97-d1853e3821c0"] }, Open ]], -Cell[196533, 5010, 1884, 51, 257, "Input"], -Cell[198420, 5063, 631, 14, 88, "Input"], +Cell[211103, 5367, 1908, 52, 82, "Input",ExpressionUUID->"9db810eb-0c78-44dc-aec4-e68718871d82"], +Cell[213014, 5421, 655, 15, 21, "Input",ExpressionUUID->"6301058d-c421-4eaa-b9cc-557560239001"], Cell[CellGroupData[{ -Cell[199076, 5081, 305, 6, 131, "Input"], -Cell[199384, 5089, 54727, 1325, 224, "Output"] +Cell[213694, 5440, 329, 7, 37, "Input",ExpressionUUID->"a2f92fb8-7fd6-40b4-b3b6-f1895b04c740"], +Cell[214026, 5449, 54938, 1329, 85, "Output",ExpressionUUID->"11acf96c-aee3-419f-927c-5c74cd5ad370"] }, Open ]], -Cell[254126, 6417, 822, 18, 131, "Input"], -Cell[254951, 6437, 297, 5, 61, "Subsubtitle"], -Cell[255251, 6444, 359, 7, 88, "Input"], -Cell[255613, 6453, 824, 25, 131, "Input"], -Cell[256440, 6480, 1178, 26, 131, "Input"], -Cell[257621, 6508, 399, 10, 88, "Input"], +Cell[268979, 6781, 1148, 23, 37, "Input",ExpressionUUID->"772da980-73d4-4177-9a96-6c731ff9d20a"], +Cell[270130, 6806, 297, 5, 21, "Subsubtitle",ExpressionUUID->"3be27e2b-e3b2-468f-98c0-a319ba457dde"], +Cell[270430, 6813, 383, 8, 21, "Input",ExpressionUUID->"2b65f9f5-73dc-4068-8969-212ca637e4c2"], +Cell[270816, 6823, 848, 26, 37, "Input",ExpressionUUID->"e462403b-c716-4eee-91c5-c7dbfdbd6e2d"], +Cell[271667, 6851, 1202, 27, 37, "Input",ExpressionUUID->"f49bbb81-1821-409e-9a7c-4648040b18ce"], +Cell[272872, 6880, 423, 11, 21, "Input",ExpressionUUID->"ed7721db-b1a2-4a8e-ba0a-fd8e03d91e5a"], Cell[CellGroupData[{ -Cell[258045, 6522, 279, 4, 61, "Subsubtitle"], -Cell[258327, 6528, 350, 9, 88, "Input"] +Cell[273320, 6895, 279, 4, 21, "Subsubtitle",ExpressionUUID->"7407d765-174c-470c-8f5e-a618c9b1ab2f"], +Cell[273602, 6901, 374, 10, 21, "Input",ExpressionUUID->"22c85d01-74a2-4ab0-aadc-9f415f2e5625"] }, Open ]], Cell[CellGroupData[{ -Cell[258714, 6542, 246, 4, 61, "Subsubtitle"], -Cell[258963, 6548, 814, 13, 131, "Input"], -Cell[259780, 6563, 5657, 166, 509, "Input"], -Cell[265440, 6731, 5674, 166, 509, "Input"], -Cell[271117, 6899, 5602, 165, 509, "Input"] +Cell[274013, 6916, 246, 4, 21, "Subsubtitle",ExpressionUUID->"bc6269f2-6f1e-4bb3-881c-d62da73a00e0"], +Cell[274262, 6922, 838, 14, 37, "Input",ExpressionUUID->"a77efe4d-2c92-466a-9ee5-09bee4330c9c"], +Cell[275103, 6938, 7158, 212, 217, "Input",ExpressionUUID->"b23ded5d-856b-4152-b45f-9afd0cab6abb"], +Cell[282264, 7152, 7183, 213, 217, "Input",ExpressionUUID->"afea1643-327e-49c5-8654-688a95d46acb"], +Cell[289450, 7367, 7106, 212, 217, "Input",ExpressionUUID->"fef7a11e-1dd8-4c10-885e-c6924832ad33"] }, Open ]], Cell[CellGroupData[{ -Cell[276756, 7069, 236, 4, 61, "Subsubtitle"], -Cell[276995, 7075, 972, 26, 131, "Input"], -Cell[277970, 7103, 5094, 164, 425, "Input"], -Cell[283067, 7269, 5103, 164, 425, "Input"], -Cell[288173, 7435, 376, 9, 131, "Input"], -Cell[288552, 7446, 1196, 24, 215, "Input"], -Cell[289751, 7472, 5256, 132, 887, "Input"], -Cell[295010, 7606, 4744, 127, 929, "Input"], -Cell[299757, 7735, 427, 10, 131, "Input"], -Cell[300187, 7747, 4195, 118, 887, "Input"], -Cell[304385, 7867, 4168, 118, 845, "Input"], -Cell[308556, 7987, 4539, 124, 887, "Input"], -Cell[313098, 8113, 4612, 125, 887, "Input"], -Cell[317713, 8240, 631, 15, 215, "Input"] +Cell[296593, 7584, 236, 4, 21, "Subsubtitle",ExpressionUUID->"afefaef2-d6dd-481e-a5de-418cbcbfaa1c"], +Cell[296832, 7590, 996, 27, 37, "Input",ExpressionUUID->"bddae2f3-5491-4f6b-a59f-80093e439436"], +Cell[297831, 7619, 6886, 220, 187, "Input",ExpressionUUID->"6c12f5eb-34c9-4c10-be38-8da6426dc669"], +Cell[304720, 7841, 6948, 221, 187, "Input",ExpressionUUID->"ca659b6d-dc91-461a-943e-607d3f607ebf"], +Cell[311671, 8064, 400, 10, 37, "Input",ExpressionUUID->"01e81ac5-1851-49ba-a288-48dc7163f39d"], +Cell[312074, 8076, 1220, 25, 67, "Input",ExpressionUUID->"660027f3-dbe7-4552-8f1f-be06da3c7f7d"], +Cell[313297, 8103, 6343, 159, 397, "Input",ExpressionUUID->"a6871df0-153c-4955-b454-6dec0f8aac6a"], +Cell[319643, 8264, 5496, 146, 367, "Input",ExpressionUUID->"b71d9672-279a-4123-ab53-1e3075234465"], +Cell[325142, 8412, 1061, 29, 82, "Input",ExpressionUUID->"6498545b-7a32-4bdd-bcba-16b669221638"], +Cell[326206, 8443, 5498, 156, 397, "Input",ExpressionUUID->"905004a8-9759-44c4-afdc-b3ff9d6b889c"], +Cell[331707, 8601, 5459, 156, 382, "Input",ExpressionUUID->"e547bf6c-b217-4f33-bf06-e45bb055fb6c"], +Cell[337169, 8759, 5855, 162, 397, "Input",ExpressionUUID->"d033a6a6-10ec-4c48-8037-a3d0629121ca"], +Cell[343027, 8923, 5977, 164, 412, "Input",ExpressionUUID->"1860b2d7-7e7c-4063-aa98-3baa3acca060"], +Cell[349007, 9089, 655, 16, 67, "Input",ExpressionUUID->"162827f2-4a00-4183-9dfd-8807fbdfad2f"] }, Open ]] } ] diff --git a/roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_JAugQprevSO.cppready b/roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_JAugQprevSO.cppready new file mode 100644 index 0000000..e69de29 diff --git a/roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_JAugQprevX0.cppready b/roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_JAugQprevX0.cppready new file mode 100644 index 0000000..e69de29 diff --git a/roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_JAugQprevX1.cppready b/roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_JAugQprevX1.cppready new file mode 100644 index 0000000..f083b29 --- /dev/null +++ b/roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_JAugQprevX1.cppready @@ -0,0 +1,44 @@ +double tmp0 = qos(3+_OFF); +double tmp1 = x1(4+_OFF); +double tmp2 = 0.01*tmp1; +double tmp3 = 0. + tmp2; +double tmp4 = qos(4+_OFF); +double tmp5 = x1(5+_OFF); +double tmp6 = qos(1+_OFF); +double tmp7 = x1(6+_OFF); +double tmp8 = -0.01*tmp7; +double tmp9 = 0. + tmp8; +double tmp10 = qos(2+_OFF); +double tmp11 = x1(7+_OFF); +double tmp12 = -0.01*tmp5; +double tmp13 = 0. + tmp12; +double tmp14 = -0.01*tmp11; +double tmp15 = 0. + tmp14; +double tmp16 = 0.01*tmp11; +double tmp17 = 0. + tmp16; +double tmp18 = 0.01*tmp5; +double tmp19 = 0. + tmp18; +double tmp20 = tmp6*tmp15; +double tmp21 = -(tmp0*tmp3); +double tmp22 = 0.01*tmp7; +double tmp23 = 0. + tmp22; +double tmp24 = -(tmp4*tmp3); +double tmp25 = tmp10*tmp9; +double tmp26 = tmp6*tmp3; +double tmp27 = tmp6*tmp13; +double tmp28 = tmp4*tmp13; +double tmp29 = tmp6*tmp9; +double tmp30 = -(tmp10*tmp3); +double tmp31 = tmp0*tmp15; +J(1+_OFF,4+_OFF) = tmp30 + tmp27 - tmp4*tmp9 - tmp0*tmp17; +J(1+_OFF,5+_OFF) = tmp21 - tmp4*tmp19 + tmp29 - tmp10*tmp15; +J(1+_OFF,6+_OFF) = tmp24 - tmp0*tmp13 - tmp10*tmp23 + tmp20; +J(2+_OFF,4+_OFF) = tmp26 + tmp10*tmp13 - tmp0*tmp9 + tmp4*tmp17; +J(2+_OFF,5+_OFF) = tmp4*tmp3 - tmp0*tmp19 + tmp25 + tmp20; +J(2+_OFF,6+_OFF) = tmp21 + tmp28 + tmp6*tmp23 + tmp10*tmp15; +J(3+_OFF,4+_OFF) = tmp24 + tmp0*tmp13 + tmp25 + tmp6*tmp17; +J(3+_OFF,5+_OFF) = tmp26 + tmp10*tmp19 + tmp0*tmp9 - tmp4*tmp15; +J(3+_OFF,6+_OFF) = tmp10*tmp3 + tmp27 - tmp4*tmp23 + tmp31; +J(4+_OFF,4+_OFF) = tmp0*tmp3 + tmp28 + tmp29 - tmp10*tmp17; +J(4+_OFF,5+_OFF) = tmp30 + tmp6*tmp19 + tmp4*tmp9 + tmp31; +J(4+_OFF,6+_OFF) = tmp26 - tmp10*tmp13 + tmp0*tmp23 + tmp4*tmp15; diff --git a/roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_JAugQprevX2.cppready b/roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_JAugQprevX2.cppready new file mode 100644 index 0000000..e69de29 diff --git a/roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_JAugQprevqOS.cppready b/roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_JAugQprevqOS.cppready new file mode 100644 index 0000000..f496c9d --- /dev/null +++ b/roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_JAugQprevqOS.cppready @@ -0,0 +1,44 @@ +double tmp0 = qos(3+_OFF); +double tmp1 = -0.01*tmp0; +double tmp2 = 0. + tmp1; +double tmp3 = x1(4+_OFF); +double tmp4 = qos(4+_OFF); +double tmp5 = x1(5+_OFF); +double tmp6 = qos(1+_OFF); +double tmp7 = 0.01*tmp6; +double tmp8 = 0. + tmp7; +double tmp9 = x1(6+_OFF); +double tmp10 = qos(2+_OFF); +double tmp11 = x1(7+_OFF); +double tmp12 = -0.01*tmp4; +double tmp13 = 0. + tmp12; +double tmp14 = -0.01*tmp10; +double tmp15 = 0. + tmp14; +double tmp16 = 0.01*tmp4; +double tmp17 = 0. + tmp16; +double tmp18 = tmp13*tmp3; +double tmp19 = 0.01*tmp10; +double tmp20 = 0. + tmp19; +double tmp21 = -(tmp8*tmp11); +double tmp22 = 0.01*tmp0; +double tmp23 = 0. + tmp22; +double tmp24 = tmp8*tmp3; +double tmp25 = tmp2*tmp9; +double tmp26 = tmp15*tmp3; +double tmp27 = -(tmp8*tmp5); +double tmp28 = tmp2*tmp3; +double tmp29 = -(tmp8*tmp9); +double tmp30 = tmp15*tmp5; +double tmp31 = tmp13*tmp11; +J(1+_OFF,1+_OFF) = tmp26 + tmp27 - tmp17*tmp9 - tmp2*tmp11; +J(1+_OFF,2+_OFF) = tmp28 - tmp13*tmp5 + tmp29 - tmp20*tmp11; +J(1+_OFF,3+_OFF) = tmp18 - tmp23*tmp5 - tmp15*tmp9 + tmp21; +J(2+_OFF,1+_OFF) = tmp24 + tmp30 + tmp25 - tmp17*tmp11; +J(2+_OFF,2+_OFF) = tmp18 + tmp2*tmp5 + tmp20*tmp9 + tmp21; +J(2+_OFF,3+_OFF) = tmp23*tmp3 + tmp13*tmp5 + tmp8*tmp9 - tmp15*tmp11; +J(3+_OFF,1+_OFF) = tmp17*tmp3 - tmp2*tmp5 + tmp15*tmp9 + tmp8*tmp11; +J(3+_OFF,2+_OFF) = tmp24 - tmp20*tmp5 + tmp25 + tmp31; +J(3+_OFF,3+_OFF) = tmp26 + tmp27 + tmp13*tmp9 + tmp23*tmp11; +J(4+_OFF,1+_OFF) = tmp28 + tmp17*tmp5 + tmp29 + tmp15*tmp11; +J(4+_OFF,2+_OFF) = tmp20*tmp3 + tmp8*tmp5 - tmp13*tmp9 + tmp2*tmp11; +J(4+_OFF,3+_OFF) = tmp24 + tmp30 - tmp23*tmp9 + tmp31; diff --git a/roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_JAugVprevSO.cppready b/roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_JAugVprevSO.cppready new file mode 100644 index 0000000..104933c --- /dev/null +++ b/roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_JAugVprevSO.cppready @@ -0,0 +1,85 @@ +double tmp0 = 1/Dt12; +double tmp1 = qos(1+_OFF); +double tmp2 = 1.*tmp1; +double tmp3 = 0. + tmp2; +double tmp4 = qos(3+_OFF); +double tmp5 = 1.*tmp4; +double tmp6 = 0. + tmp5; +double tmp7 = qos(2+_OFF); +double tmp8 = 1.*tmp7; +double tmp9 = 0. + tmp8; +double tmp10 = qos(4+_OFF); +double tmp11 = 1.*tmp10; +double tmp12 = 0. + tmp11; +double tmp13 = x1(6+_OFF); +double tmp14 = x2(4+_OFF); +double tmp15 = x1(7+_OFF); +double tmp16 = x2(5+_OFF); +double tmp17 = x1(4+_OFF); +double tmp18 = x2(6+_OFF); +double tmp19 = x1(5+_OFF); +double tmp20 = x2(7+_OFF); +double tmp21 = -(tmp3*tmp6); +double tmp22 = tmp9*tmp12; +double tmp23 = tmp21 + tmp22; +double tmp24 = std::pow(tmp3,2); +double tmp25 = std::pow(tmp9,2); +double tmp26 = std::pow(tmp6,2); +double tmp27 = -tmp26; +double tmp28 = std::pow(tmp12,2); +double tmp29 = -tmp28; +double tmp30 = tmp24 + tmp25 + tmp27 + tmp29; +double tmp31 = tmp9*tmp6; +double tmp32 = tmp3*tmp12; +double tmp33 = tmp31 + tmp32; +double tmp34 = -2*tmp0*tmp15*tmp14; +double tmp35 = 2*tmp0*tmp13*tmp16; +double tmp36 = -2*tmp0*tmp19*tmp18; +double tmp37 = 2*tmp0*tmp17*tmp20; +double tmp38 = tmp34 + tmp35 + tmp36 + tmp37; +double tmp39 = 2*tmp0*tmp13*tmp14; +double tmp40 = 2*tmp0*tmp15*tmp16; +double tmp41 = -2*tmp0*tmp17*tmp18; +double tmp42 = -2*tmp0*tmp19*tmp20; +double tmp43 = tmp39 + tmp40 + tmp41 + tmp42; +double tmp44 = 2*tmp0*tmp15*tmp14; +double tmp45 = -2*tmp0*tmp13*tmp16; +double tmp46 = 2*tmp0*tmp19*tmp18; +double tmp47 = -2*tmp0*tmp17*tmp20; +double tmp48 = tmp44 + tmp45 + tmp46 + tmp47; +double tmp49 = tmp3*tmp9; +double tmp50 = tmp6*tmp12; +double tmp51 = tmp49 + tmp50; +double tmp52 = -2*tmp0*tmp19*tmp14; +double tmp53 = 2*tmp0*tmp17*tmp16; +double tmp54 = 2*tmp0*tmp15*tmp18; +double tmp55 = -2*tmp0*tmp13*tmp20; +double tmp56 = tmp52 + tmp53 + tmp54 + tmp55; +double tmp57 = -(tmp3*tmp12); +double tmp58 = tmp31 + tmp57; +double tmp59 = -2*tmp0*tmp13*tmp14; +double tmp60 = -2*tmp0*tmp15*tmp16; +double tmp61 = 2*tmp0*tmp17*tmp18; +double tmp62 = 2*tmp0*tmp19*tmp20; +double tmp63 = tmp59 + tmp60 + tmp61 + tmp62; +double tmp64 = -tmp25; +double tmp65 = tmp24 + tmp64 + tmp26 + tmp29; +double tmp66 = 2*tmp0*tmp19*tmp14; +double tmp67 = -2*tmp0*tmp17*tmp16; +double tmp68 = -2*tmp0*tmp15*tmp18; +double tmp69 = 2*tmp0*tmp13*tmp20; +double tmp70 = tmp66 + tmp67 + tmp68 + tmp69; +double tmp71 = tmp24 + tmp64 + tmp27 + tmp28; +double tmp72 = tmp3*tmp6; +double tmp73 = tmp72 + tmp22; +double tmp74 = -(tmp3*tmp9); +double tmp75 = tmp74 + tmp50; +J(1+_OFF,1+_OFF) = 2*tmp33*tmp38 + 2*tmp23*tmp43; +J(1+_OFF,2+_OFF) = tmp30*tmp48 + 2*tmp23*tmp56; +J(1+_OFF,3+_OFF) = tmp30*tmp63 + 2*tmp33*tmp70; +J(2+_OFF,1+_OFF) = tmp65*tmp38 + 2*tmp51*tmp43; +J(2+_OFF,2+_OFF) = 2*tmp58*tmp48 + 2*tmp51*tmp56; +J(2+_OFF,3+_OFF) = 2*tmp58*tmp63 + tmp65*tmp70; +J(3+_OFF,1+_OFF) = 2*tmp75*tmp38 + tmp71*tmp43; +J(3+_OFF,2+_OFF) = 2*tmp73*tmp48 + tmp71*tmp56; +J(3+_OFF,3+_OFF) = 2*tmp73*tmp63 + 2*tmp75*tmp70; diff --git a/roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_JAugVprevX0.cppready b/roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_JAugVprevX0.cppready new file mode 100644 index 0000000..aa8f22c --- /dev/null +++ b/roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_JAugVprevX0.cppready @@ -0,0 +1,67 @@ +double tmp0 = 1/Dt01; +double tmp1 = qos(1+_OFF); +double tmp2 = qos(3+_OFF); +double tmp3 = qos(2+_OFF); +double tmp4 = qos(4+_OFF); +double tmp5 = x1(4+_OFF); +double tmp6 = x1(6+_OFF); +double tmp7 = x1(5+_OFF); +double tmp8 = x1(7+_OFF); +double tmp9 = std::pow(tmp1,2); +double tmp10 = std::pow(tmp3,2); +double tmp11 = std::pow(tmp2,2); +double tmp12 = -tmp11; +double tmp13 = std::pow(tmp4,2); +double tmp14 = -tmp13; +double tmp15 = tmp9 + tmp10 + tmp12 + tmp14; +double tmp16 = tmp7*tmp6; +double tmp17 = -(tmp1*tmp2); +double tmp18 = tmp3*tmp4; +double tmp19 = tmp17 + tmp18; +double tmp20 = tmp3*tmp2; +double tmp21 = tmp1*tmp4; +double tmp22 = tmp20 + tmp21; +double tmp23 = std::pow(tmp8,2); +double tmp24 = 2*tmp23; +double tmp25 = std::pow(tmp7,2); +double tmp26 = 2*tmp25; +double tmp27 = std::pow(tmp6,2); +double tmp28 = 2*tmp27; +double tmp29 = tmp5*tmp6; +double tmp30 = tmp5*tmp7; +double tmp31 = -(tmp5*tmp8); +double tmp32 = tmp16 + tmp31; +double tmp33 = tmp7*tmp8; +double tmp34 = tmp29 + tmp33; +double tmp35 = -1 + tmp28 + tmp24; +double tmp36 = -(tmp1*tmp4); +double tmp37 = tmp20 + tmp36; +double tmp38 = tmp5*tmp8; +double tmp39 = tmp16 + tmp38; +double tmp40 = tmp1*tmp3; +double tmp41 = tmp2*tmp4; +double tmp42 = tmp40 + tmp41; +double tmp43 = -(tmp6*tmp8); +double tmp44 = tmp30 + tmp43; +double tmp45 = -tmp10; +double tmp46 = tmp9 + tmp45 + tmp11 + tmp14; +double tmp47 = -1 + tmp26 + tmp24; +double tmp48 = -1 + tmp26 + tmp28; +double tmp49 = -(tmp7*tmp8); +double tmp50 = tmp29 + tmp49; +double tmp51 = tmp6*tmp8; +double tmp52 = tmp30 + tmp51; +double tmp53 = tmp1*tmp2; +double tmp54 = tmp53 + tmp18; +double tmp55 = tmp9 + tmp45 + tmp12 + tmp13; +double tmp56 = -(tmp1*tmp3); +double tmp57 = tmp56 + tmp41; +J(1+_OFF,1+_OFF) = -0.004*tmp0*tmp22*tmp32 - 0.004*tmp0*tmp19*tmp34 + 0.001*tmp0*tmp15*tmp35; +J(1+_OFF,2+_OFF) = -0.002*tmp0*tmp15*tmp39 + 0.004*tmp0*tmp19*tmp44 + 0.002*tmp0*tmp22*tmp47; +J(1+_OFF,3+_OFF) = 0.002*tmp0*tmp19*tmp48 + 0.002*tmp0*tmp15*tmp50 - 0.004*tmp0*tmp22*tmp52; +J(2+_OFF,1+_OFF) = -0.002*tmp0*tmp46*tmp32 - 0.004*tmp0*tmp42*tmp34 + 0.002*tmp0*tmp37*tmp35; +J(2+_OFF,2+_OFF) = -0.004*tmp0*tmp37*tmp39 + 0.004*tmp0*tmp42*tmp44 + 0.001*tmp0*tmp46*tmp47; +J(2+_OFF,3+_OFF) = 0.002*tmp0*tmp42*tmp48 + 0.004*tmp0*tmp37*tmp50 - 0.002*tmp0*tmp46*tmp52; +J(3+_OFF,1+_OFF) = -0.004*tmp0*tmp57*tmp32 - 0.002*tmp0*tmp55*tmp34 + 0.002*tmp0*tmp54*tmp35; +J(3+_OFF,2+_OFF) = -0.004*tmp0*tmp54*tmp39 + 0.002*tmp0*tmp55*tmp44 + 0.002*tmp0*tmp57*tmp47; +J(3+_OFF,3+_OFF) = 0.001*tmp0*tmp55*tmp48 + 0.004*tmp0*tmp54*tmp50 - 0.004*tmp0*tmp57*tmp52; diff --git a/roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_JAugVprevX1.cppready b/roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_JAugVprevX1.cppready new file mode 100644 index 0000000..5aa01fa --- /dev/null +++ b/roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_JAugVprevX1.cppready @@ -0,0 +1,320 @@ +double tmp0 = 1/Dt01; +double tmp1 = qos(1+_OFF); +double tmp2 = qos(3+_OFF); +double tmp3 = qos(2+_OFF); +double tmp4 = qos(4+_OFF); +double tmp5 = x1(4+_OFF); +double tmp6 = 1.*tmp5; +double tmp7 = 0. + tmp6; +double tmp8 = x1(6+_OFF); +double tmp9 = 1.*tmp8; +double tmp10 = 0. + tmp9; +double tmp11 = x1(5+_OFF); +double tmp12 = 1.*tmp11; +double tmp13 = 0. + tmp12; +double tmp14 = x1(7+_OFF); +double tmp15 = 1.*tmp14; +double tmp16 = 0. + tmp15; +double tmp17 = std::pow(tmp1,2); +double tmp18 = std::pow(tmp3,2); +double tmp19 = std::pow(tmp2,2); +double tmp20 = -tmp19; +double tmp21 = std::pow(tmp4,2); +double tmp22 = -tmp21; +double tmp23 = tmp17 + tmp18 + tmp20 + tmp22; +double tmp24 = tmp13*tmp10; +double tmp25 = -(tmp1*tmp2); +double tmp26 = tmp3*tmp4; +double tmp27 = tmp25 + tmp26; +double tmp28 = tmp3*tmp2; +double tmp29 = tmp1*tmp4; +double tmp30 = tmp28 + tmp29; +double tmp31 = std::pow(tmp16,2); +double tmp32 = 2*tmp31; +double tmp33 = std::pow(tmp13,2); +double tmp34 = 2*tmp33; +double tmp35 = std::pow(tmp10,2); +double tmp36 = 2*tmp35; +double tmp37 = tmp7*tmp10; +double tmp38 = tmp7*tmp13; +double tmp39 = -0.01*tmp8; +double tmp40 = 0. + tmp39; +double tmp41 = 0.01*tmp5; +double tmp42 = 0. + tmp41; +double tmp43 = 0.01*tmp14; +double tmp44 = 0. + tmp43; +double tmp45 = -0.01*tmp11; +double tmp46 = 0. + tmp45; +double tmp47 = 1/Dt12; +double tmp48 = x2(4+_OFF); +double tmp49 = so(3+_OFF); +double tmp50 = so(2+_OFF); +double tmp51 = x2(5+_OFF); +double tmp52 = x2(6+_OFF); +double tmp53 = x2(7+_OFF); +double tmp54 = x0(3+_OFF); +double tmp55 = x1(3+_OFF); +double tmp56 = -tmp55; +double tmp57 = 0. + tmp54 + tmp56; +double tmp58 = 4*tmp10*tmp44; +double tmp59 = x0(1+_OFF); +double tmp60 = x1(1+_OFF); +double tmp61 = tmp46*tmp10; +double tmp62 = tmp7*tmp44; +double tmp63 = x0(2+_OFF); +double tmp64 = x1(2+_OFF); +double tmp65 = so(1+_OFF); +double tmp66 = -tmp59; +double tmp67 = 0. + tmp66 + tmp60; +double tmp68 = tmp42*tmp10; +double tmp69 = tmp13*tmp44; +double tmp70 = -tmp64; +double tmp71 = 0. + tmp63 + tmp70; +double tmp72 = 4*tmp42*tmp13; +double tmp73 = 4*tmp40*tmp16; +double tmp74 = tmp42*tmp7; +double tmp75 = tmp46*tmp13; +double tmp76 = -tmp54; +double tmp77 = 0. + tmp76 + tmp55; +double tmp78 = tmp13*tmp40; +double tmp79 = tmp42*tmp16; +double tmp80 = -0.01*tmp14; +double tmp81 = 0. + tmp80; +double tmp82 = 0.01*tmp11; +double tmp83 = 0. + tmp82; +double tmp84 = -2*tmp47*tmp49*tmp40*tmp51; +double tmp85 = 2*tmp47*tmp49*tmp42*tmp53; +double tmp86 = 4*tmp13*tmp81; +double tmp87 = tmp7*tmp81; +double tmp88 = -(tmp42*tmp16); +double tmp89 = tmp40*tmp10; +double tmp90 = -tmp60; +double tmp91 = 0. + tmp59 + tmp90; +double tmp92 = 4*tmp42*tmp10; +double tmp93 = 4*tmp83*tmp16; +double tmp94 = -tmp63; +double tmp95 = 0. + tmp94 + tmp64; +double tmp96 = tmp42*tmp13; +double tmp97 = tmp10*tmp81; +double tmp98 = 0.01*tmp8; +double tmp99 = 0. + tmp98; +double tmp100 = -(tmp46*tmp16); +double tmp101 = 2*tmp47*tmp65*tmp42*tmp51; +double tmp102 = 2*tmp47*tmp50*tmp42*tmp52; +double tmp103 = -2*tmp47*tmp65*tmp81*tmp52; +double tmp104 = -2*tmp47*tmp50*tmp46*tmp53; +double tmp105 = 4*tmp13*tmp99; +double tmp106 = tmp7*tmp99; +double tmp107 = tmp13*tmp81; +double tmp108 = tmp46*tmp16; +double tmp109 = -(tmp81*tmp16); +double tmp110 = 4*tmp46*tmp10; +double tmp111 = 4*tmp42*tmp16; +double tmp112 = tmp7*tmp46; +double tmp113 = tmp99*tmp10; +double tmp114 = tmp81*tmp16; +double tmp115 = -(tmp7*tmp16); +double tmp116 = tmp24 + tmp115; +double tmp117 = tmp13*tmp16; +double tmp118 = tmp37 + tmp117; +double tmp119 = -1 + tmp36 + tmp32; +double tmp120 = -(tmp1*tmp4); +double tmp121 = tmp28 + tmp120; +double tmp122 = tmp7*tmp16; +double tmp123 = tmp24 + tmp122; +double tmp124 = tmp1*tmp3; +double tmp125 = tmp2*tmp4; +double tmp126 = tmp124 + tmp125; +double tmp127 = -(tmp10*tmp16); +double tmp128 = tmp38 + tmp127; +double tmp129 = -tmp18; +double tmp130 = tmp17 + tmp129 + tmp19 + tmp22; +double tmp131 = -1 + tmp34 + tmp32; +double tmp132 = -1 + tmp34 + tmp36; +double tmp133 = -(tmp13*tmp16); +double tmp134 = tmp37 + tmp133; +double tmp135 = tmp10*tmp16; +double tmp136 = tmp38 + tmp135; +double tmp137 = -(tmp13*tmp40); +double tmp138 = tmp137 + tmp61 + tmp62 + tmp88; +double tmp139 = 2*tmp57*tmp138; +double tmp140 = tmp7*tmp40; +double tmp141 = tmp140 + tmp68 + tmp69 + tmp108; +double tmp142 = 2*tmp95*tmp141; +double tmp143 = tmp58 + tmp73; +double tmp144 = tmp91*tmp143; +double tmp145 = tmp139 + tmp142 + tmp144; +double tmp146 = tmp0*tmp145; +double tmp147 = 2*tmp47*tmp50*tmp40*tmp48; +double tmp148 = -2*tmp47*tmp49*tmp44*tmp48; +double tmp149 = -2*tmp47*tmp50*tmp44*tmp51; +double tmp150 = 2*tmp47*tmp49*tmp46*tmp52; +double tmp151 = tmp146 + tmp147 + tmp148 + tmp84 + tmp149 + tmp102 + tmp150 + tmp85 + tmp104; +double tmp152 = tmp72 + tmp58; +double tmp153 = tmp57*tmp152; +double tmp154 = tmp78 + tmp61 + tmp62 + tmp79; +double tmp155 = 2*tmp67*tmp154; +double tmp156 = -(tmp40*tmp10); +double tmp157 = -(tmp44*tmp16); +double tmp158 = tmp74 + tmp75 + tmp156 + tmp157; +double tmp159 = 2*tmp71*tmp158; +double tmp160 = tmp153 + tmp155 + tmp159; +double tmp161 = tmp0*tmp160; +double tmp162 = -2*tmp47*tmp50*tmp42*tmp48; +double tmp163 = 2*tmp47*tmp65*tmp44*tmp48; +double tmp164 = 2*tmp47*tmp50*tmp46*tmp51; +double tmp165 = 2*tmp47*tmp65*tmp40*tmp51; +double tmp166 = -2*tmp47*tmp65*tmp46*tmp52; +double tmp167 = 2*tmp47*tmp50*tmp40*tmp52; +double tmp168 = -2*tmp47*tmp65*tmp42*tmp53; +double tmp169 = -2*tmp47*tmp50*tmp44*tmp53; +double tmp170 = tmp161 + tmp162 + tmp163 + tmp164 + tmp165 + tmp166 + tmp167 + tmp168 + tmp169; +double tmp171 = -(tmp7*tmp40); +double tmp172 = tmp171 + tmp68 + tmp69 + tmp100; +double tmp173 = 2*tmp67*tmp172; +double tmp174 = tmp72 + tmp73; +double tmp175 = tmp71*tmp174; +double tmp176 = tmp44*tmp16; +double tmp177 = tmp74 + tmp75 + tmp89 + tmp176; +double tmp178 = 2*tmp77*tmp177; +double tmp179 = tmp173 + tmp175 + tmp178; +double tmp180 = tmp0*tmp179; +double tmp181 = 2*tmp47*tmp49*tmp42*tmp48; +double tmp182 = -2*tmp47*tmp65*tmp40*tmp48; +double tmp183 = -2*tmp47*tmp49*tmp46*tmp51; +double tmp184 = 2*tmp47*tmp65*tmp44*tmp51; +double tmp185 = -2*tmp47*tmp65*tmp42*tmp52; +double tmp186 = -2*tmp47*tmp49*tmp40*tmp52; +double tmp187 = 2*tmp47*tmp65*tmp46*tmp53; +double tmp188 = 2*tmp47*tmp49*tmp44*tmp53; +double tmp189 = tmp180 + tmp181 + tmp182 + tmp183 + tmp184 + tmp185 + tmp186 + tmp187 + tmp188; +double tmp190 = tmp83*tmp10; +double tmp191 = tmp78 + tmp190 + tmp87 + tmp79; +double tmp192 = 2*tmp77*tmp191; +double tmp193 = tmp86 + tmp93; +double tmp194 = tmp71*tmp193; +double tmp195 = -(tmp7*tmp83); +double tmp196 = -(tmp40*tmp16); +double tmp197 = tmp195 + tmp96 + tmp97 + tmp196; +double tmp198 = 2*tmp67*tmp197; +double tmp199 = tmp192 + tmp194 + tmp198; +double tmp200 = tmp0*tmp199; +double tmp201 = -2*tmp47*tmp65*tmp83*tmp48; +double tmp202 = 2*tmp47*tmp49*tmp81*tmp48; +double tmp203 = -2*tmp47*tmp49*tmp83*tmp52; +double tmp204 = 2*tmp47*tmp65*tmp40*tmp53; +double tmp205 = tmp200 + tmp201 + tmp202 + tmp101 + tmp84 + tmp203 + tmp103 + tmp85 + tmp204; +double tmp206 = tmp92 + tmp86; +double tmp207 = tmp57*tmp206; +double tmp208 = -(tmp83*tmp10); +double tmp209 = tmp78 + tmp208 + tmp87 + tmp88; +double tmp210 = 2*tmp71*tmp209; +double tmp211 = tmp83*tmp13; +double tmp212 = tmp74 + tmp211 + tmp89 + tmp114; +double tmp213 = 2*tmp67*tmp212; +double tmp214 = tmp207 + tmp210 + tmp213; +double tmp215 = tmp0*tmp214; +double tmp216 = 2*tmp47*tmp65*tmp42*tmp48; +double tmp217 = -2*tmp47*tmp50*tmp81*tmp48; +double tmp218 = 2*tmp47*tmp65*tmp83*tmp51; +double tmp219 = 2*tmp47*tmp50*tmp40*tmp51; +double tmp220 = 2*tmp47*tmp50*tmp83*tmp52; +double tmp221 = -2*tmp47*tmp65*tmp40*tmp52; +double tmp222 = -2*tmp47*tmp50*tmp42*tmp53; +double tmp223 = -2*tmp47*tmp65*tmp81*tmp53; +double tmp224 = tmp215 + tmp216 + tmp217 + tmp218 + tmp219 + tmp220 + tmp221 + tmp222 + tmp223; +double tmp225 = tmp92 + tmp93; +double tmp226 = tmp91*tmp225; +double tmp227 = tmp7*tmp83; +double tmp228 = tmp40*tmp16; +double tmp229 = tmp227 + tmp96 + tmp97 + tmp228; +double tmp230 = 2*tmp95*tmp229; +double tmp231 = -(tmp83*tmp13); +double tmp232 = tmp74 + tmp231 + tmp89 + tmp109; +double tmp233 = 2*tmp57*tmp232; +double tmp234 = tmp226 + tmp230 + tmp233; +double tmp235 = tmp0*tmp234; +double tmp236 = -2*tmp47*tmp49*tmp42*tmp48; +double tmp237 = 2*tmp47*tmp50*tmp83*tmp48; +double tmp238 = -2*tmp47*tmp50*tmp42*tmp51; +double tmp239 = -2*tmp47*tmp49*tmp83*tmp51; +double tmp240 = 2*tmp47*tmp49*tmp40*tmp52; +double tmp241 = 2*tmp47*tmp50*tmp81*tmp52; +double tmp242 = -2*tmp47*tmp50*tmp40*tmp53; +double tmp243 = 2*tmp47*tmp49*tmp81*tmp53; +double tmp244 = tmp235 + tmp236 + tmp237 + tmp238 + tmp239 + tmp240 + tmp241 + tmp242 + tmp243; +double tmp245 = tmp105 + tmp110; +double tmp246 = tmp57*tmp245; +double tmp247 = -(tmp42*tmp10); +double tmp248 = tmp106 + tmp247 + tmp107 + tmp100; +double tmp249 = 2*tmp71*tmp248; +double tmp250 = tmp99*tmp16; +double tmp251 = tmp112 + tmp96 + tmp97 + tmp250; +double tmp252 = 2*tmp67*tmp251; +double tmp253 = tmp246 + tmp249 + tmp252; +double tmp254 = tmp0*tmp253; +double tmp255 = 2*tmp47*tmp65*tmp46*tmp48; +double tmp256 = -2*tmp47*tmp50*tmp99*tmp48; +double tmp257 = 2*tmp47*tmp50*tmp81*tmp51; +double tmp258 = -2*tmp47*tmp65*tmp99*tmp53; +double tmp259 = tmp254 + tmp255 + tmp256 + tmp101 + tmp257 + tmp102 + tmp103 + tmp104 + tmp258; +double tmp260 = tmp105 + tmp111; +double tmp261 = tmp71*tmp260; +double tmp262 = tmp106 + tmp68 + tmp107 + tmp108; +double tmp263 = 2*tmp77*tmp262; +double tmp264 = -(tmp42*tmp7); +double tmp265 = tmp264 + tmp75 + tmp113 + tmp109; +double tmp266 = 2*tmp67*tmp265; +double tmp267 = tmp261 + tmp263 + tmp266; +double tmp268 = tmp0*tmp267; +double tmp269 = -2*tmp47*tmp65*tmp42*tmp48; +double tmp270 = 2*tmp47*tmp49*tmp99*tmp48; +double tmp271 = 2*tmp47*tmp65*tmp46*tmp51; +double tmp272 = -2*tmp47*tmp49*tmp81*tmp51; +double tmp273 = -2*tmp47*tmp49*tmp42*tmp52; +double tmp274 = -2*tmp47*tmp65*tmp99*tmp52; +double tmp275 = 2*tmp47*tmp49*tmp46*tmp53; +double tmp276 = 2*tmp47*tmp65*tmp81*tmp53; +double tmp277 = tmp268 + tmp269 + tmp270 + tmp271 + tmp272 + tmp273 + tmp274 + tmp275 + tmp276; +double tmp278 = tmp110 + tmp111; +double tmp279 = tmp91*tmp278; +double tmp280 = -(tmp42*tmp13); +double tmp281 = -(tmp99*tmp16); +double tmp282 = tmp112 + tmp280 + tmp97 + tmp281; +double tmp283 = 2*tmp57*tmp282; +double tmp284 = tmp74 + tmp75 + tmp113 + tmp114; +double tmp285 = 2*tmp95*tmp284; +double tmp286 = tmp279 + tmp283 + tmp285; +double tmp287 = tmp0*tmp286; +double tmp288 = 2*tmp47*tmp50*tmp42*tmp48; +double tmp289 = -2*tmp47*tmp49*tmp46*tmp48; +double tmp290 = -2*tmp47*tmp49*tmp42*tmp51; +double tmp291 = -2*tmp47*tmp50*tmp46*tmp51; +double tmp292 = 2*tmp47*tmp50*tmp99*tmp52; +double tmp293 = 2*tmp47*tmp49*tmp81*tmp52; +double tmp294 = 2*tmp47*tmp49*tmp99*tmp53; +double tmp295 = -2*tmp47*tmp50*tmp81*tmp53; +double tmp296 = tmp287 + tmp288 + tmp289 + tmp290 + tmp291 + tmp292 + tmp293 + tmp294 + tmp295; +double tmp297 = tmp1*tmp2; +double tmp298 = tmp297 + tmp26; +double tmp299 = tmp17 + tmp129 + tmp20 + tmp21; +double tmp300 = -(tmp1*tmp3); +double tmp301 = tmp300 + tmp125; +J(1+_OFF,1+_OFF) = 0.004*tmp0*tmp30*tmp116 + 0.004*tmp0*tmp27*tmp118 - 0.001*tmp0*tmp23*tmp119; +J(1+_OFF,2+_OFF) = 0.002*tmp0*tmp23*tmp123 - 0.004*tmp0*tmp27*tmp128 - 0.002*tmp0*tmp30*tmp131; +J(1+_OFF,3+_OFF) = -0.002*tmp0*tmp27*tmp132 - 0.002*tmp0*tmp23*tmp134 + 0.004*tmp0*tmp30*tmp136; +J(1+_OFF,4+_OFF) = tmp23*tmp151 + 2*tmp27*tmp170 + 2*tmp30*tmp189; +J(1+_OFF,5+_OFF) = 2*tmp30*tmp205 + 2*tmp27*tmp224 + tmp23*tmp244; +J(1+_OFF,6+_OFF) = 2*tmp27*tmp259 + 2*tmp30*tmp277 + tmp23*tmp296; +J(2+_OFF,1+_OFF) = 0.002*tmp0*tmp130*tmp116 + 0.004*tmp0*tmp126*tmp118 - 0.002*tmp0*tmp121*tmp119; +J(2+_OFF,2+_OFF) = 0.004*tmp0*tmp121*tmp123 - 0.004*tmp0*tmp126*tmp128 - 0.001*tmp0*tmp130*tmp131; +J(2+_OFF,3+_OFF) = -0.002*tmp0*tmp126*tmp132 - 0.004*tmp0*tmp121*tmp134 + 0.002*tmp0*tmp130*tmp136; +J(2+_OFF,4+_OFF) = 2*tmp121*tmp151 + 2*tmp126*tmp170 + tmp130*tmp189; +J(2+_OFF,5+_OFF) = tmp130*tmp205 + 2*tmp126*tmp224 + 2*tmp121*tmp244; +J(2+_OFF,6+_OFF) = 2*tmp126*tmp259 + tmp130*tmp277 + 2*tmp121*tmp296; +J(3+_OFF,1+_OFF) = 0.004*tmp0*tmp301*tmp116 + 0.002*tmp0*tmp299*tmp118 - 0.002*tmp0*tmp298*tmp119; +J(3+_OFF,2+_OFF) = 0.004*tmp0*tmp298*tmp123 - 0.002*tmp0*tmp299*tmp128 - 0.002*tmp0*tmp301*tmp131; +J(3+_OFF,3+_OFF) = -0.001*tmp0*tmp299*tmp132 - 0.004*tmp0*tmp298*tmp134 + 0.004*tmp0*tmp301*tmp136; +J(3+_OFF,4+_OFF) = 2*tmp298*tmp151 + tmp299*tmp170 + 2*tmp301*tmp189; +J(3+_OFF,5+_OFF) = 2*tmp301*tmp205 + tmp299*tmp224 + 2*tmp298*tmp244; +J(3+_OFF,6+_OFF) = tmp299*tmp259 + 2*tmp301*tmp277 + 2*tmp298*tmp296; diff --git a/roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_JAugVprevX2.cppready b/roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_JAugVprevX2.cppready new file mode 100644 index 0000000..70cf6fb --- /dev/null +++ b/roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_JAugVprevX2.cppready @@ -0,0 +1,139 @@ +double tmp0 = 1/Dt12; +double tmp1 = x2(4+_OFF); +double tmp2 = 0.01*tmp1; +double tmp3 = 0. + tmp2; +double tmp4 = so(3+_OFF); +double tmp5 = x1(6+_OFF); +double tmp6 = so(2+_OFF); +double tmp7 = x1(7+_OFF); +double tmp8 = x2(5+_OFF); +double tmp9 = -0.01*tmp8; +double tmp10 = 0. + tmp9; +double tmp11 = x2(6+_OFF); +double tmp12 = -0.01*tmp11; +double tmp13 = 0. + tmp12; +double tmp14 = x1(4+_OFF); +double tmp15 = x1(5+_OFF); +double tmp16 = x2(7+_OFF); +double tmp17 = 0.01*tmp16; +double tmp18 = 0. + tmp17; +double tmp19 = qos(1+_OFF); +double tmp20 = qos(3+_OFF); +double tmp21 = qos(2+_OFF); +double tmp22 = qos(4+_OFF); +double tmp23 = so(1+_OFF); +double tmp24 = tmp21*tmp20; +double tmp25 = tmp19*tmp22; +double tmp26 = tmp24 + tmp25; +double tmp27 = -2*tmp0*tmp4*tmp7*tmp3; +double tmp28 = 0.01*tmp8; +double tmp29 = 0. + tmp28; +double tmp30 = 2*tmp0*tmp4*tmp15*tmp13; +double tmp31 = -0.01*tmp16; +double tmp32 = 0. + tmp31; +double tmp33 = -(tmp19*tmp20); +double tmp34 = tmp21*tmp22; +double tmp35 = tmp33 + tmp34; +double tmp36 = std::pow(tmp19,2); +double tmp37 = std::pow(tmp21,2); +double tmp38 = std::pow(tmp20,2); +double tmp39 = -tmp38; +double tmp40 = std::pow(tmp22,2); +double tmp41 = -tmp40; +double tmp42 = tmp36 + tmp37 + tmp39 + tmp41; +double tmp43 = -2*tmp0*tmp23*tmp15*tmp3; +double tmp44 = -2*tmp0*tmp6*tmp5*tmp3; +double tmp45 = 2*tmp0*tmp6*tmp7*tmp10; +double tmp46 = 0.01*tmp11; +double tmp47 = 0. + tmp46; +double tmp48 = 2*tmp0*tmp23*tmp5*tmp32; +double tmp49 = -2*tmp0*tmp4*tmp5*tmp10; +double tmp50 = -2*tmp0*tmp6*tmp14*tmp13; +double tmp51 = 2*tmp0*tmp4*tmp14*tmp18; +double tmp52 = 2*tmp0*tmp6*tmp15*tmp18; +double tmp53 = tmp44 + tmp27 + tmp49 + tmp45 + tmp50 + tmp30 + tmp51 + tmp52; +double tmp54 = 2*tmp0*tmp6*tmp14*tmp3; +double tmp55 = 2*tmp0*tmp23*tmp7*tmp3; +double tmp56 = -2*tmp0*tmp6*tmp15*tmp10; +double tmp57 = 2*tmp0*tmp23*tmp5*tmp10; +double tmp58 = -2*tmp0*tmp23*tmp15*tmp13; +double tmp59 = -2*tmp0*tmp6*tmp5*tmp13; +double tmp60 = -2*tmp0*tmp23*tmp14*tmp18; +double tmp61 = 2*tmp0*tmp6*tmp7*tmp18; +double tmp62 = tmp54 + tmp55 + tmp56 + tmp57 + tmp58 + tmp59 + tmp60 + tmp61; +double tmp63 = -2*tmp0*tmp4*tmp14*tmp3; +double tmp64 = 2*tmp0*tmp23*tmp5*tmp3; +double tmp65 = 2*tmp0*tmp4*tmp15*tmp10; +double tmp66 = -2*tmp0*tmp23*tmp7*tmp10; +double tmp67 = 2*tmp0*tmp23*tmp14*tmp13; +double tmp68 = 2*tmp0*tmp4*tmp5*tmp13; +double tmp69 = -2*tmp0*tmp23*tmp15*tmp18; +double tmp70 = -2*tmp0*tmp4*tmp7*tmp18; +double tmp71 = tmp63 + tmp64 + tmp65 + tmp66 + tmp67 + tmp68 + tmp69 + tmp70; +double tmp72 = -tmp37; +double tmp73 = tmp36 + tmp72 + tmp38 + tmp41; +double tmp74 = 2*tmp0*tmp23*tmp14*tmp29; +double tmp75 = 2*tmp0*tmp4*tmp5*tmp29; +double tmp76 = -2*tmp0*tmp23*tmp7*tmp13; +double tmp77 = -2*tmp0*tmp4*tmp14*tmp32; +double tmp78 = tmp43 + tmp27 + tmp74 + tmp75 + tmp30 + tmp76 + tmp77 + tmp48; +double tmp79 = tmp19*tmp21; +double tmp80 = tmp20*tmp22; +double tmp81 = tmp79 + tmp80; +double tmp82 = -2*tmp0*tmp23*tmp14*tmp3; +double tmp83 = 2*tmp0*tmp6*tmp7*tmp3; +double tmp84 = -2*tmp0*tmp23*tmp15*tmp29; +double tmp85 = -2*tmp0*tmp6*tmp5*tmp29; +double tmp86 = -2*tmp0*tmp6*tmp15*tmp13; +double tmp87 = 2*tmp0*tmp23*tmp5*tmp13; +double tmp88 = 2*tmp0*tmp6*tmp14*tmp32; +double tmp89 = 2*tmp0*tmp23*tmp7*tmp32; +double tmp90 = tmp82 + tmp83 + tmp84 + tmp85 + tmp86 + tmp87 + tmp88 + tmp89; +double tmp91 = -(tmp19*tmp22); +double tmp92 = tmp24 + tmp91; +double tmp93 = 2*tmp0*tmp4*tmp14*tmp3; +double tmp94 = 2*tmp0*tmp6*tmp15*tmp3; +double tmp95 = -2*tmp0*tmp6*tmp14*tmp29; +double tmp96 = 2*tmp0*tmp4*tmp15*tmp29; +double tmp97 = -2*tmp0*tmp4*tmp5*tmp13; +double tmp98 = 2*tmp0*tmp6*tmp7*tmp13; +double tmp99 = -2*tmp0*tmp6*tmp5*tmp32; +double tmp100 = -2*tmp0*tmp4*tmp7*tmp32; +double tmp101 = tmp93 + tmp94 + tmp95 + tmp96 + tmp97 + tmp98 + tmp99 + tmp100; +double tmp102 = -2*tmp0*tmp23*tmp14*tmp10; +double tmp103 = 2*tmp0*tmp6*tmp14*tmp47; +double tmp104 = 2*tmp0*tmp23*tmp7*tmp47; +double tmp105 = -2*tmp0*tmp6*tmp15*tmp32; +double tmp106 = tmp43 + tmp44 + tmp102 + tmp45 + tmp103 + tmp104 + tmp105 + tmp48; +double tmp107 = 2*tmp0*tmp23*tmp14*tmp3; +double tmp108 = 2*tmp0*tmp4*tmp5*tmp3; +double tmp109 = -2*tmp0*tmp23*tmp15*tmp10; +double tmp110 = -2*tmp0*tmp4*tmp7*tmp10; +double tmp111 = -2*tmp0*tmp4*tmp14*tmp47; +double tmp112 = 2*tmp0*tmp23*tmp5*tmp47; +double tmp113 = 2*tmp0*tmp4*tmp15*tmp32; +double tmp114 = -2*tmp0*tmp23*tmp7*tmp32; +double tmp115 = tmp107 + tmp108 + tmp109 + tmp110 + tmp111 + tmp112 + tmp113 + tmp114; +double tmp116 = -2*tmp0*tmp6*tmp14*tmp3; +double tmp117 = 2*tmp0*tmp4*tmp15*tmp3; +double tmp118 = 2*tmp0*tmp4*tmp14*tmp10; +double tmp119 = 2*tmp0*tmp6*tmp15*tmp10; +double tmp120 = -2*tmp0*tmp6*tmp5*tmp47; +double tmp121 = -2*tmp0*tmp4*tmp7*tmp47; +double tmp122 = -2*tmp0*tmp4*tmp5*tmp32; +double tmp123 = 2*tmp0*tmp6*tmp7*tmp32; +double tmp124 = tmp116 + tmp117 + tmp118 + tmp119 + tmp120 + tmp121 + tmp122 + tmp123; +double tmp125 = -(tmp19*tmp21); +double tmp126 = tmp125 + tmp80; +double tmp127 = tmp36 + tmp72 + tmp39 + tmp40; +double tmp128 = tmp19*tmp20; +double tmp129 = tmp128 + tmp34; +J(1+_OFF,4+_OFF) = tmp42*tmp53 + 2*tmp35*tmp62 + 2*tmp26*tmp71; +J(1+_OFF,5+_OFF) = 2*tmp26*tmp78 + 2*tmp35*tmp90 + tmp42*tmp101; +J(1+_OFF,6+_OFF) = 2*tmp35*tmp106 + 2*tmp26*tmp115 + tmp42*tmp124; +J(2+_OFF,4+_OFF) = 2*tmp92*tmp53 + 2*tmp81*tmp62 + tmp73*tmp71; +J(2+_OFF,5+_OFF) = tmp73*tmp78 + 2*tmp81*tmp90 + 2*tmp92*tmp101; +J(2+_OFF,6+_OFF) = 2*tmp81*tmp106 + tmp73*tmp115 + 2*tmp92*tmp124; +J(3+_OFF,4+_OFF) = 2*tmp129*tmp53 + tmp127*tmp62 + 2*tmp126*tmp71; +J(3+_OFF,5+_OFF) = 2*tmp126*tmp78 + tmp127*tmp90 + 2*tmp129*tmp101; +J(3+_OFF,6+_OFF) = tmp127*tmp106 + 2*tmp126*tmp115 + 2*tmp129*tmp124; diff --git a/roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_JAugVprevqOS.cppready b/roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_JAugVprevqOS.cppready new file mode 100644 index 0000000..af6cc9a --- /dev/null +++ b/roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_JAugVprevqOS.cppready @@ -0,0 +1,161 @@ +double tmp0 = qos(1+_OFF); +double tmp1 = qos(2+_OFF); +double tmp2 = qos(3+_OFF); +double tmp3 = qos(4+_OFF); +double tmp4 = x1(4+_OFF); +double tmp5 = x1(6+_OFF); +double tmp6 = x1(5+_OFF); +double tmp7 = x1(7+_OFF); +double tmp8 = 1/Dt12; +double tmp9 = x2(4+_OFF); +double tmp10 = so(2+_OFF); +double tmp11 = so(3+_OFF); +double tmp12 = x2(5+_OFF); +double tmp13 = x2(6+_OFF); +double tmp14 = x2(7+_OFF); +double tmp15 = 1.*tmp1; +double tmp16 = 0. + tmp15; +double tmp17 = -0.01*tmp2; +double tmp18 = 0. + tmp17; +double tmp19 = -0.01*tmp1; +double tmp20 = 0. + tmp19; +double tmp21 = 1.*tmp2; +double tmp22 = 0. + tmp21; +double tmp23 = 1.*tmp0; +double tmp24 = 0. + tmp23; +double tmp25 = 0.01*tmp3; +double tmp26 = 0. + tmp25; +double tmp27 = 0.01*tmp0; +double tmp28 = 0. + tmp27; +double tmp29 = 1.*tmp3; +double tmp30 = 0. + tmp29; +double tmp31 = 1/Dt01; +double tmp32 = x0(3+_OFF); +double tmp33 = x1(3+_OFF); +double tmp34 = -tmp33; +double tmp35 = tmp32 + tmp34; +double tmp36 = std::pow(tmp5,2); +double tmp37 = 2*tmp36; +double tmp38 = x0(1+_OFF); +double tmp39 = x1(1+_OFF); +double tmp40 = tmp4*tmp5; +double tmp41 = x0(2+_OFF); +double tmp42 = x1(2+_OFF); +double tmp43 = so(1+_OFF); +double tmp44 = -tmp38; +double tmp45 = tmp44 + tmp39; +double tmp46 = tmp6*tmp5; +double tmp47 = tmp4*tmp6; +double tmp48 = -tmp42; +double tmp49 = tmp41 + tmp48; +double tmp50 = std::pow(tmp6,2); +double tmp51 = 2*tmp50; +double tmp52 = std::pow(tmp7,2); +double tmp53 = 2*tmp52; +double tmp54 = -tmp41; +double tmp55 = tmp54 + tmp42; +double tmp56 = tmp4*tmp7; +double tmp57 = tmp46 + tmp56; +double tmp58 = 2*tmp55*tmp57; +double tmp59 = -(tmp6*tmp7); +double tmp60 = tmp40 + tmp59; +double tmp61 = 2*tmp35*tmp60; +double tmp62 = -tmp39; +double tmp63 = tmp38 + tmp62; +double tmp64 = -1 + tmp37 + tmp53; +double tmp65 = tmp63*tmp64; +double tmp66 = tmp58 + tmp61 + tmp65; +double tmp67 = tmp31*tmp66; +double tmp68 = -2*tmp8*tmp11*tmp5*tmp9; +double tmp69 = 2*tmp8*tmp10*tmp7*tmp9; +double tmp70 = -2*tmp8*tmp10*tmp5*tmp12; +double tmp71 = -2*tmp8*tmp11*tmp7*tmp12; +double tmp72 = 2*tmp8*tmp11*tmp4*tmp13; +double tmp73 = 2*tmp8*tmp10*tmp6*tmp13; +double tmp74 = -2*tmp8*tmp10*tmp4*tmp14; +double tmp75 = 2*tmp8*tmp11*tmp6*tmp14; +double tmp76 = tmp67 + tmp68 + tmp69 + tmp70 + tmp71 + tmp72 + tmp73 + tmp74 + tmp75; +double tmp77 = 0.01*tmp1; +double tmp78 = 0. + tmp77; +double tmp79 = -0.01*tmp3; +double tmp80 = 0. + tmp79; +double tmp81 = -1 + tmp51 + tmp37; +double tmp82 = tmp35*tmp81; +double tmp83 = tmp6*tmp7; +double tmp84 = tmp40 + tmp83; +double tmp85 = 2*tmp45*tmp84; +double tmp86 = -(tmp5*tmp7); +double tmp87 = tmp47 + tmp86; +double tmp88 = 2*tmp49*tmp87; +double tmp89 = tmp82 + tmp85 + tmp88; +double tmp90 = tmp31*tmp89; +double tmp91 = -2*tmp8*tmp10*tmp6*tmp9; +double tmp92 = 2*tmp8*tmp43*tmp5*tmp9; +double tmp93 = 2*tmp8*tmp10*tmp4*tmp12; +double tmp94 = 2*tmp8*tmp43*tmp7*tmp12; +double tmp95 = -2*tmp8*tmp43*tmp4*tmp13; +double tmp96 = 2*tmp8*tmp10*tmp7*tmp13; +double tmp97 = -2*tmp8*tmp43*tmp6*tmp14; +double tmp98 = -2*tmp8*tmp10*tmp5*tmp14; +double tmp99 = tmp90 + tmp91 + tmp92 + tmp93 + tmp94 + tmp95 + tmp96 + tmp97 + tmp98; +double tmp100 = -(tmp4*tmp7); +double tmp101 = tmp46 + tmp100; +double tmp102 = 2*tmp45*tmp101; +double tmp103 = -tmp32; +double tmp104 = tmp103 + tmp33; +double tmp105 = tmp5*tmp7; +double tmp106 = tmp47 + tmp105; +double tmp107 = 2*tmp104*tmp106; +double tmp108 = -1 + tmp51 + tmp53; +double tmp109 = tmp49*tmp108; +double tmp110 = tmp102 + tmp107 + tmp109; +double tmp111 = tmp31*tmp110; +double tmp112 = 2*tmp8*tmp11*tmp6*tmp9; +double tmp113 = -2*tmp8*tmp43*tmp7*tmp9; +double tmp114 = -2*tmp8*tmp11*tmp4*tmp12; +double tmp115 = 2*tmp8*tmp43*tmp5*tmp12; +double tmp116 = -2*tmp8*tmp43*tmp6*tmp13; +double tmp117 = -2*tmp8*tmp11*tmp7*tmp13; +double tmp118 = 2*tmp8*tmp43*tmp4*tmp14; +double tmp119 = 2*tmp8*tmp11*tmp5*tmp14; +double tmp120 = tmp111 + tmp112 + tmp113 + tmp114 + tmp115 + tmp116 + tmp117 + tmp118 + tmp119; +double tmp121 = tmp28*tmp16; +double tmp122 = 0.01*tmp2; +double tmp123 = 0. + tmp122; +double tmp124 = tmp80*tmp30; +double tmp125 = tmp28*tmp22; +double tmp126 = tmp16*tmp26; +double tmp127 = tmp28*tmp24; +double tmp128 = tmp20*tmp16; +double tmp129 = 2*tmp24*tmp20; +double tmp130 = -2*tmp18*tmp30; +double tmp131 = tmp22*tmp80; +double tmp132 = tmp16*tmp18; +double tmp133 = tmp28*tmp30; +double tmp134 = 2*tmp24*tmp18; +double tmp135 = -2*tmp78*tmp30; +double tmp136 = -(tmp28*tmp24); +double tmp137 = tmp123*tmp22; +double tmp138 = tmp20*tmp30; +double tmp139 = 2*tmp24*tmp80; +double tmp140 = -2*tmp28*tmp30; +double tmp141 = -2*tmp28*tmp16; +double tmp142 = -2*tmp22*tmp26; +double tmp143 = tmp18*tmp22; +double tmp144 = tmp26*tmp30; +double tmp145 = tmp78*tmp16; +double tmp146 = -2*tmp28*tmp22; +double tmp147 = -2*tmp16*tmp80; +double tmp148 = tmp78*tmp22; +double tmp149 = tmp123*tmp30; +double tmp150 = -2*tmp16*tmp123; +double tmp151 = -2*tmp20*tmp22; +J(1+_OFF,1+_OFF) = (tmp129 + 2*tmp28*tmp16 + tmp142 + tmp130)*tmp76 + 2*(tmp132 - tmp20*tmp22 - tmp24*tmp26 + tmp133)*tmp99 + 2*(tmp24*tmp18 + tmp125 + tmp126 + tmp138)*tmp120; +J(1+_OFF,2+_OFF) = (tmp134 + tmp146 + 2*tmp16*tmp80 + tmp135)*tmp76 + 2*(tmp136 + tmp145 - tmp18*tmp22 + tmp124)*tmp99 + 2*(tmp24*tmp78 + tmp121 + tmp131 + tmp18*tmp30)*tmp120; +J(1+_OFF,3+_OFF) = (2*tmp16*tmp123 + tmp151 + tmp139 + tmp140)*tmp76 + 2*(-(tmp24*tmp20) + tmp121 - tmp22*tmp80 + tmp149)*tmp99 + 2*(tmp127 + tmp128 + tmp137 + tmp124)*tmp120; +J(2+_OFF,1+_OFF) = 2*(-(tmp24*tmp18) + tmp125 + tmp126 - tmp20*tmp30)*tmp76 + 2*(tmp127 + tmp128 + tmp143 + tmp144)*tmp99 + (tmp129 + tmp141 + 2*tmp22*tmp26 + tmp130)*tmp120; +J(2+_OFF,2+_OFF) = 2*(-(tmp24*tmp78) + tmp121 + tmp131 - tmp18*tmp30)*tmp76 + 2*(tmp132 + tmp148 + tmp24*tmp80 + tmp133)*tmp99 + (tmp134 + 2*tmp28*tmp22 + tmp147 + tmp135)*tmp120; +J(2+_OFF,3+_OFF) = 2*(tmp136 + tmp128 + tmp137 - tmp80*tmp30)*tmp76 + 2*(tmp24*tmp123 + tmp125 + tmp16*tmp80 + tmp138)*tmp99 + (tmp150 + 2*tmp20*tmp22 + tmp139 + tmp140)*tmp120; +J(3+_OFF,1+_OFF) = 2*(tmp132 + tmp20*tmp22 + tmp24*tmp26 + tmp133)*tmp76 + (tmp129 + tmp141 + tmp142 + 2*tmp18*tmp30)*tmp99 + 2*(tmp136 - tmp20*tmp16 + tmp143 + tmp144)*tmp120; +J(3+_OFF,2+_OFF) = 2*(tmp127 + tmp145 + tmp143 + tmp124)*tmp76 + (tmp134 + tmp146 + tmp147 + 2*tmp78*tmp30)*tmp99 + 2*(-(tmp16*tmp18) + tmp148 - tmp24*tmp80 + tmp133)*tmp120; +J(3+_OFF,3+_OFF) = 2*(tmp24*tmp20 + tmp121 + tmp131 + tmp149)*tmp76 + (tmp150 + tmp151 + tmp139 + 2*tmp28*tmp30)*tmp99 + 2*(-(tmp24*tmp123) + tmp125 - tmp16*tmp80 + tmp138)*tmp120; diff --git a/roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_JAugWprevSO.cppready b/roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_JAugWprevSO.cppready new file mode 100644 index 0000000..e69de29 diff --git a/roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_JAugWprevX0.cppready b/roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_JAugWprevX0.cppready new file mode 100644 index 0000000..6b90955 --- /dev/null +++ b/roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_JAugWprevX0.cppready @@ -0,0 +1,165 @@ +double tmp0 = x0(7+_OFF); +double tmp1 = x0(6+_OFF); +double tmp2 = x0(5+_OFF); +double tmp3 = x0(4+_OFF); +double tmp4 = x1(4+_OFF); +double tmp5 = x1(5+_OFF); +double tmp6 = x1(6+_OFF); +double tmp7 = x1(7+_OFF); +double tmp8 = qos(2+_OFF); +double tmp9 = qos(1+_OFF); +double tmp10 = qos(4+_OFF); +double tmp11 = qos(3+_OFF); +double tmp12 = 1/Dt01; +double tmp13 = 1.*tmp3; +double tmp14 = 0. + tmp13; +double tmp15 = -0.01*tmp2; +double tmp16 = 0. + tmp15; +double tmp17 = 0.01*tmp3; +double tmp18 = 0. + tmp17; +double tmp19 = 1.*tmp2; +double tmp20 = 0. + tmp19; +double tmp21 = 1.*tmp1; +double tmp22 = 0. + tmp21; +double tmp23 = 0.01*tmp0; +double tmp24 = 0. + tmp23; +double tmp25 = -0.01*tmp1; +double tmp26 = 0. + tmp25; +double tmp27 = 1.*tmp0; +double tmp28 = 0. + tmp27; +double tmp29 = std::pow(tmp14,2); +double tmp30 = std::pow(tmp20,2); +double tmp31 = std::pow(tmp22,2); +double tmp32 = std::pow(tmp28,2); +double tmp33 = tmp29 + tmp30 + tmp31 + tmp32; +double tmp34 = -(tmp11*tmp4); +double tmp35 = tmp10*tmp5; +double tmp36 = -(tmp9*tmp6); +double tmp37 = -(tmp8*tmp7); +double tmp38 = tmp34 + tmp35 + tmp36 + tmp37; +double tmp39 = -(tmp8*tmp4); +double tmp40 = -(tmp9*tmp5); +double tmp41 = -(tmp10*tmp6); +double tmp42 = tmp11*tmp7; +double tmp43 = tmp39 + tmp40 + tmp41 + tmp42; +double tmp44 = tmp38*tmp43; +double tmp45 = -(tmp10*tmp4); +double tmp46 = -(tmp11*tmp5); +double tmp47 = tmp8*tmp6; +double tmp48 = -(tmp9*tmp7); +double tmp49 = tmp45 + tmp46 + tmp47 + tmp48; +double tmp50 = tmp9*tmp4; +double tmp51 = -(tmp8*tmp5); +double tmp52 = -(tmp11*tmp6); +double tmp53 = -(tmp10*tmp7); +double tmp54 = tmp50 + tmp51 + tmp52 + tmp53; +double tmp55 = -(tmp49*tmp54); +double tmp56 = tmp44 + tmp55; +double tmp57 = 2*tmp14*tmp16; +double tmp58 = 2*tmp18*tmp20; +double tmp59 = 2*tmp22*tmp24; +double tmp60 = 2*tmp26*tmp28; +double tmp61 = tmp57 + tmp58 + tmp59 + tmp60; +double tmp62 = std::pow(tmp33,-2); +double tmp63 = 1/tmp33; +double tmp64 = tmp49*tmp43; +double tmp65 = tmp38*tmp54; +double tmp66 = tmp64 + tmp65; +double tmp67 = std::pow(tmp49,2); +double tmp68 = -tmp67; +double tmp69 = std::pow(tmp38,2); +double tmp70 = -tmp69; +double tmp71 = std::pow(tmp43,2); +double tmp72 = std::pow(tmp54,2); +double tmp73 = tmp68 + tmp70 + tmp71 + tmp72; +double tmp74 = tmp22*tmp4; +double tmp75 = -(tmp28*tmp5); +double tmp76 = -(tmp14*tmp6); +double tmp77 = tmp20*tmp7; +double tmp78 = tmp74 + tmp75 + tmp76 + tmp77; +double tmp79 = tmp18*tmp4; +double tmp80 = 0.01*tmp2; +double tmp81 = 0. + tmp80; +double tmp82 = -0.01*tmp0; +double tmp83 = 0. + tmp82; +double tmp84 = 2*tmp14*tmp26; +double tmp85 = 2*tmp18*tmp22; +double tmp86 = 2*tmp20*tmp83; +double tmp87 = 2*tmp81*tmp28; +double tmp88 = tmp84 + tmp85 + tmp86 + tmp87; +double tmp89 = -(tmp28*tmp4); +double tmp90 = -(tmp22*tmp5); +double tmp91 = tmp20*tmp6; +double tmp92 = tmp14*tmp7; +double tmp93 = tmp89 + tmp90 + tmp91 + tmp92; +double tmp94 = -(tmp26*tmp5); +double tmp95 = tmp20*tmp4; +double tmp96 = -(tmp14*tmp5); +double tmp97 = tmp28*tmp6; +double tmp98 = -(tmp22*tmp7); +double tmp99 = tmp95 + tmp96 + tmp97 + tmp98; +double tmp100 = -(tmp18*tmp5); +double tmp101 = 0.01*tmp1; +double tmp102 = 0. + tmp101; +double tmp103 = 2*tmp20*tmp102; +double tmp104 = 2*tmp16*tmp22; +double tmp105 = 2*tmp14*tmp83; +double tmp106 = 2*tmp18*tmp28; +double tmp107 = tmp103 + tmp104 + tmp105 + tmp106; +double tmp108 = -(tmp16*tmp5); +double tmp109 = tmp83*tmp7; +double tmp110 = tmp18*tmp6; +double tmp111 = tmp26*tmp6; +double tmp112 = -(tmp24*tmp7); +double tmp113 = tmp79 + tmp108 + tmp111 + tmp112; +double tmp114 = tmp49*tmp54; +double tmp115 = tmp44 + tmp114; +double tmp116 = -(tmp26*tmp4); +double tmp117 = -(tmp24*tmp5); +double tmp118 = tmp16*tmp7; +double tmp119 = tmp116 + tmp117 + tmp110 + tmp118; +double tmp120 = tmp49*tmp38; +double tmp121 = -(tmp43*tmp54); +double tmp122 = tmp120 + tmp121; +double tmp123 = tmp24*tmp4; +double tmp124 = -(tmp16*tmp6); +double tmp125 = tmp18*tmp7; +double tmp126 = tmp123 + tmp94 + tmp124 + tmp125; +double tmp127 = -tmp71; +double tmp128 = tmp68 + tmp69 + tmp127 + tmp72; +double tmp129 = tmp83*tmp4; +double tmp130 = tmp81*tmp6; +double tmp131 = -(tmp18*tmp7); +double tmp132 = tmp129 + tmp94 + tmp130 + tmp131; +double tmp133 = -(tmp81*tmp4); +double tmp134 = tmp83*tmp6; +double tmp135 = tmp26*tmp7; +double tmp136 = tmp133 + tmp100 + tmp134 + tmp135; +double tmp137 = -(tmp81*tmp5); +double tmp138 = -(tmp26*tmp6); +double tmp139 = tmp79 + tmp137 + tmp138 + tmp109; +double tmp140 = tmp102*tmp4; +double tmp141 = -(tmp83*tmp5); +double tmp142 = -(tmp16*tmp7); +double tmp143 = tmp140 + tmp141 + tmp110 + tmp142; +double tmp144 = -(tmp18*tmp4); +double tmp145 = tmp102*tmp6; +double tmp146 = tmp144 + tmp108 + tmp145 + tmp109; +double tmp147 = tmp16*tmp4; +double tmp148 = -(tmp83*tmp6); +double tmp149 = tmp102*tmp7; +double tmp150 = tmp147 + tmp100 + tmp148 + tmp149; +double tmp151 = -(tmp38*tmp54); +double tmp152 = tmp64 + tmp151; +double tmp153 = tmp43*tmp54; +double tmp154 = tmp120 + tmp153; +double tmp155 = tmp67 + tmp70 + tmp127 + tmp72; +J(1+_OFF,4+_OFF) = -4*tmp12*tmp63*tmp126*tmp56 + 4*tmp12*tmp61*tmp62*tmp78*tmp56 - 4*tmp12*tmp61*tmp62*tmp93*tmp66 + 4*tmp12*tmp63*tmp119*tmp66 + 2*tmp12*tmp61*tmp62*tmp99*tmp73 - 2*tmp12*tmp63*tmp113*tmp73; +J(1+_OFF,5+_OFF) = 4*tmp12*tmp88*tmp62*tmp78*tmp56 - 4*tmp12*tmp63*tmp139*tmp56 - 4*tmp12*tmp88*tmp62*tmp93*tmp66 + 4*tmp12*tmp63*tmp136*tmp66 - 2*tmp12*tmp63*tmp132*tmp73 + 2*tmp12*tmp88*tmp62*tmp99*tmp73; +J(1+_OFF,6+_OFF) = 4*tmp12*tmp107*tmp62*tmp78*tmp56 - 4*tmp12*tmp63*tmp150*tmp56 - 4*tmp12*tmp107*tmp62*tmp93*tmp66 + 4*tmp12*tmp63*tmp146*tmp66 - 2*tmp12*tmp63*tmp143*tmp73 + 2*tmp12*tmp107*tmp62*tmp99*tmp73; +J(2+_OFF,4+_OFF) = 4*tmp12*tmp61*tmp62*tmp99*tmp115 - 4*tmp12*tmp63*tmp113*tmp115 - 4*tmp12*tmp61*tmp62*tmp93*tmp122 + 4*tmp12*tmp63*tmp119*tmp122 - 2*tmp12*tmp63*tmp126*tmp128 + 2*tmp12*tmp61*tmp62*tmp78*tmp128; +J(2+_OFF,5+_OFF) = -4*tmp12*tmp63*tmp132*tmp115 + 4*tmp12*tmp88*tmp62*tmp99*tmp115 - 4*tmp12*tmp88*tmp62*tmp93*tmp122 + 4*tmp12*tmp63*tmp136*tmp122 + 2*tmp12*tmp88*tmp62*tmp78*tmp128 - 2*tmp12*tmp63*tmp139*tmp128; +J(2+_OFF,6+_OFF) = -4*tmp12*tmp63*tmp143*tmp115 + 4*tmp12*tmp107*tmp62*tmp99*tmp115 - 4*tmp12*tmp107*tmp62*tmp93*tmp122 + 4*tmp12*tmp63*tmp146*tmp122 + 2*tmp12*tmp107*tmp62*tmp78*tmp128 - 2*tmp12*tmp63*tmp150*tmp128; +J(3+_OFF,4+_OFF) = 4*tmp12*tmp61*tmp62*tmp99*tmp152 - 4*tmp12*tmp63*tmp113*tmp152 - 4*tmp12*tmp63*tmp126*tmp154 + 4*tmp12*tmp61*tmp62*tmp78*tmp154 - 2*tmp12*tmp61*tmp62*tmp93*tmp155 + 2*tmp12*tmp63*tmp119*tmp155; +J(3+_OFF,5+_OFF) = -4*tmp12*tmp63*tmp132*tmp152 + 4*tmp12*tmp88*tmp62*tmp99*tmp152 + 4*tmp12*tmp88*tmp62*tmp78*tmp154 - 4*tmp12*tmp63*tmp139*tmp154 - 2*tmp12*tmp88*tmp62*tmp93*tmp155 + 2*tmp12*tmp63*tmp136*tmp155; +J(3+_OFF,6+_OFF) = -4*tmp12*tmp63*tmp143*tmp152 + 4*tmp12*tmp107*tmp62*tmp99*tmp152 + 4*tmp12*tmp107*tmp62*tmp78*tmp154 - 4*tmp12*tmp63*tmp150*tmp154 - 2*tmp12*tmp107*tmp62*tmp93*tmp155 + 2*tmp12*tmp63*tmp146*tmp155; diff --git a/roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_JAugWprevX1.cppready b/roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_JAugWprevX1.cppready new file mode 100644 index 0000000..9e05203 --- /dev/null +++ b/roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_JAugWprevX1.cppready @@ -0,0 +1,228 @@ +double tmp0 = x0(7+_OFF); +double tmp1 = x0(6+_OFF); +double tmp2 = x0(5+_OFF); +double tmp3 = x0(4+_OFF); +double tmp4 = x1(4+_OFF); +double tmp5 = x1(5+_OFF); +double tmp6 = x1(6+_OFF); +double tmp7 = x1(7+_OFF); +double tmp8 = qos(4+_OFF); +double tmp9 = 1.*tmp4; +double tmp10 = 0. + tmp9; +double tmp11 = qos(3+_OFF); +double tmp12 = 1.*tmp5; +double tmp13 = 0. + tmp12; +double tmp14 = qos(2+_OFF); +double tmp15 = 1.*tmp6; +double tmp16 = 0. + tmp15; +double tmp17 = qos(1+_OFF); +double tmp18 = 1.*tmp7; +double tmp19 = 0. + tmp18; +double tmp20 = 0.01*tmp4; +double tmp21 = 0. + tmp20; +double tmp22 = -0.01*tmp5; +double tmp23 = 0. + tmp22; +double tmp24 = -0.01*tmp6; +double tmp25 = 0. + tmp24; +double tmp26 = 0.01*tmp7; +double tmp27 = 0. + tmp26; +double tmp28 = 1/Dt01; +double tmp29 = std::pow(tmp3,2); +double tmp30 = std::pow(tmp2,2); +double tmp31 = std::pow(tmp1,2); +double tmp32 = std::pow(tmp0,2); +double tmp33 = tmp29 + tmp30 + tmp31 + tmp32; +double tmp34 = 1/tmp33; +double tmp35 = -(tmp14*tmp21); +double tmp36 = tmp17*tmp23; +double tmp37 = -(tmp8*tmp25); +double tmp38 = -(tmp11*tmp27); +double tmp39 = tmp35 + tmp36 + tmp37 + tmp38; +double tmp40 = -(tmp8*tmp10); +double tmp41 = -(tmp11*tmp13); +double tmp42 = tmp14*tmp16; +double tmp43 = -(tmp17*tmp19); +double tmp44 = tmp40 + tmp41 + tmp42 + tmp43; +double tmp45 = -(tmp17*tmp21); +double tmp46 = -(tmp14*tmp23); +double tmp47 = tmp11*tmp25; +double tmp48 = -(tmp8*tmp27); +double tmp49 = tmp45 + tmp46 + tmp47 + tmp48; +double tmp50 = -(tmp11*tmp10); +double tmp51 = tmp8*tmp13; +double tmp52 = -(tmp17*tmp16); +double tmp53 = -(tmp14*tmp19); +double tmp54 = tmp50 + tmp51 + tmp52 + tmp53; +double tmp55 = tmp8*tmp21; +double tmp56 = -(tmp11*tmp23); +double tmp57 = -(tmp14*tmp25); +double tmp58 = -(tmp17*tmp27); +double tmp59 = tmp55 + tmp56 + tmp57 + tmp58; +double tmp60 = -(tmp14*tmp10); +double tmp61 = -(tmp17*tmp13); +double tmp62 = -(tmp8*tmp16); +double tmp63 = tmp11*tmp19; +double tmp64 = tmp60 + tmp61 + tmp62 + tmp63; +double tmp65 = -(tmp11*tmp21); +double tmp66 = -(tmp8*tmp23); +double tmp67 = -(tmp17*tmp25); +double tmp68 = tmp14*tmp27; +double tmp69 = tmp65 + tmp66 + tmp67 + tmp68; +double tmp70 = tmp17*tmp10; +double tmp71 = -(tmp14*tmp13); +double tmp72 = -(tmp11*tmp16); +double tmp73 = -(tmp8*tmp19); +double tmp74 = tmp70 + tmp71 + tmp72 + tmp73; +double tmp75 = tmp2*tmp10; +double tmp76 = -(tmp3*tmp13); +double tmp77 = tmp0*tmp16; +double tmp78 = -(tmp1*tmp19); +double tmp79 = tmp75 + tmp76 + tmp77 + tmp78; +double tmp80 = 0.01*tmp5; +double tmp81 = 0. + tmp80; +double tmp82 = -0.01*tmp7; +double tmp83 = 0. + tmp82; +double tmp84 = tmp1*tmp10; +double tmp85 = -(tmp0*tmp13); +double tmp86 = -(tmp3*tmp16); +double tmp87 = tmp2*tmp19; +double tmp88 = tmp84 + tmp85 + tmp86 + tmp87; +double tmp89 = -(tmp8*tmp81); +double tmp90 = tmp17*tmp25; +double tmp91 = -(tmp14*tmp83); +double tmp92 = tmp65 + tmp89 + tmp90 + tmp91; +double tmp93 = -(tmp8*tmp21); +double tmp94 = tmp11*tmp81; +double tmp95 = -(tmp17*tmp83); +double tmp96 = tmp93 + tmp94 + tmp57 + tmp95; +double tmp97 = -(tmp14*tmp81); +double tmp98 = -(tmp11*tmp25); +double tmp99 = tmp8*tmp83; +double tmp100 = tmp45 + tmp97 + tmp98 + tmp99; +double tmp101 = tmp14*tmp21; +double tmp102 = -(tmp17*tmp81); +double tmp103 = -(tmp11*tmp83); +double tmp104 = tmp101 + tmp102 + tmp37 + tmp103; +double tmp105 = -(tmp0*tmp10); +double tmp106 = -(tmp1*tmp13); +double tmp107 = tmp2*tmp16; +double tmp108 = tmp3*tmp19; +double tmp109 = tmp105 + tmp106 + tmp107 + tmp108; +double tmp110 = -(tmp3*tmp21); +double tmp111 = tmp54*tmp64; +double tmp112 = -(tmp44*tmp74); +double tmp113 = tmp111 + tmp112; +double tmp114 = tmp44*tmp64; +double tmp115 = tmp54*tmp74; +double tmp116 = tmp114 + tmp115; +double tmp117 = tmp2*tmp25; +double tmp118 = std::pow(tmp44,2); +double tmp119 = -tmp118; +double tmp120 = std::pow(tmp54,2); +double tmp121 = -tmp120; +double tmp122 = std::pow(tmp64,2); +double tmp123 = std::pow(tmp74,2); +double tmp124 = tmp119 + tmp121 + tmp122 + tmp123; +double tmp125 = 0.01*tmp6; +double tmp126 = 0. + tmp125; +double tmp127 = tmp11*tmp21; +double tmp128 = -(tmp17*tmp126); +double tmp129 = tmp127 + tmp66 + tmp128 + tmp91; +double tmp130 = -(tmp14*tmp126); +double tmp131 = tmp17*tmp83; +double tmp132 = tmp93 + tmp56 + tmp130 + tmp131; +double tmp133 = tmp14*tmp23; +double tmp134 = -(tmp11*tmp126); +double tmp135 = -(tmp8*tmp83); +double tmp136 = tmp45 + tmp133 + tmp134 + tmp135; +double tmp137 = -(tmp17*tmp23); +double tmp138 = tmp8*tmp126; +double tmp139 = tmp35 + tmp137 + tmp138 + tmp103; +double tmp140 = tmp2*tmp21; +double tmp141 = tmp2*tmp23; +double tmp142 = -(tmp0*tmp83); +double tmp143 = -(tmp1*tmp21); +double tmp144 = tmp49*tmp54; +double tmp145 = tmp59*tmp64; +double tmp146 = -2*tmp69*tmp44; +double tmp147 = 2*tmp39*tmp74; +double tmp148 = -(tmp1*tmp25); +double tmp149 = tmp0*tmp27; +double tmp150 = tmp110 + tmp141 + tmp148 + tmp149; +double tmp151 = -(tmp0*tmp23); +double tmp152 = tmp3*tmp25; +double tmp153 = tmp2*tmp27; +double tmp154 = tmp143 + tmp151 + tmp152 + tmp153; +double tmp155 = -(tmp0*tmp21); +double tmp156 = tmp1*tmp23; +double tmp157 = -(tmp3*tmp27); +double tmp158 = tmp155 + tmp156 + tmp117 + tmp157; +double tmp159 = -2*tmp104*tmp44; +double tmp160 = 2*tmp92*tmp74; +double tmp161 = tmp96*tmp54; +double tmp162 = tmp100*tmp64; +double tmp163 = tmp0*tmp21; +double tmp164 = -(tmp1*tmp81); +double tmp165 = -(tmp3*tmp83); +double tmp166 = tmp163 + tmp164 + tmp117 + tmp165; +double tmp167 = tmp44*tmp74; +double tmp168 = tmp111 + tmp167; +double tmp169 = tmp3*tmp81; +double tmp170 = -(tmp0*tmp25); +double tmp171 = -(tmp1*tmp83); +double tmp172 = tmp140 + tmp169 + tmp170 + tmp171; +double tmp173 = tmp44*tmp54; +double tmp174 = -(tmp64*tmp74); +double tmp175 = tmp173 + tmp174; +double tmp176 = tmp2*tmp81; +double tmp177 = tmp1*tmp25; +double tmp178 = tmp110 + tmp176 + tmp177 + tmp142; +double tmp179 = -tmp122; +double tmp180 = tmp119 + tmp120 + tmp179 + tmp123; +double tmp181 = -2*tmp136*tmp44; +double tmp182 = 2*tmp132*tmp74; +double tmp183 = tmp129*tmp54; +double tmp184 = tmp139*tmp64; +double tmp185 = tmp0*tmp23; +double tmp186 = -(tmp3*tmp126); +double tmp187 = tmp2*tmp83; +double tmp188 = tmp143 + tmp185 + tmp186 + tmp187; +double tmp189 = tmp3*tmp21; +double tmp190 = -(tmp1*tmp126); +double tmp191 = tmp189 + tmp141 + tmp190 + tmp142; +double tmp192 = -(tmp3*tmp23); +double tmp193 = -(tmp0*tmp126); +double tmp194 = tmp1*tmp83; +double tmp195 = tmp140 + tmp192 + tmp193 + tmp194; +double tmp196 = tmp49*tmp44; +double tmp197 = tmp69*tmp64; +double tmp198 = -2*tmp59*tmp54; +double tmp199 = -2*tmp49*tmp64; +double tmp200 = tmp59*tmp44; +double tmp201 = tmp69*tmp54; +double tmp202 = tmp100*tmp44; +double tmp203 = tmp104*tmp54; +double tmp204 = -2*tmp100*tmp54; +double tmp205 = -2*tmp96*tmp64; +double tmp206 = tmp96*tmp44; +double tmp207 = tmp104*tmp64; +double tmp208 = -(tmp54*tmp74); +double tmp209 = tmp114 + tmp208; +double tmp210 = tmp64*tmp74; +double tmp211 = tmp173 + tmp210; +double tmp212 = tmp118 + tmp121 + tmp179 + tmp123; +double tmp213 = -2*tmp139*tmp54; +double tmp214 = -2*tmp129*tmp64; +double tmp215 = tmp139*tmp44; +double tmp216 = tmp136*tmp54; +double tmp217 = tmp129*tmp44; +double tmp218 = tmp136*tmp64; +J(1+_OFF,4+_OFF) = 4*tmp28*tmp34*tmp109*(tmp196 + tmp39*tmp54 + tmp197 + tmp59*tmp74) - 4*tmp28*tmp34*tmp88*(-(tmp39*tmp44) + tmp144 + tmp145 - tmp69*tmp74) - 2*tmp28*tmp34*tmp79*(tmp146 + tmp198 + 2*tmp49*tmp64 + tmp147) - 4*tmp28*tmp34*tmp158*tmp113 + 4*tmp28*tmp34*tmp154*tmp116 - 2*tmp28*tmp34*tmp150*tmp124; +J(1+_OFF,5+_OFF) = -2*tmp28*tmp34*tmp79*(tmp159 + tmp204 + 2*tmp96*tmp64 + tmp160) - 4*tmp28*tmp34*tmp88*(-(tmp92*tmp44) + tmp161 + tmp162 - tmp104*tmp74) + 4*tmp28*tmp34*tmp109*(tmp206 + tmp92*tmp54 + tmp207 + tmp100*tmp74) - 4*tmp28*tmp34*tmp178*tmp113 + 4*tmp28*tmp34*tmp172*tmp116 - 2*tmp28*tmp34*tmp166*tmp124; +J(1+_OFF,6+_OFF) = -2*tmp28*tmp34*tmp79*(tmp181 + tmp213 + 2*tmp129*tmp64 + tmp182) + 4*tmp28*tmp34*tmp109*(tmp217 + tmp132*tmp54 + tmp218 + tmp139*tmp74) - 4*tmp28*tmp34*tmp88*(-(tmp132*tmp44) + tmp183 + tmp184 - tmp136*tmp74) - 4*tmp28*tmp34*tmp195*tmp113 + 4*tmp28*tmp34*tmp191*tmp116 - 2*tmp28*tmp34*tmp188*tmp124; +J(2+_OFF,4+_OFF) = -4*tmp28*tmp34*tmp79*(tmp39*tmp44 + tmp144 + tmp145 + tmp69*tmp74) - 2*tmp28*tmp34*tmp88*(tmp146 + 2*tmp59*tmp54 + tmp199 + tmp147) + 4*tmp28*tmp34*tmp109*(tmp200 + tmp201 - tmp39*tmp64 - tmp49*tmp74) - 4*tmp28*tmp34*tmp150*tmp168 + 4*tmp28*tmp34*tmp154*tmp175 - 2*tmp28*tmp34*tmp158*tmp180; +J(2+_OFF,5+_OFF) = 4*tmp28*tmp34*tmp109*(tmp202 + tmp203 - tmp92*tmp64 - tmp96*tmp74) - 2*tmp28*tmp34*tmp88*(tmp159 + 2*tmp100*tmp54 + tmp205 + tmp160) - 4*tmp28*tmp34*tmp79*(tmp92*tmp44 + tmp161 + tmp162 + tmp104*tmp74) - 4*tmp28*tmp34*tmp166*tmp168 + 4*tmp28*tmp34*tmp172*tmp175 - 2*tmp28*tmp34*tmp178*tmp180; +J(2+_OFF,6+_OFF) = -2*tmp28*tmp34*tmp88*(tmp181 + 2*tmp139*tmp54 + tmp214 + tmp182) + 4*tmp28*tmp34*tmp109*(tmp215 + tmp216 - tmp132*tmp64 - tmp129*tmp74) - 4*tmp28*tmp34*tmp79*(tmp132*tmp44 + tmp183 + tmp184 + tmp136*tmp74) - 4*tmp28*tmp34*tmp188*tmp168 + 4*tmp28*tmp34*tmp191*tmp175 - 2*tmp28*tmp34*tmp195*tmp180; +J(3+_OFF,4+_OFF) = -4*tmp28*tmp34*tmp79*(tmp196 - tmp39*tmp54 + tmp197 - tmp59*tmp74) + 2*tmp28*tmp34*tmp109*(2*tmp69*tmp44 + tmp198 + tmp199 + tmp147) - 4*tmp28*tmp34*tmp88*(tmp200 + tmp201 + tmp39*tmp64 + tmp49*tmp74) - 4*tmp28*tmp34*tmp150*tmp209 - 4*tmp28*tmp34*tmp158*tmp211 + 2*tmp28*tmp34*tmp154*tmp212; +J(3+_OFF,5+_OFF) = -4*tmp28*tmp34*tmp88*(tmp202 + tmp203 + tmp92*tmp64 + tmp96*tmp74) + 2*tmp28*tmp34*tmp109*(2*tmp104*tmp44 + tmp204 + tmp205 + tmp160) - 4*tmp28*tmp34*tmp79*(tmp206 - tmp92*tmp54 + tmp207 - tmp100*tmp74) - 4*tmp28*tmp34*tmp166*tmp209 - 4*tmp28*tmp34*tmp178*tmp211 + 2*tmp28*tmp34*tmp172*tmp212; +J(3+_OFF,6+_OFF) = 2*tmp28*tmp34*tmp109*(2*tmp136*tmp44 + tmp213 + tmp214 + tmp182) - 4*tmp28*tmp34*tmp88*(tmp215 + tmp216 + tmp132*tmp64 + tmp129*tmp74) - 4*tmp28*tmp34*tmp79*(tmp217 - tmp132*tmp54 + tmp218 - tmp139*tmp74) - 4*tmp28*tmp34*tmp188*tmp209 - 4*tmp28*tmp34*tmp195*tmp211 + 2*tmp28*tmp34*tmp191*tmp212; diff --git a/roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_JAugWprevX2.cppready b/roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_JAugWprevX2.cppready new file mode 100644 index 0000000..e69de29 diff --git a/roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_JAugWprevqOS.cppready b/roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_JAugWprevqOS.cppready new file mode 100644 index 0000000..064edd2 --- /dev/null +++ b/roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_JAugWprevqOS.cppready @@ -0,0 +1,164 @@ +double tmp0 = x0(7+_OFF); +double tmp1 = x0(6+_OFF); +double tmp2 = x0(5+_OFF); +double tmp3 = x0(4+_OFF); +double tmp4 = x1(4+_OFF); +double tmp5 = x1(5+_OFF); +double tmp6 = x1(6+_OFF); +double tmp7 = x1(7+_OFF); +double tmp8 = qos(2+_OFF); +double tmp9 = qos(1+_OFF); +double tmp10 = qos(4+_OFF); +double tmp11 = qos(3+_OFF); +double tmp12 = -0.01*tmp11; +double tmp13 = 0. + tmp12; +double tmp14 = 0.01*tmp10; +double tmp15 = 0. + tmp14; +double tmp16 = 0.01*tmp9; +double tmp17 = 0. + tmp16; +double tmp18 = -0.01*tmp8; +double tmp19 = 0. + tmp18; +double tmp20 = 1.*tmp8; +double tmp21 = 0. + tmp20; +double tmp22 = 1.*tmp9; +double tmp23 = 0. + tmp22; +double tmp24 = 1.*tmp10; +double tmp25 = 0. + tmp24; +double tmp26 = 1.*tmp11; +double tmp27 = 0. + tmp26; +double tmp28 = 1/Dt01; +double tmp29 = std::pow(tmp3,2); +double tmp30 = std::pow(tmp2,2); +double tmp31 = std::pow(tmp1,2); +double tmp32 = std::pow(tmp0,2); +double tmp33 = tmp29 + tmp30 + tmp31 + tmp32; +double tmp34 = 1/tmp33; +double tmp35 = -(tmp25*tmp4); +double tmp36 = -(tmp27*tmp5); +double tmp37 = tmp21*tmp6; +double tmp38 = -(tmp23*tmp7); +double tmp39 = tmp35 + tmp36 + tmp37 + tmp38; +double tmp40 = tmp19*tmp4; +double tmp41 = -(tmp17*tmp5); +double tmp42 = -(tmp15*tmp6); +double tmp43 = -(tmp13*tmp7); +double tmp44 = tmp40 + tmp41 + tmp42 + tmp43; +double tmp45 = -(tmp15*tmp4); +double tmp46 = tmp13*tmp5; +double tmp47 = -(tmp19*tmp6); +double tmp48 = -(tmp17*tmp7); +double tmp49 = tmp45 + tmp46 + tmp47 + tmp48; +double tmp50 = -(tmp21*tmp4); +double tmp51 = -(tmp23*tmp5); +double tmp52 = -(tmp25*tmp6); +double tmp53 = tmp27*tmp7; +double tmp54 = tmp50 + tmp51 + tmp52 + tmp53; +double tmp55 = -(tmp27*tmp4); +double tmp56 = tmp25*tmp5; +double tmp57 = -(tmp23*tmp6); +double tmp58 = -(tmp21*tmp7); +double tmp59 = tmp55 + tmp56 + tmp57 + tmp58; +double tmp60 = -(tmp17*tmp4); +double tmp61 = -(tmp19*tmp5); +double tmp62 = -(tmp13*tmp6); +double tmp63 = tmp15*tmp7; +double tmp64 = tmp60 + tmp61 + tmp62 + tmp63; +double tmp65 = -(tmp13*tmp4); +double tmp66 = -(tmp15*tmp5); +double tmp67 = tmp17*tmp6; +double tmp68 = -(tmp19*tmp7); +double tmp69 = tmp65 + tmp66 + tmp67 + tmp68; +double tmp70 = tmp23*tmp4; +double tmp71 = -(tmp21*tmp5); +double tmp72 = -(tmp27*tmp6); +double tmp73 = -(tmp25*tmp7); +double tmp74 = tmp70 + tmp71 + tmp72 + tmp73; +double tmp75 = tmp2*tmp4; +double tmp76 = -(tmp3*tmp5); +double tmp77 = tmp0*tmp6; +double tmp78 = -(tmp1*tmp7); +double tmp79 = tmp75 + tmp76 + tmp77 + tmp78; +double tmp80 = -0.01*tmp10; +double tmp81 = 0. + tmp80; +double tmp82 = 0.01*tmp8; +double tmp83 = 0. + tmp82; +double tmp84 = tmp1*tmp4; +double tmp85 = -(tmp0*tmp5); +double tmp86 = -(tmp3*tmp6); +double tmp87 = tmp2*tmp7; +double tmp88 = tmp84 + tmp85 + tmp86 + tmp87; +double tmp89 = tmp13*tmp4; +double tmp90 = -(tmp81*tmp5); +double tmp91 = -(tmp17*tmp6); +double tmp92 = -(tmp83*tmp7); +double tmp93 = tmp89 + tmp90 + tmp91 + tmp92; +double tmp94 = -(tmp81*tmp4); +double tmp95 = -(tmp13*tmp5); +double tmp96 = -(tmp83*tmp6); +double tmp97 = tmp17*tmp7; +double tmp98 = tmp94 + tmp95 + tmp96 + tmp97; +double tmp99 = tmp83*tmp5; +double tmp100 = -(tmp81*tmp7); +double tmp101 = tmp60 + tmp99 + tmp62 + tmp100; +double tmp102 = -(tmp83*tmp4); +double tmp103 = tmp81*tmp6; +double tmp104 = tmp102 + tmp41 + tmp103 + tmp43; +double tmp105 = -(tmp0*tmp4); +double tmp106 = -(tmp1*tmp5); +double tmp107 = tmp2*tmp6; +double tmp108 = tmp3*tmp7; +double tmp109 = tmp105 + tmp106 + tmp107 + tmp108; +double tmp110 = 0.01*tmp11; +double tmp111 = 0. + tmp110; +double tmp112 = -(tmp111*tmp4); +double tmp113 = tmp19*tmp7; +double tmp114 = tmp112 + tmp90 + tmp91 + tmp113; +double tmp115 = tmp81*tmp4; +double tmp116 = -(tmp111*tmp5); +double tmp117 = tmp115 + tmp116 + tmp47 + tmp48; +double tmp118 = tmp111*tmp6; +double tmp119 = tmp60 + tmp61 + tmp118 + tmp100; +double tmp120 = -(tmp19*tmp4); +double tmp121 = tmp17*tmp5; +double tmp122 = -(tmp81*tmp6); +double tmp123 = -(tmp111*tmp7); +double tmp124 = tmp120 + tmp121 + tmp122 + tmp123; +double tmp125 = tmp49*tmp54; +double tmp126 = tmp59*tmp64; +double tmp127 = -2*tmp39*tmp69; +double tmp128 = 2*tmp44*tmp74; +double tmp129 = -2*tmp39*tmp104; +double tmp130 = 2*tmp93*tmp74; +double tmp131 = tmp98*tmp59; +double tmp132 = tmp54*tmp101; +double tmp133 = -2*tmp39*tmp119; +double tmp134 = 2*tmp117*tmp74; +double tmp135 = tmp114*tmp59; +double tmp136 = tmp124*tmp54; +double tmp137 = tmp69*tmp54; +double tmp138 = tmp39*tmp64; +double tmp139 = -2*tmp49*tmp59; +double tmp140 = -2*tmp54*tmp64; +double tmp141 = tmp49*tmp39; +double tmp142 = tmp69*tmp59; +double tmp143 = tmp59*tmp104; +double tmp144 = tmp39*tmp101; +double tmp145 = -2*tmp98*tmp54; +double tmp146 = -2*tmp59*tmp101; +double tmp147 = tmp98*tmp39; +double tmp148 = tmp104*tmp54; +double tmp149 = -2*tmp59*tmp124; +double tmp150 = -2*tmp114*tmp54; +double tmp151 = tmp39*tmp124; +double tmp152 = tmp59*tmp119; +double tmp153 = tmp39*tmp114; +double tmp154 = tmp54*tmp119; +J(1+_OFF,1+_OFF) = 4*tmp28*tmp34*tmp109*(tmp59*tmp44 + tmp137 + tmp138 + tmp49*tmp74) - 4*tmp28*tmp34*tmp88*(-(tmp39*tmp44) + tmp125 + tmp126 - tmp69*tmp74) - 2*tmp28*tmp34*tmp79*(tmp127 + tmp139 + 2*tmp54*tmp64 + tmp128); +J(1+_OFF,2+_OFF) = -2*tmp28*tmp34*tmp79*(tmp129 + 2*tmp98*tmp54 + tmp146 + tmp130) - 4*tmp28*tmp34*tmp88*(-(tmp39*tmp93) + tmp131 + tmp132 - tmp104*tmp74) + 4*tmp28*tmp34*tmp109*(tmp147 + tmp93*tmp59 + tmp148 + tmp101*tmp74); +J(1+_OFF,3+_OFF) = -2*tmp28*tmp34*tmp79*(tmp149 + 2*tmp114*tmp54 + tmp133 + tmp134) + 4*tmp28*tmp34*tmp109*(tmp153 + tmp117*tmp59 + tmp154 + tmp124*tmp74) - 4*tmp28*tmp34*tmp88*(-(tmp117*tmp39) + tmp135 + tmp136 - tmp119*tmp74); +J(2+_OFF,1+_OFF) = -4*tmp28*tmp34*tmp79*(tmp39*tmp44 + tmp125 + tmp126 + tmp69*tmp74) - 2*tmp28*tmp34*tmp88*(tmp127 + 2*tmp49*tmp59 + tmp140 + tmp128) + 4*tmp28*tmp34*tmp109*(tmp141 + tmp142 - tmp44*tmp54 - tmp64*tmp74); +J(2+_OFF,2+_OFF) = 4*tmp28*tmp34*tmp109*(tmp143 - tmp93*tmp54 + tmp144 - tmp98*tmp74) - 2*tmp28*tmp34*tmp88*(tmp129 + tmp145 + 2*tmp59*tmp101 + tmp130) - 4*tmp28*tmp34*tmp79*(tmp39*tmp93 + tmp131 + tmp132 + tmp104*tmp74); +J(2+_OFF,3+_OFF) = -2*tmp28*tmp34*tmp88*(2*tmp59*tmp124 + tmp150 + tmp133 + tmp134) + 4*tmp28*tmp34*tmp109*(tmp151 - tmp117*tmp54 + tmp152 - tmp114*tmp74) - 4*tmp28*tmp34*tmp79*(tmp117*tmp39 + tmp135 + tmp136 + tmp119*tmp74); +J(3+_OFF,1+_OFF) = -4*tmp28*tmp34*tmp79*(-(tmp59*tmp44) + tmp137 + tmp138 - tmp49*tmp74) + 2*tmp28*tmp34*tmp109*(2*tmp39*tmp69 + tmp139 + tmp140 + tmp128) - 4*tmp28*tmp34*tmp88*(tmp141 + tmp142 + tmp44*tmp54 + tmp64*tmp74); +J(3+_OFF,2+_OFF) = -4*tmp28*tmp34*tmp88*(tmp143 + tmp93*tmp54 + tmp144 + tmp98*tmp74) + 2*tmp28*tmp34*tmp109*(2*tmp39*tmp104 + tmp145 + tmp146 + tmp130) - 4*tmp28*tmp34*tmp79*(tmp147 - tmp93*tmp59 + tmp148 - tmp101*tmp74); +J(3+_OFF,3+_OFF) = 2*tmp28*tmp34*tmp109*(tmp149 + tmp150 + 2*tmp39*tmp119 + tmp134) - 4*tmp28*tmp34*tmp88*(tmp151 + tmp117*tmp54 + tmp152 + tmp114*tmp74) - 4*tmp28*tmp34*tmp79*(tmp153 - tmp117*tmp59 + tmp154 - tmp124*tmp74); diff --git a/roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_Vprev.cppready b/roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_Vprev.cppready new file mode 100644 index 0000000..84f8427 --- /dev/null +++ b/roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_Vprev.cppready @@ -0,0 +1,114 @@ +double tmp0 = x1(4+_OFF); +double tmp1 = x1(6+_OFF); +double tmp2 = x1(5+_OFF); +double tmp3 = x1(7+_OFF); +double tmp4 = 1/Dt12; +double tmp5 = x2(4+_OFF); +double tmp6 = so(2+_OFF); +double tmp7 = so(3+_OFF); +double tmp8 = x2(5+_OFF); +double tmp9 = x2(6+_OFF); +double tmp10 = x2(7+_OFF); +double tmp11 = qos(1+_OFF); +double tmp12 = qos(3+_OFF); +double tmp13 = qos(2+_OFF); +double tmp14 = qos(4+_OFF); +double tmp15 = 1/Dt01; +double tmp16 = x0(3+_OFF); +double tmp17 = x1(3+_OFF); +double tmp18 = -tmp17; +double tmp19 = tmp16 + tmp18; +double tmp20 = std::pow(tmp1,2); +double tmp21 = 2*tmp20; +double tmp22 = x0(1+_OFF); +double tmp23 = x1(1+_OFF); +double tmp24 = tmp0*tmp1; +double tmp25 = x0(2+_OFF); +double tmp26 = x1(2+_OFF); +double tmp27 = so(1+_OFF); +double tmp28 = -tmp22; +double tmp29 = tmp28 + tmp23; +double tmp30 = tmp2*tmp1; +double tmp31 = tmp0*tmp2; +double tmp32 = -tmp26; +double tmp33 = tmp25 + tmp32; +double tmp34 = std::pow(tmp2,2); +double tmp35 = 2*tmp34; +double tmp36 = std::pow(tmp3,2); +double tmp37 = 2*tmp36; +double tmp38 = tmp13*tmp12; +double tmp39 = -tmp25; +double tmp40 = tmp39 + tmp26; +double tmp41 = tmp0*tmp3; +double tmp42 = tmp30 + tmp41; +double tmp43 = 2*tmp40*tmp42; +double tmp44 = -(tmp2*tmp3); +double tmp45 = tmp24 + tmp44; +double tmp46 = 2*tmp19*tmp45; +double tmp47 = -tmp23; +double tmp48 = tmp22 + tmp47; +double tmp49 = -1 + tmp21 + tmp37; +double tmp50 = tmp48*tmp49; +double tmp51 = tmp43 + tmp46 + tmp50; +double tmp52 = tmp15*tmp51; +double tmp53 = -2*tmp4*tmp7*tmp1*tmp5; +double tmp54 = 2*tmp4*tmp6*tmp3*tmp5; +double tmp55 = -2*tmp4*tmp6*tmp1*tmp8; +double tmp56 = -2*tmp4*tmp7*tmp3*tmp8; +double tmp57 = 2*tmp4*tmp7*tmp0*tmp9; +double tmp58 = 2*tmp4*tmp6*tmp2*tmp9; +double tmp59 = -2*tmp4*tmp6*tmp0*tmp10; +double tmp60 = 2*tmp4*tmp7*tmp2*tmp10; +double tmp61 = tmp52 + tmp53 + tmp54 + tmp55 + tmp56 + tmp57 + tmp58 + tmp59 + tmp60; +double tmp62 = -1 + tmp35 + tmp21; +double tmp63 = tmp19*tmp62; +double tmp64 = tmp2*tmp3; +double tmp65 = tmp24 + tmp64; +double tmp66 = 2*tmp29*tmp65; +double tmp67 = -(tmp1*tmp3); +double tmp68 = tmp31 + tmp67; +double tmp69 = 2*tmp33*tmp68; +double tmp70 = tmp63 + tmp66 + tmp69; +double tmp71 = tmp15*tmp70; +double tmp72 = -2*tmp4*tmp6*tmp2*tmp5; +double tmp73 = 2*tmp4*tmp27*tmp1*tmp5; +double tmp74 = 2*tmp4*tmp6*tmp0*tmp8; +double tmp75 = 2*tmp4*tmp27*tmp3*tmp8; +double tmp76 = -2*tmp4*tmp27*tmp0*tmp9; +double tmp77 = 2*tmp4*tmp6*tmp3*tmp9; +double tmp78 = -2*tmp4*tmp27*tmp2*tmp10; +double tmp79 = -2*tmp4*tmp6*tmp1*tmp10; +double tmp80 = tmp71 + tmp72 + tmp73 + tmp74 + tmp75 + tmp76 + tmp77 + tmp78 + tmp79; +double tmp81 = std::pow(tmp11,2); +double tmp82 = std::pow(tmp13,2); +double tmp83 = std::pow(tmp12,2); +double tmp84 = std::pow(tmp14,2); +double tmp85 = -tmp84; +double tmp86 = -(tmp0*tmp3); +double tmp87 = tmp30 + tmp86; +double tmp88 = 2*tmp29*tmp87; +double tmp89 = -tmp16; +double tmp90 = tmp89 + tmp17; +double tmp91 = tmp1*tmp3; +double tmp92 = tmp31 + tmp91; +double tmp93 = 2*tmp90*tmp92; +double tmp94 = -1 + tmp35 + tmp37; +double tmp95 = tmp33*tmp94; +double tmp96 = tmp88 + tmp93 + tmp95; +double tmp97 = tmp15*tmp96; +double tmp98 = 2*tmp4*tmp7*tmp2*tmp5; +double tmp99 = -2*tmp4*tmp27*tmp3*tmp5; +double tmp100 = -2*tmp4*tmp7*tmp0*tmp8; +double tmp101 = 2*tmp4*tmp27*tmp1*tmp8; +double tmp102 = -2*tmp4*tmp27*tmp2*tmp9; +double tmp103 = -2*tmp4*tmp7*tmp3*tmp9; +double tmp104 = 2*tmp4*tmp27*tmp0*tmp10; +double tmp105 = 2*tmp4*tmp7*tmp1*tmp10; +double tmp106 = tmp97 + tmp98 + tmp99 + tmp100 + tmp101 + tmp102 + tmp103 + tmp104 + tmp105; +double tmp107 = tmp13*tmp14; +double tmp108 = -tmp82; +double tmp109 = -tmp83; +double tmp110 = tmp12*tmp14; +part(1+_OFF) = (tmp81 + tmp82 + tmp109 + tmp85)*tmp61 + 2*(-(tmp11*tmp12) + tmp107)*tmp80 + 2*(tmp38 + tmp11*tmp14)*tmp106; +part(2+_OFF) = 2*(tmp38 - tmp11*tmp14)*tmp61 + 2*(tmp11*tmp13 + tmp110)*tmp80 + (tmp81 + tmp108 + tmp83 + tmp85)*tmp106; +part(3+_OFF) = 2*(tmp11*tmp12 + tmp107)*tmp61 + (tmp81 + tmp108 + tmp109 + tmp84)*tmp80 + 2*(-(tmp11*tmp13) + tmp110)*tmp106; diff --git a/roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_Wprev.cppready b/roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_Wprev.cppready new file mode 100644 index 0000000..d1fac8c --- /dev/null +++ b/roamfree/ROAMestimation/generated/BackwardAugmentedStateEstimator_v7_Wprev.cppready @@ -0,0 +1,67 @@ +double tmp0 = x0(6+_OFF); +double tmp1 = x0(7+_OFF); +double tmp2 = x0(4+_OFF); +double tmp3 = x0(5+_OFF); +double tmp4 = x1(4+_OFF); +double tmp5 = x1(5+_OFF); +double tmp6 = x1(6+_OFF); +double tmp7 = x1(7+_OFF); +double tmp8 = qos(2+_OFF); +double tmp9 = qos(1+_OFF); +double tmp10 = qos(4+_OFF); +double tmp11 = qos(3+_OFF); +double tmp12 = 1/Dt01; +double tmp13 = std::pow(tmp2,2); +double tmp14 = std::pow(tmp3,2); +double tmp15 = std::pow(tmp0,2); +double tmp16 = std::pow(tmp1,2); +double tmp17 = tmp13 + tmp14 + tmp15 + tmp16; +double tmp18 = 1/tmp17; +double tmp19 = -(tmp10*tmp4); +double tmp20 = -(tmp11*tmp5); +double tmp21 = tmp8*tmp6; +double tmp22 = -(tmp9*tmp7); +double tmp23 = tmp19 + tmp20 + tmp21 + tmp22; +double tmp24 = -(tmp8*tmp4); +double tmp25 = -(tmp9*tmp5); +double tmp26 = -(tmp10*tmp6); +double tmp27 = tmp11*tmp7; +double tmp28 = tmp24 + tmp25 + tmp26 + tmp27; +double tmp29 = -(tmp11*tmp4); +double tmp30 = tmp10*tmp5; +double tmp31 = -(tmp9*tmp6); +double tmp32 = -(tmp8*tmp7); +double tmp33 = tmp29 + tmp30 + tmp31 + tmp32; +double tmp34 = tmp9*tmp4; +double tmp35 = -(tmp8*tmp5); +double tmp36 = -(tmp11*tmp6); +double tmp37 = -(tmp10*tmp7); +double tmp38 = tmp34 + tmp35 + tmp36 + tmp37; +double tmp39 = tmp3*tmp4; +double tmp40 = -(tmp2*tmp5); +double tmp41 = tmp1*tmp6; +double tmp42 = -(tmp0*tmp7); +double tmp43 = tmp39 + tmp40 + tmp41 + tmp42; +double tmp44 = tmp33*tmp28; +double tmp45 = -(tmp1*tmp4); +double tmp46 = -(tmp0*tmp5); +double tmp47 = tmp3*tmp6; +double tmp48 = tmp2*tmp7; +double tmp49 = tmp45 + tmp46 + tmp47 + tmp48; +double tmp50 = tmp0*tmp4; +double tmp51 = -(tmp1*tmp5); +double tmp52 = -(tmp2*tmp6); +double tmp53 = tmp3*tmp7; +double tmp54 = tmp50 + tmp51 + tmp52 + tmp53; +double tmp55 = std::pow(tmp23,2); +double tmp56 = -tmp55; +double tmp57 = std::pow(tmp33,2); +double tmp58 = std::pow(tmp28,2); +double tmp59 = std::pow(tmp38,2); +double tmp60 = tmp23*tmp28; +double tmp61 = tmp23*tmp33; +double tmp62 = -tmp57; +double tmp63 = -tmp58; +part(1+_OFF) = -4*tmp12*tmp18*tmp54*(tmp44 - tmp23*tmp38) + 4*tmp12*tmp18*tmp49*(tmp60 + tmp33*tmp38) - 2*tmp12*tmp18*tmp43*(tmp56 + tmp62 + tmp58 + tmp59); +part(2+_OFF) = -4*tmp12*tmp18*tmp43*(tmp44 + tmp23*tmp38) + 4*tmp12*tmp18*tmp49*(tmp61 - tmp28*tmp38) - 2*tmp12*tmp18*tmp54*(tmp56 + tmp57 + tmp63 + tmp59); +part(3+_OFF) = -4*tmp12*tmp18*tmp43*(tmp60 - tmp33*tmp38) - 4*tmp12*tmp18*tmp54*(tmp61 + tmp28*tmp38) + 2*tmp12*tmp18*tmp49*(tmp55 + tmp62 + tmp63 + tmp59); diff --git a/roamfree/ROAMestimation/include/ROAMestimation/AugmentedStateCalculator.h b/roamfree/ROAMestimation/include/ROAMestimation/AugmentedStateCalculator.h index a58cafa..a1bc820 100644 --- a/roamfree/ROAMestimation/include/ROAMestimation/AugmentedStateCalculator.h +++ b/roamfree/ROAMestimation/include/ROAMestimation/AugmentedStateCalculator.h @@ -110,6 +110,22 @@ class AugmentedStateCalculator: public GenericCalculator # include "generated/BackwardAugmentedStateEstimator_v7_IMUintdP.cppready" } + if (_usedComponents[PREVIOUS_ORIENTATION]) + { + _x.segment(29, 4) = x1.segment(3, 4); + } + if (_usedComponents[PREVIOUS_LINEARVELOCITY]) + { + Eigen::VectorBlock &&part = _x.segment(33, 3); + +# include "generated/BackwardAugmentedStateEstimator_v7_Vprev.cppready" + } + if (_usedComponents[PREVIOUS_ANGULARVELOCITY]) + { + Eigen::VectorBlock &&part = _x.segment(36, 3); + +# include "generated/BackwardAugmentedStateEstimator_v7_Wprev.cppready" + } return true; diff --git a/roamfree/ROAMestimation/include/ROAMestimation/AugmentedStateJacobianCalculator.h b/roamfree/ROAMestimation/include/ROAMestimation/AugmentedStateJacobianCalculator.h index db9f998..1af7e1f 100644 --- a/roamfree/ROAMestimation/include/ROAMestimation/AugmentedStateJacobianCalculator.h +++ b/roamfree/ROAMestimation/include/ROAMestimation/AugmentedStateJacobianCalculator.h @@ -241,6 +241,21 @@ class AugmentedStateJacobianCalculator: public GenericCalculator { # include "generated/BackwardAugmentedStateEstimator_v7_JAugIMUintdPX2.cppready" return true; } + case PREVIOUS_ORIENTATION: { + assert(ORDER > 1); + // do nothing + return true; + } + case PREVIOUS_LINEARVELOCITY: { + assert(ORDER > 1); + // do nothing + return true; + } + case PREVIOUS_ANGULARVELOCITY: { + assert(ORDER > 1); + // do nothing + return true; + } } } @@ -264,6 +279,21 @@ class AugmentedStateJacobianCalculator: public GenericCalculator { # include "generated/BackwardAugmentedStateEstimator_v7_JAugIMUintdPX1.cppready" return true; } + case PREVIOUS_ORIENTATION: { + assert(ORDER > 1); +# include "generated/BackwardAugmentedStateEstimator_v7_JAugQprevX1.cppready" + return true; + } + case PREVIOUS_LINEARVELOCITY: { + assert(ORDER > 1); +# include "generated/BackwardAugmentedStateEstimator_v7_JAugVprevX1.cppready" + return true; + } + case PREVIOUS_ANGULARVELOCITY: { + assert(ORDER > 1); +# include "generated/BackwardAugmentedStateEstimator_v7_JAugWprevX1.cppready" + return true; + } } } @@ -311,6 +341,21 @@ class AugmentedStateJacobianCalculator: public GenericCalculator { # include "generated/BackwardAugmentedStateEstimator_v7_JAugIMUintdPX0.cppready" return true; } + case PREVIOUS_ORIENTATION: { + assert(ORDER > 1); +# include "generated/BackwardAugmentedStateEstimator_v7_JAugQprevX0.cppready" + return true; + } + case PREVIOUS_LINEARVELOCITY: { + assert(ORDER > 1); +# include "generated/BackwardAugmentedStateEstimator_v7_JAugVprevX0.cppready" + return true; + } + case PREVIOUS_ANGULARVELOCITY: { + assert(ORDER > 1); +# include "generated/BackwardAugmentedStateEstimator_v7_JAugWprevX0.cppready" + return true; + } } } @@ -363,6 +408,51 @@ class AugmentedStateJacobianCalculator: public GenericCalculator { break; } + case PREVIOUS_ORIENTATION: { + assert(ORDER > 1); + switch (x) { + case (ORDER + 1): { +# include "generated/BackwardAugmentedStateEstimator_v7_JAugQprevSO.cppready" + return true; + } + case (ORDER + 2): { +# include "generated/BackwardAugmentedStateEstimator_v7_JAugQprevqOS.cppready" + return true; + } + } + + break; + } + case PREVIOUS_LINEARVELOCITY: { + assert(ORDER > 1); + switch (x) { + case (ORDER + 1): { +# include "generated/BackwardAugmentedStateEstimator_v7_JAugVprevSO.cppready" + return true; + } + case (ORDER + 2): { +# include "generated/BackwardAugmentedStateEstimator_v7_JAugVprevqOS.cppready" + return true; + } + } + + break; + } + case PREVIOUS_ANGULARVELOCITY: { + assert(ORDER > 1); + switch (x) { + case (ORDER + 1): { +# include "generated/BackwardAugmentedStateEstimator_v7_JAugWprevSO.cppready" + return true; + } + case (ORDER + 2): { +# include "generated/BackwardAugmentedStateEstimator_v7_JAugWprevqOS.cppready" + return true; + } + } + + break; + } } return false; diff --git a/roamfree/ROAMestimation/include/ROAMestimation/Enums.h b/roamfree/ROAMestimation/include/ROAMestimation/Enums.h index b7ff710..eff8f89 100644 --- a/roamfree/ROAMestimation/include/ROAMestimation/Enums.h +++ b/roamfree/ROAMestimation/include/ROAMestimation/Enums.h @@ -94,7 +94,10 @@ enum AugStateComponents { DELTA_POSITION, DELTA_ORIENTATION, IMUINT_DELTAPOSE, - AUGSTATE_N_COMPONENTS + PREVIOUS_ORIENTATION, + PREVIOUS_LINEARVELOCITY, + PREVIOUS_ANGULARVELOCITY, + AUGSTATE_N_COMPONENTS, }; enum SolverMethod { diff --git a/roamfree/ROAMestimation/include/ROAMestimation/GenericEdge.h b/roamfree/ROAMestimation/include/ROAMestimation/GenericEdge.h index c698211..e8a7c21 100644 --- a/roamfree/ROAMestimation/include/ROAMestimation/GenericEdge.h +++ b/roamfree/ROAMestimation/include/ROAMestimation/GenericEdge.h @@ -119,7 +119,7 @@ using g2o::BaseMultiEdge::resize; template GenericEdge::GenericEdge(int nParams) : _paramsPtrsSize(nParams), _name("undefined"), _tstamp( - -std::numeric_limits::infinity()), _Dt01(0), _Dt12(0), _x(33) { + -std::numeric_limits::infinity()), _Dt01(0), _Dt12(0), _x(39) { _paramsPtrs = new double *[_paramsPtrsSize]; // 1 Euclidean 3D, 1 Quaternion the possible function specific params } diff --git a/roamfree/ROAMestimation/include/ROAMestimation/QuaternionGenericEdge.h b/roamfree/ROAMestimation/include/ROAMestimation/QuaternionGenericEdge.h index e533fbd..f512280 100644 --- a/roamfree/ROAMestimation/include/ROAMestimation/QuaternionGenericEdge.h +++ b/roamfree/ROAMestimation/include/ROAMestimation/QuaternionGenericEdge.h @@ -110,7 +110,7 @@ class QuaternionGenericEdge: public ROAMestimation::GenericEdge for (int k = 0; k < AUGSTATE_N_COMPONENTS; k++) { if (_F._usedComponents[k] == true) { - _JErrx[k] = new Eigen::MatrixXd((int) MT::_ERROR_SIZE, (k == 1 || k == 7) ? 4 : 3); + _JErrx[k] = new Eigen::MatrixXd((int) MT::_ERROR_SIZE, (k == 1 || k == 7 || k == 9) ? 4 : 3); } isNotTheIdentity[k] = true; @@ -232,7 +232,7 @@ class QuaternionGenericEdge: public ROAMestimation::GenericEdge // check that I have no duplicates in the vertex set for (int i = 0; i < freeVertexPosition-1; i++) { for (int j = i+1; j< freeVertexPosition; j++) { - assert( _vertices[i]!= _vertices[j]); + // assert( _vertices[i]!= _vertices[j]); } } # endif @@ -370,7 +370,7 @@ class QuaternionGenericEdge: public ROAMestimation::GenericEdge switch (ov->dimension()) { // this is for the dimension of temporaries case 3: // SO or qOS, cols = 3 - if (k == 1 || k == 7) { // with respect to some quaternion, rows = 4 + if (k == 1 || k == 7 || k == 9) { // with respect to some quaternion, rows = 4 computeJacobianBlock(k, v, _JxArgs4x3); _jacobianOplus[v].noalias() += (*_JErrx[k]) * _JxArgs4x3; } else { @@ -382,7 +382,7 @@ class QuaternionGenericEdge: public ROAMestimation::GenericEdge case 6: // pose vertices, cols = 6 - if (k == 1 || k == 7) { // with respect to q or delta q, rows = 4 + if (k == 1 || k == 7 || k == 9) { // with respect to q or delta q, rows = 4 computeJacobianBlock(k, v, _JxArgs4x6); _jacobianOplus[v].noalias() += (*_JErrx[k]) * _JxArgs4x6; } else { diff --git a/roamfree/ROAMfunctions/generated/PlaneDynamicModel_Err.cppready b/roamfree/ROAMfunctions/generated/PlaneDynamicModel_Err.cppready index 00a4db8..4935443 100644 --- a/roamfree/ROAMfunctions/generated/PlaneDynamicModel_Err.cppready +++ b/roamfree/ROAMfunctions/generated/PlaneDynamicModel_Err.cppready @@ -1,209 +1,272 @@ -double tmp0 = x(5+_OFF); -double tmp1 = x(6+_OFF); -double tmp2 = x(4+_OFF); +double tmp0 = x(4+_OFF); +double tmp1 = x(5+_OFF); +double tmp2 = x(6+_OFF); double tmp3 = x(7+_OFF); -double tmp4 = wind(2+_OFF); -double tmp5 = tmp0*tmp1; -double tmp6 = wind(3+_OFF); -double tmp7 = -(tmp2*tmp1); -double tmp8 = tmp0*tmp3; -double tmp9 = tmp7 + tmp8; -double tmp10 = wind(1+_OFF); -double tmp11 = std::pow(tmp2,2); -double tmp12 = std::pow(tmp0,2); -double tmp13 = std::pow(tmp1,2); -double tmp14 = std::pow(tmp3,2); -double tmp15 = -tmp14; -double tmp16 = -(tmp2*tmp3); -double tmp17 = tmp5 + tmp16; -double tmp18 = -2*tmp10*tmp17; -double tmp19 = tmp2*tmp0; -double tmp20 = tmp1*tmp3; -double tmp21 = tmp19 + tmp20; -double tmp22 = -2*tmp6*tmp21; -double tmp23 = -tmp12; -double tmp24 = tmp11 + tmp23 + tmp13 + tmp15; -double tmp25 = -(tmp4*tmp24); -double tmp26 = x(9+_OFF); -double tmp27 = tmp18 + tmp22 + tmp25 + tmp26; -double tmp28 = -tmp13; -double tmp29 = x(8+_OFF); -double tmp30 = tmp2*tmp3; -double tmp31 = tmp5 + tmp30; -double tmp32 = -2*tmp4*tmp31; -double tmp33 = -2*tmp6*tmp9; -double tmp34 = tmp11 + tmp12 + tmp28 + tmp15; -double tmp35 = -(tmp10*tmp34); -double tmp36 = tmp32 + tmp33 + tmp35 + tmp29; -double tmp37 = std::pow(tmp36,2); -double tmp38 = tmp2*tmp1; -double tmp39 = tmp38 + tmp8; -double tmp40 = -(tmp2*tmp0); -double tmp41 = tmp40 + tmp20; -double tmp42 = tmp11 + tmp23 + tmp28 + tmp14; -double tmp43 = x(10+_OFF); -double tmp44 = std::pow(tmp27,2); -double tmp45 = -2*tmp10*tmp39; -double tmp46 = -2*tmp4*tmp41; -double tmp47 = -(tmp6*tmp42); -double tmp48 = tmp45 + tmp46 + tmp47 + tmp43; -double tmp49 = std::pow(tmp48,2); -double tmp50 = tmp37 + tmp44 + tmp49; -double tmp51 = cp(2+_OFF); -double tmp52 = 2*tmp10*tmp39; -double tmp53 = 2*tmp4*tmp41; -double tmp54 = tmp6*tmp42; -double tmp55 = -tmp43; -double tmp56 = tmp52 + tmp53 + tmp54 + tmp55; -double tmp57 = std::pow(tmp56,2); -double tmp58 = tmp37 + tmp57; +double tmp4 = -(tmp0*tmp2); +double tmp5 = tmp1*tmp3; +double tmp6 = tmp4 + tmp5; +double tmp7 = x(30+_OFF); +double tmp8 = x(31+_OFF); +double tmp9 = x(32+_OFF); +double tmp10 = x(33+_OFF); +double tmp11 = tmp8*tmp9; +double tmp12 = std::pow(tmp7,2); +double tmp13 = std::pow(tmp8,2); +double tmp14 = std::pow(tmp9,2); +double tmp15 = std::pow(tmp10,2); +double tmp16 = -tmp15; +double tmp17 = wind(1+_OFF); +double tmp18 = tmp8*tmp10; +double tmp19 = wind(2+_OFF); +double tmp20 = tmp9*tmp10; +double tmp21 = wind(3+_OFF); +double tmp22 = -tmp13; +double tmp23 = -tmp14; +double tmp24 = tmp7*tmp10; +double tmp25 = tmp11 + tmp24; +double tmp26 = -2*tmp19*tmp25; +double tmp27 = -(tmp7*tmp9); +double tmp28 = tmp27 + tmp18; +double tmp29 = -2*tmp21*tmp28; +double tmp30 = tmp12 + tmp13 + tmp23 + tmp16; +double tmp31 = -(tmp17*tmp30); +double tmp32 = x(34+_OFF); +double tmp33 = tmp26 + tmp29 + tmp31 + tmp32; +double tmp34 = tmp7*tmp9; +double tmp35 = tmp34 + tmp18; +double tmp36 = -2*tmp17*tmp35; +double tmp37 = -(tmp7*tmp8); +double tmp38 = tmp37 + tmp20; +double tmp39 = -2*tmp19*tmp38; +double tmp40 = tmp12 + tmp22 + tmp23 + tmp15; +double tmp41 = -(tmp21*tmp40); +double tmp42 = x(36+_OFF); +double tmp43 = tmp36 + tmp39 + tmp41 + tmp42; +double tmp44 = std::atan2(tmp43,tmp33); +double tmp45 = -(tmp7*tmp10); +double tmp46 = tmp11 + tmp45; +double tmp47 = tmp7*tmp8; +double tmp48 = tmp47 + tmp20; +double tmp49 = tmp12 + tmp22 + tmp14 + tmp16; +double tmp50 = -2*tmp17*tmp46; +double tmp51 = -2*tmp21*tmp48; +double tmp52 = -(tmp19*tmp49); +double tmp53 = x(35+_OFF); +double tmp54 = tmp50 + tmp51 + tmp52 + tmp53; +double tmp55 = std::pow(tmp33,2); +double tmp56 = std::pow(tmp54,2); +double tmp57 = std::pow(tmp43,2); +double tmp58 = tmp55 + tmp56 + tmp57; double tmp59 = 1/std::sqrt(tmp58); -double tmp60 = std::atan2(tmp56,tmp36); -double tmp61 = 1/std::sqrt(tmp50); -double tmp62 = tmp27*tmp61; -double tmp63 = std::asin(tmp62); -double tmp64 = -(tmp10*tmp11); -double tmp65 = -(tmp10*tmp12); -double tmp66 = 2*tmp6*tmp2*tmp1; -double tmp67 = -2*tmp4*tmp0*tmp1; -double tmp68 = tmp10*tmp13; -double tmp69 = -2*tmp4*tmp2*tmp3; -double tmp70 = -2*tmp6*tmp0*tmp3; -double tmp71 = tmp10*tmp14; -double tmp72 = tmp64 + tmp65 + tmp66 + tmp67 + tmp68 + tmp69 + tmp70 + tmp71 + tmp29; -double tmp73 = cp(1+_OFF); -double tmp74 = std::sqrt(tmp50); -double tmp75 = std::pow(tmp73,2); -double tmp76 = z(1+_OFF); -double tmp77 = gravity(1+_OFF); -double tmp78 = cp(4+_OFF); -double tmp79 = 1/tmp78; -double tmp80 = fDrag(1+_OFF); -double tmp81 = fDrag(2+_OFF); -double tmp82 = tmp60*tmp81; -double tmp83 = std::pow(tmp60,2); -double tmp84 = fDrag(3+_OFF); +double tmp60 = tmp54*tmp59; +double tmp61 = std::asin(tmp60); +double tmp62 = cp(2+_OFF); +double tmp63 = std::pow(tmp0,2); +double tmp64 = std::pow(tmp1,2); +double tmp65 = std::pow(tmp2,2); +double tmp66 = -tmp65; +double tmp67 = std::pow(tmp3,2); +double tmp68 = -tmp67; +double tmp69 = tmp63 + tmp64 + tmp66 + tmp68; +double tmp70 = tmp1*tmp2; +double tmp71 = tmp0*tmp3; +double tmp72 = tmp70 + tmp71; +double tmp73 = fLat(1+_OFF); +double tmp74 = std::sqrt(tmp58); +double tmp75 = tmp55 + tmp57; +double tmp76 = 1/std::sqrt(tmp75); +double tmp77 = fDrag(1+_OFF); +double tmp78 = fDrag(2+_OFF); +double tmp79 = -(tmp44*tmp78); +double tmp80 = std::pow(tmp44,2); +double tmp81 = fDrag(3+_OFF); +double tmp82 = tmp80*tmp81; +double tmp83 = std::pow(tmp61,2); +double tmp84 = fDrag(4+_OFF); double tmp85 = tmp83*tmp84; -double tmp86 = std::pow(tmp63,2); -double tmp87 = fDrag(4+_OFF); -double tmp88 = tmp86*tmp87; -double tmp89 = tmp80 + tmp82 + tmp85 + tmp88; -double tmp90 = fLat(1+_OFF); -double tmp91 = 1/tmp50; -double tmp92 = -(tmp44*tmp91); -double tmp93 = 1 + tmp92; -double tmp94 = std::sqrt(tmp93); -double tmp95 = tmp4*tmp11; -double tmp96 = 2*tmp6*tmp2*tmp0; -double tmp97 = -(tmp4*tmp12); -double tmp98 = 2*tmp10*tmp0*tmp1; -double tmp99 = tmp4*tmp13; -double tmp100 = -2*tmp10*tmp2*tmp3; -double tmp101 = 2*tmp6*tmp1*tmp3; -double tmp102 = -(tmp4*tmp14); -double tmp103 = -tmp26; -double tmp104 = tmp95 + tmp96 + tmp97 + tmp98 + tmp99 + tmp100 + tmp101 + tmp102 + tmp103; -double tmp105 = fLift(1+_OFF); -double tmp106 = fLift(2+_OFF); -double tmp107 = tmp60*tmp106; -double tmp108 = tmp105 + tmp107; -double tmp109 = -(tmp6*tmp11); -double tmp110 = 2*tmp4*tmp2*tmp0; -double tmp111 = tmp6*tmp12; -double tmp112 = -2*tmp10*tmp2*tmp1; -double tmp113 = tmp6*tmp13; -double tmp114 = -2*tmp10*tmp0*tmp3; -double tmp115 = -2*tmp4*tmp1*tmp3; -double tmp116 = -(tmp6*tmp14); -double tmp117 = tmp109 + tmp110 + tmp111 + tmp112 + tmp113 + tmp114 + tmp115 + tmp116 + tmp43; -double tmp118 = ibd(2+_OFF); -double tmp119 = ibd(3+_OFF); -double tmp120 = ibod(1+_OFF); -double tmp121 = ibod(2+_OFF); -double tmp122 = ibod(3+_OFF); -double tmp123 = ibd(1+_OFF); -double tmp124 = std::pow(tmp122,2); -double tmp125 = x(12+_OFF); -double tmp126 = x(11+_OFF); -double tmp127 = x(13+_OFF); -double tmp128 = cp(3+_OFF); -double tmp129 = tmp123*tmp118*tmp119; -double tmp130 = std::pow(tmp120,2); -double tmp131 = -(tmp118*tmp130); -double tmp132 = std::pow(tmp121,2); -double tmp133 = -(tmp119*tmp132); -double tmp134 = 2*tmp120*tmp121*tmp122; -double tmp135 = -(tmp123*tmp124); -double tmp136 = tmp129 + tmp131 + tmp133 + tmp134 + tmp135; -double tmp137 = 1/tmp136; -double tmp138 = std::pow(tmp127,2); -double tmp139 = cBar(1+_OFF); -double tmp140 = std::pow(tmp126,2); -double tmp141 = std::pow(tmp125,2); -double tmp142 = -(tmp119*tmp121); -double tmp143 = tmp120*tmp122; -double tmp144 = tmp142 + tmp143; -double tmp145 = -(tmp120*tmp126*tmp125); -double tmp146 = -(tmp122*tmp141); -double tmp147 = tmp121*tmp126*tmp127; -double tmp148 = tmp118*tmp125*tmp127; -double tmp149 = -(tmp119*tmp125*tmp127); -double tmp150 = tmp122*tmp138; -double tmp151 = mRoll(2+_OFF); -double tmp152 = -(tmp63*tmp151); -double tmp153 = mRoll(3+_OFF); -double tmp154 = (tmp128*tmp153*tmp61*tmp126)/2.; -double tmp155 = mRoll(4+_OFF); -double tmp156 = -(tmp128*tmp155*tmp61*tmp127)/2.; -double tmp157 = mRoll(1+_OFF); -double tmp158 = z(2+_OFF); -double tmp159 = -(tmp157*tmp158); -double tmp160 = tmp152 + tmp154 + tmp156 + tmp159; -double tmp161 = 0.5*tmp51*tmp128*tmp50*tmp160; -double tmp162 = tmp145 + tmp146 + tmp147 + tmp148 + tmp149 + tmp150 + tmp161; -double tmp163 = tmp120*tmp140; -double tmp164 = tmp122*tmp126*tmp125; -double tmp165 = -(tmp123*tmp126*tmp127); -double tmp166 = tmp119*tmp126*tmp127; -double tmp167 = -(tmp121*tmp125*tmp127); -double tmp168 = -(tmp120*tmp138); -double tmp169 = mPitch(1+_OFF); -double tmp170 = -tmp169; -double tmp171 = mPitch(3+_OFF); -double tmp172 = -(tmp60*tmp171); -double tmp173 = mPitch(4+_OFF); -double tmp174 = (tmp139*tmp173*tmp61*tmp125)/2.; -double tmp175 = mPitch(2+_OFF); -double tmp176 = z(3+_OFF); -double tmp177 = -(tmp175*tmp176); -double tmp178 = tmp170 + tmp172 + tmp174 + tmp177; -double tmp179 = 0.5*tmp139*tmp51*tmp50*tmp178; -double tmp180 = tmp163 + tmp164 + tmp165 + tmp166 + tmp167 + tmp168 + tmp179; -double tmp181 = -(tmp121*tmp140); -double tmp182 = tmp123*tmp126*tmp125; -double tmp183 = -(tmp118*tmp126*tmp125); -double tmp184 = tmp121*tmp141; -double tmp185 = -(tmp122*tmp126*tmp127); -double tmp186 = tmp120*tmp125*tmp127; -double tmp187 = mYaw(2+_OFF); -double tmp188 = tmp63*tmp187; -double tmp189 = mYaw(3+_OFF); -double tmp190 = (tmp128*tmp189*tmp61*tmp127)/2.; -double tmp191 = mYaw(1+_OFF); -double tmp192 = z(4+_OFF); -double tmp193 = tmp191*tmp192; -double tmp194 = tmp188 + tmp190 + tmp193; -double tmp195 = 0.5*tmp51*tmp128*tmp50*tmp194; -double tmp196 = tmp181 + tmp182 + tmp183 + tmp184 + tmp185 + tmp186 + tmp195; -double tmp197 = -(tmp118*tmp120); -double tmp198 = tmp121*tmp122; -double tmp199 = tmp197 + tmp198; -double tmp200 = tmp120*tmp121; -double tmp201 = -(tmp123*tmp122); -double tmp202 = tmp200 + tmp201; -err(1+_OFF) = 2*tmp77*tmp9 + x(14+_OFF) - tmp79*(0.5*tmp63*tmp51*tmp90*tmp72*tmp104*tmp59*tmp74 - 0.5*tmp51*tmp108*tmp59*(tmp6*tmp11 - 2*tmp4*tmp2*tmp0 - tmp6*tmp12 + 2*tmp10*tmp2*tmp1 - tmp6*tmp13 + 2*tmp10*tmp0*tmp3 + 2*tmp4*tmp1*tmp3 + tmp6*tmp14 + tmp55)*tmp50 + 0.5*tmp51*tmp89*tmp72*tmp59*tmp50*tmp94 + 1.*tmp75*((fThrust(3+_OFF)*tmp50)/std::pow(M_PI,2) + (tmp73*fThrust(2+_OFF)*tmp74*tmp76)/M_PI + tmp75*fThrust(1+_OFF)*std::pow(tmp76,2))); -err(2+_OFF) = 2*tmp77*tmp21 - tmp79*(0. + 0.5*tmp51*tmp89*(-(tmp4*tmp11) - 2*tmp6*tmp2*tmp0 + tmp4*tmp12 - 2*tmp10*tmp0*tmp1 - tmp4*tmp13 + 2*tmp10*tmp2*tmp3 - 2*tmp6*tmp1*tmp3 + tmp4*tmp14 + tmp26)*tmp74 + 0.5*tmp63*tmp51*tmp90*tmp50*tmp94) + x(15+_OFF); -err(3+_OFF) = tmp77*tmp42 - tmp79*(0.5*tmp63*tmp51*tmp90*tmp104*tmp59*tmp117*tmp74 - 0.5*tmp51*tmp108*tmp72*tmp59*tmp50 + 0.5*tmp51*tmp89*tmp59*tmp117*tmp50*tmp94) + x(16+_OFF); -err(4+_OFF) = x(17+_OFF) - (tmp118*tmp119 - tmp124)*tmp137*tmp162 - tmp144*tmp137*tmp180 - tmp199*tmp137*tmp196; -err(5+_OFF) = x(18+_OFF) - tmp144*tmp137*tmp162 - (tmp123*tmp119 - tmp130)*tmp137*tmp180 - tmp202*tmp137*tmp196; -err(6+_OFF) = x(19+_OFF) - tmp199*tmp137*tmp162 - tmp202*tmp137*tmp180 - (tmp123*tmp118 - tmp132)*tmp137*tmp196; +double tmp86 = tmp77 + tmp79 + tmp82 + tmp85; +double tmp87 = 1/tmp58; +double tmp88 = -(tmp56*tmp87); +double tmp89 = 1 + tmp88; +double tmp90 = std::sqrt(tmp89); +double tmp91 = fLift(1+_OFF); +double tmp92 = -tmp91; +double tmp93 = fLift(2+_OFF); +double tmp94 = tmp44*tmp93; +double tmp95 = tmp92 + tmp94; +double tmp96 = cp(1+_OFF); +double tmp97 = std::pow(tmp96,2); +double tmp98 = z(1+_OFF); +double tmp99 = gravity(1+_OFF); +double tmp100 = cp(4+_OFF); +double tmp101 = 1/tmp100; +double tmp102 = tmp0*tmp1; +double tmp103 = tmp2*tmp3; +double tmp104 = tmp102 + tmp103; +double tmp105 = 0.634*tmp62*tmp86*tmp54*tmp74; +double tmp106 = 0.634*tmp61*tmp62*tmp73*tmp58*tmp90; +double tmp107 = 0. + tmp105 + tmp106; +double tmp108 = -(tmp0*tmp3); +double tmp109 = tmp70 + tmp108; +double tmp110 = -tmp64; +double tmp111 = tmp63 + tmp110 + tmp65 + tmp68; +double tmp112 = -0.634*tmp61*tmp62*tmp73*tmp54*tmp43*tmp76*tmp74; +double tmp113 = 0.634*tmp62*tmp95*tmp33*tmp76*tmp58; +double tmp114 = 0.634*tmp62*tmp86*tmp43*tmp76*tmp58*tmp90; +double tmp115 = tmp112 + tmp113 + tmp114; +double tmp116 = -0.634*tmp61*tmp62*tmp73*tmp33*tmp54*tmp76*tmp74; +double tmp117 = -0.634*tmp62*tmp95*tmp43*tmp76*tmp58; +double tmp118 = 0.634*tmp62*tmp86*tmp33*tmp76*tmp58*tmp90; +double tmp119 = std::pow(M_PI,-2); +double tmp120 = fThrust(3+_OFF); +double tmp121 = tmp119*tmp120*tmp58; +double tmp122 = 1/M_PI; +double tmp123 = fThrust(2+_OFF); +double tmp124 = tmp122*tmp96*tmp123*tmp74*tmp98; +double tmp125 = fThrust(1+_OFF); +double tmp126 = std::pow(tmp98,2); +double tmp127 = tmp97*tmp125*tmp126; +double tmp128 = tmp121 + tmp124 + tmp127; +double tmp129 = 1.268*tmp97*tmp128; +double tmp130 = tmp116 + tmp117 + tmp118 + tmp129; +double tmp131 = tmp63 + tmp110 + tmp66 + tmp67; +double tmp132 = tmp0*tmp2; +double tmp133 = tmp132 + tmp5; +double tmp134 = -(tmp0*tmp1); +double tmp135 = tmp134 + tmp103; +double tmp136 = ibd(2+_OFF); +double tmp137 = ibd(3+_OFF); +double tmp138 = ibod(1+_OFF); +double tmp139 = ibod(2+_OFF); +double tmp140 = ibod(3+_OFF); +double tmp141 = ibd(1+_OFF); +double tmp142 = std::pow(tmp140,2); +double tmp143 = x(12+_OFF); +double tmp144 = x(11+_OFF); +double tmp145 = x(13+_OFF); +double tmp146 = 4*tmp72*tmp25; +double tmp147 = 4*tmp6*tmp28; +double tmp148 = tmp69*tmp30; +double tmp149 = tmp146 + tmp147 + tmp148; +double tmp150 = cp(3+_OFF); +double tmp151 = 2*tmp69*tmp46; +double tmp152 = 4*tmp6*tmp48; +double tmp153 = 2*tmp72*tmp49; +double tmp154 = tmp151 + tmp152 + tmp153; +double tmp155 = cBar(1+_OFF); +double tmp156 = 2*tmp69*tmp35; +double tmp157 = 4*tmp72*tmp38; +double tmp158 = 2*tmp6*tmp40; +double tmp159 = tmp156 + tmp157 + tmp158; +double tmp160 = x(39+_OFF); +double tmp161 = tmp141*tmp136*tmp137; +double tmp162 = std::pow(tmp138,2); +double tmp163 = -(tmp136*tmp162); +double tmp164 = std::pow(tmp139,2); +double tmp165 = -(tmp137*tmp164); +double tmp166 = 2*tmp138*tmp139*tmp140; +double tmp167 = -(tmp141*tmp142); +double tmp168 = tmp161 + tmp163 + tmp165 + tmp166 + tmp167; +double tmp169 = 1/tmp168; +double tmp170 = std::pow(tmp145,2); +double tmp171 = 2*tmp111*tmp25; +double tmp172 = 4*tmp104*tmp28; +double tmp173 = 2*tmp109*tmp30; +double tmp174 = tmp171 + tmp172 + tmp173; +double tmp175 = mRoll(2+_OFF); +double tmp176 = -(tmp61*tmp175); +double tmp177 = mRoll(3+_OFF); +double tmp178 = x(37+_OFF); +double tmp179 = (tmp150*tmp177*tmp59*tmp178)/2.; +double tmp180 = mRoll(4+_OFF); +double tmp181 = -0.5*(tmp150*tmp180*tmp59*tmp160); +double tmp182 = mRoll(1+_OFF); +double tmp183 = z(2+_OFF); +double tmp184 = tmp182*tmp183; +double tmp185 = tmp176 + tmp179 + tmp181 + tmp184; +double tmp186 = 4*tmp109*tmp46; +double tmp187 = 4*tmp104*tmp48; +double tmp188 = tmp111*tmp49; +double tmp189 = tmp186 + tmp187 + tmp188; +double tmp190 = mPitch(1+_OFF); +double tmp191 = -tmp190; +double tmp192 = mPitch(3+_OFF); +double tmp193 = tmp44*tmp192; +double tmp194 = mPitch(4+_OFF); +double tmp195 = x(38+_OFF); +double tmp196 = (tmp155*tmp194*tmp59*tmp195)/2.; +double tmp197 = mPitch(2+_OFF); +double tmp198 = z(3+_OFF); +double tmp199 = -(tmp197*tmp198); +double tmp200 = tmp191 + tmp193 + tmp196 + tmp199; +double tmp201 = 4*tmp109*tmp35; +double tmp202 = 2*tmp111*tmp38; +double tmp203 = 2*tmp104*tmp40; +double tmp204 = tmp201 + tmp202 + tmp203; +double tmp205 = mYaw(2+_OFF); +double tmp206 = tmp61*tmp205; +double tmp207 = mYaw(3+_OFF); +double tmp208 = (tmp150*tmp207*tmp59*tmp160)/2.; +double tmp209 = mYaw(1+_OFF); +double tmp210 = z(4+_OFF); +double tmp211 = -(tmp209*tmp210); +double tmp212 = tmp206 + tmp208 + tmp211; +double tmp213 = std::pow(tmp144,2); +double tmp214 = std::pow(tmp143,2); +double tmp215 = 4*tmp135*tmp25; +double tmp216 = 2*tmp131*tmp28; +double tmp217 = 2*tmp133*tmp30; +double tmp218 = tmp215 + tmp216 + tmp217; +double tmp219 = 4*tmp133*tmp46; +double tmp220 = 2*tmp131*tmp48; +double tmp221 = 2*tmp135*tmp49; +double tmp222 = tmp219 + tmp220 + tmp221; +double tmp223 = 4*tmp133*tmp35; +double tmp224 = 4*tmp135*tmp38; +double tmp225 = tmp131*tmp40; +double tmp226 = tmp223 + tmp224 + tmp225; +double tmp227 = -(tmp137*tmp139); +double tmp228 = tmp138*tmp140; +double tmp229 = tmp227 + tmp228; +double tmp230 = -(tmp138*tmp144*tmp143); +double tmp231 = -(tmp140*tmp214); +double tmp232 = tmp139*tmp144*tmp145; +double tmp233 = tmp136*tmp143*tmp145; +double tmp234 = -(tmp137*tmp143*tmp145); +double tmp235 = tmp140*tmp170; +double tmp236 = 0.634*tmp62*tmp150*tmp149*tmp58*tmp185; +double tmp237 = 0.634*tmp155*tmp62*tmp154*tmp58*tmp200; +double tmp238 = 0.634*tmp62*tmp150*tmp159*tmp58*tmp212; +double tmp239 = tmp230 + tmp231 + tmp232 + tmp233 + tmp234 + tmp235 + tmp236 + tmp237 + tmp238; +double tmp240 = tmp138*tmp213; +double tmp241 = tmp140*tmp144*tmp143; +double tmp242 = -(tmp141*tmp144*tmp145); +double tmp243 = tmp137*tmp144*tmp145; +double tmp244 = -(tmp139*tmp143*tmp145); +double tmp245 = -(tmp138*tmp170); +double tmp246 = 0.634*tmp62*tmp150*tmp174*tmp58*tmp185; +double tmp247 = 0.634*tmp155*tmp62*tmp189*tmp58*tmp200; +double tmp248 = 0.634*tmp62*tmp150*tmp204*tmp58*tmp212; +double tmp249 = tmp240 + tmp241 + tmp242 + tmp243 + tmp244 + tmp245 + tmp246 + tmp247 + tmp248; +double tmp250 = -(tmp139*tmp213); +double tmp251 = tmp141*tmp144*tmp143; +double tmp252 = -(tmp136*tmp144*tmp143); +double tmp253 = tmp139*tmp214; +double tmp254 = -(tmp140*tmp144*tmp145); +double tmp255 = tmp138*tmp143*tmp145; +double tmp256 = 0.634*tmp62*tmp150*tmp218*tmp58*tmp185; +double tmp257 = 0.634*tmp155*tmp62*tmp222*tmp58*tmp200; +double tmp258 = 0.634*tmp62*tmp150*tmp226*tmp58*tmp212; +double tmp259 = tmp250 + tmp251 + tmp252 + tmp253 + tmp254 + tmp255 + tmp256 + tmp257 + tmp258; +double tmp260 = -(tmp136*tmp138); +double tmp261 = tmp139*tmp140; +double tmp262 = tmp260 + tmp261; +double tmp263 = tmp138*tmp139; +double tmp264 = -(tmp141*tmp140); +double tmp265 = tmp263 + tmp264; +err(1+_OFF) = 2*tmp99*tmp6 + x(14+_OFF) - tmp101*(tmp154*tmp107 + tmp159*tmp115 + tmp149*tmp130); +err(2+_OFF) = 2*tmp99*tmp104 + x(15+_OFF) - tmp101*(tmp189*tmp107 + tmp204*tmp115 + tmp174*tmp130); +err(3+_OFF) = tmp99*tmp131 + x(16+_OFF) - tmp101*(tmp222*tmp107 + tmp226*tmp115 + tmp218*tmp130); +err(4+_OFF) = x(17+_OFF) - (tmp136*tmp137 - tmp142)*tmp169*tmp239 - tmp229*tmp169*tmp249 - tmp262*tmp169*tmp259; +err(5+_OFF) = x(18+_OFF) - tmp229*tmp169*tmp239 - (tmp141*tmp137 - tmp162)*tmp169*tmp249 - tmp265*tmp169*tmp259; +err(6+_OFF) = x(19+_OFF) - tmp262*tmp169*tmp239 - tmp265*tmp169*tmp249 - (tmp141*tmp136 - tmp164)*tmp169*tmp259; diff --git a/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrCBar.cppready b/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrCBar.cppready index f826481..a285cbf 100644 --- a/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrCBar.cppready +++ b/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrCBar.cppready @@ -1,97 +1,148 @@ -double tmp0 = ibd(3+_OFF); -double tmp1 = ibd(2+_OFF); +double tmp0 = ibd(2+_OFF); +double tmp1 = ibd(3+_OFF); double tmp2 = ibod(1+_OFF); double tmp3 = ibod(2+_OFF); double tmp4 = ibod(3+_OFF); double tmp5 = ibd(1+_OFF); -double tmp6 = x(4+_OFF); -double tmp7 = x(6+_OFF); -double tmp8 = x(5+_OFF); -double tmp9 = x(7+_OFF); -double tmp10 = wind(1+_OFF); -double tmp11 = tmp8*tmp7; -double tmp12 = wind(3+_OFF); -double tmp13 = wind(2+_OFF); -double tmp14 = std::pow(tmp6,2); -double tmp15 = std::pow(tmp8,2); -double tmp16 = std::pow(tmp7,2); -double tmp17 = std::pow(tmp9,2); -double tmp18 = -tmp17; -double tmp19 = tmp8*tmp9; -double tmp20 = tmp7*tmp9; -double tmp21 = -tmp15; -double tmp22 = -tmp16; -double tmp23 = cp(2+_OFF); -double tmp24 = tmp6*tmp9; -double tmp25 = tmp11 + tmp24; -double tmp26 = -2*tmp13*tmp25; -double tmp27 = -(tmp6*tmp7); -double tmp28 = tmp27 + tmp19; -double tmp29 = -2*tmp12*tmp28; -double tmp30 = tmp14 + tmp15 + tmp22 + tmp18; -double tmp31 = -(tmp10*tmp30); -double tmp32 = x(8+_OFF); -double tmp33 = tmp26 + tmp29 + tmp31 + tmp32; -double tmp34 = std::pow(tmp33,2); -double tmp35 = -(tmp6*tmp9); -double tmp36 = tmp11 + tmp35; -double tmp37 = -2*tmp10*tmp36; -double tmp38 = tmp6*tmp8; -double tmp39 = tmp38 + tmp20; -double tmp40 = -2*tmp12*tmp39; -double tmp41 = tmp14 + tmp21 + tmp16 + tmp18; -double tmp42 = -(tmp13*tmp41); -double tmp43 = x(9+_OFF); -double tmp44 = tmp37 + tmp40 + tmp42 + tmp43; -double tmp45 = std::pow(tmp44,2); -double tmp46 = tmp6*tmp7; -double tmp47 = tmp46 + tmp19; -double tmp48 = -2*tmp10*tmp47; -double tmp49 = -(tmp6*tmp8); -double tmp50 = tmp49 + tmp20; -double tmp51 = -2*tmp13*tmp50; -double tmp52 = tmp14 + tmp21 + tmp22 + tmp17; -double tmp53 = -(tmp12*tmp52); -double tmp54 = x(10+_OFF); -double tmp55 = tmp48 + tmp51 + tmp53 + tmp54; -double tmp56 = std::pow(tmp55,2); -double tmp57 = tmp34 + tmp45 + tmp56; -double tmp58 = cBar(1+_OFF); -double tmp59 = mPitch(4+_OFF); -double tmp60 = x(12+_OFF); -double tmp61 = std::pow(tmp2,2); -double tmp62 = tmp5*tmp1*tmp0; -double tmp63 = -(tmp1*tmp61); -double tmp64 = std::pow(tmp3,2); -double tmp65 = -(tmp0*tmp64); -double tmp66 = 2*tmp2*tmp3*tmp4; -double tmp67 = std::pow(tmp4,2); -double tmp68 = -(tmp5*tmp67); -double tmp69 = tmp62 + tmp63 + tmp65 + tmp66 + tmp68; -double tmp70 = 1/tmp69; -double tmp71 = std::sqrt(tmp57); -double tmp72 = 0.25*tmp58*tmp23*tmp59*tmp71*tmp60; -double tmp73 = mPitch(1+_OFF); -double tmp74 = -tmp73; -double tmp75 = 2*tmp10*tmp47; -double tmp76 = 2*tmp13*tmp50; -double tmp77 = tmp12*tmp52; -double tmp78 = -tmp54; -double tmp79 = tmp75 + tmp76 + tmp77 + tmp78; -double tmp80 = std::atan2(tmp79,tmp33); -double tmp81 = mPitch(3+_OFF); -double tmp82 = -(tmp80*tmp81); -double tmp83 = 1/std::sqrt(tmp57); -double tmp84 = (tmp58*tmp59*tmp83*tmp60)/2.; -double tmp85 = mPitch(2+_OFF); -double tmp86 = z(3+_OFF); -double tmp87 = -(tmp85*tmp86); -double tmp88 = tmp74 + tmp82 + tmp84 + tmp87; -double tmp89 = 0.5*tmp23*tmp57*tmp88; -double tmp90 = tmp72 + tmp89; +double tmp6 = std::pow(tmp4,2); +double tmp7 = x(4+_OFF); +double tmp8 = x(6+_OFF); +double tmp9 = x(5+_OFF); +double tmp10 = x(7+_OFF); +double tmp11 = x(30+_OFF); +double tmp12 = x(31+_OFF); +double tmp13 = x(32+_OFF); +double tmp14 = x(33+_OFF); +double tmp15 = tmp12*tmp13; +double tmp16 = std::pow(tmp11,2); +double tmp17 = std::pow(tmp12,2); +double tmp18 = std::pow(tmp13,2); +double tmp19 = std::pow(tmp14,2); +double tmp20 = -tmp19; +double tmp21 = wind(1+_OFF); +double tmp22 = -(tmp11*tmp14); +double tmp23 = tmp15 + tmp22; +double tmp24 = wind(3+_OFF); +double tmp25 = tmp11*tmp12; +double tmp26 = tmp13*tmp14; +double tmp27 = tmp25 + tmp26; +double tmp28 = wind(2+_OFF); +double tmp29 = -tmp17; +double tmp30 = tmp16 + tmp29 + tmp18 + tmp20; +double tmp31 = tmp12*tmp14; +double tmp32 = -tmp18; +double tmp33 = cp(2+_OFF); +double tmp34 = std::pow(tmp7,2); +double tmp35 = std::pow(tmp9,2); +double tmp36 = std::pow(tmp8,2); +double tmp37 = -tmp36; +double tmp38 = std::pow(tmp10,2); +double tmp39 = -tmp38; +double tmp40 = tmp34 + tmp35 + tmp37 + tmp39; +double tmp41 = 2*tmp40*tmp23; +double tmp42 = -(tmp7*tmp8); +double tmp43 = tmp9*tmp10; +double tmp44 = tmp42 + tmp43; +double tmp45 = 4*tmp44*tmp27; +double tmp46 = tmp9*tmp8; +double tmp47 = tmp7*tmp10; +double tmp48 = tmp46 + tmp47; +double tmp49 = 2*tmp48*tmp30; +double tmp50 = tmp41 + tmp45 + tmp49; +double tmp51 = tmp11*tmp14; +double tmp52 = tmp15 + tmp51; +double tmp53 = -2*tmp28*tmp52; +double tmp54 = -(tmp11*tmp13); +double tmp55 = tmp54 + tmp31; +double tmp56 = -2*tmp24*tmp55; +double tmp57 = tmp16 + tmp17 + tmp32 + tmp20; +double tmp58 = -(tmp21*tmp57); +double tmp59 = x(34+_OFF); +double tmp60 = tmp53 + tmp56 + tmp58 + tmp59; +double tmp61 = std::pow(tmp60,2); +double tmp62 = -2*tmp21*tmp23; +double tmp63 = -2*tmp24*tmp27; +double tmp64 = -(tmp28*tmp30); +double tmp65 = x(35+_OFF); +double tmp66 = tmp62 + tmp63 + tmp64 + tmp65; +double tmp67 = std::pow(tmp66,2); +double tmp68 = tmp11*tmp13; +double tmp69 = tmp68 + tmp31; +double tmp70 = -2*tmp21*tmp69; +double tmp71 = -(tmp11*tmp12); +double tmp72 = tmp71 + tmp26; +double tmp73 = -2*tmp28*tmp72; +double tmp74 = tmp16 + tmp29 + tmp32 + tmp19; +double tmp75 = -(tmp24*tmp74); +double tmp76 = x(36+_OFF); +double tmp77 = tmp70 + tmp73 + tmp75 + tmp76; +double tmp78 = std::pow(tmp77,2); +double tmp79 = tmp61 + tmp67 + tmp78; +double tmp80 = cBar(1+_OFF); +double tmp81 = mPitch(4+_OFF); +double tmp82 = x(38+_OFF); +double tmp83 = tmp5*tmp0*tmp1; +double tmp84 = std::pow(tmp2,2); +double tmp85 = -(tmp0*tmp84); +double tmp86 = std::pow(tmp3,2); +double tmp87 = -(tmp1*tmp86); +double tmp88 = 2*tmp2*tmp3*tmp4; +double tmp89 = -(tmp5*tmp6); +double tmp90 = tmp83 + tmp85 + tmp87 + tmp88 + tmp89; +double tmp91 = 1/tmp90; +double tmp92 = std::sqrt(tmp79); +double tmp93 = tmp7*tmp8; +double tmp94 = tmp93 + tmp43; +double tmp95 = 4*tmp94*tmp23; +double tmp96 = -tmp35; +double tmp97 = tmp34 + tmp96 + tmp37 + tmp38; +double tmp98 = 2*tmp97*tmp27; +double tmp99 = -(tmp7*tmp9); +double tmp100 = tmp8*tmp10; +double tmp101 = tmp99 + tmp100; +double tmp102 = 2*tmp101*tmp30; +double tmp103 = tmp95 + tmp98 + tmp102; +double tmp104 = mPitch(1+_OFF); +double tmp105 = -tmp104; +double tmp106 = std::atan2(tmp77,tmp60); +double tmp107 = mPitch(3+_OFF); +double tmp108 = tmp106*tmp107; +double tmp109 = 1/std::sqrt(tmp79); +double tmp110 = (tmp80*tmp81*tmp109*tmp82)/2.; +double tmp111 = mPitch(2+_OFF); +double tmp112 = z(3+_OFF); +double tmp113 = -(tmp111*tmp112); +double tmp114 = tmp105 + tmp108 + tmp110 + tmp113; +double tmp115 = -(tmp7*tmp10); +double tmp116 = tmp46 + tmp115; +double tmp117 = 4*tmp116*tmp23; +double tmp118 = tmp7*tmp9; +double tmp119 = tmp118 + tmp100; +double tmp120 = 4*tmp119*tmp27; +double tmp121 = tmp34 + tmp96 + tmp36 + tmp39; +double tmp122 = tmp121*tmp30; +double tmp123 = tmp117 + tmp120 + tmp122; +double tmp124 = -(tmp1*tmp3); +double tmp125 = tmp2*tmp4; +double tmp126 = tmp124 + tmp125; +double tmp127 = 0.317*tmp80*tmp33*tmp81*tmp50*tmp92*tmp82; +double tmp128 = 0.634*tmp33*tmp50*tmp79*tmp114; +double tmp129 = tmp127 + tmp128; +double tmp130 = 0.317*tmp80*tmp33*tmp81*tmp103*tmp92*tmp82; +double tmp131 = 0.634*tmp33*tmp103*tmp79*tmp114; +double tmp132 = tmp130 + tmp131; +double tmp133 = 0.317*tmp80*tmp33*tmp81*tmp123*tmp92*tmp82; +double tmp134 = 0.634*tmp33*tmp123*tmp79*tmp114; +double tmp135 = tmp133 + tmp134; +double tmp136 = -(tmp0*tmp2); +double tmp137 = tmp3*tmp4; +double tmp138 = tmp136 + tmp137; +double tmp139 = tmp2*tmp3; +double tmp140 = -(tmp5*tmp4); +double tmp141 = tmp139 + tmp140; J(1+_OFF,1+_OFF) = 0; J(2+_OFF,1+_OFF) = 0; J(3+_OFF,1+_OFF) = 0; -J(4+_OFF,1+_OFF) = -((-(tmp0*tmp3) + tmp2*tmp4)*tmp70*tmp90); -J(5+_OFF,1+_OFF) = -((tmp5*tmp0 - tmp61)*tmp70*tmp90); -J(6+_OFF,1+_OFF) = -((tmp2*tmp3 - tmp5*tmp4)*tmp70*tmp90); +J(4+_OFF,1+_OFF) = -((tmp0*tmp1 - tmp6)*tmp91*tmp129) - tmp138*tmp91*tmp132 - tmp126*tmp91*tmp135; +J(5+_OFF,1+_OFF) = -(tmp126*tmp91*tmp129) - tmp141*tmp91*tmp132 - (tmp5*tmp1 - tmp84)*tmp91*tmp135; +J(6+_OFF,1+_OFF) = -(tmp138*tmp91*tmp129) - (tmp5*tmp0 - tmp86)*tmp91*tmp132 - tmp141*tmp91*tmp135; diff --git a/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrCp.cppready b/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrCp.cppready index 158d11b..8ca3dd2 100644 --- a/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrCp.cppready +++ b/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrCp.cppready @@ -2,242 +2,309 @@ double tmp0 = x(4+_OFF); double tmp1 = x(6+_OFF); double tmp2 = x(5+_OFF); double tmp3 = x(7+_OFF); -double tmp4 = wind(1+_OFF); -double tmp5 = tmp2*tmp1; -double tmp6 = wind(3+_OFF); -double tmp7 = wind(2+_OFF); -double tmp8 = std::pow(tmp0,2); -double tmp9 = std::pow(tmp2,2); -double tmp10 = std::pow(tmp1,2); -double tmp11 = std::pow(tmp3,2); -double tmp12 = -tmp11; -double tmp13 = tmp2*tmp3; -double tmp14 = tmp1*tmp3; -double tmp15 = -tmp9; -double tmp16 = -tmp10; -double tmp17 = cp(1+_OFF); -double tmp18 = z(1+_OFF); -double tmp19 = tmp0*tmp3; -double tmp20 = tmp5 + tmp19; -double tmp21 = -2*tmp7*tmp20; -double tmp22 = -(tmp0*tmp1); -double tmp23 = tmp22 + tmp13; -double tmp24 = -2*tmp6*tmp23; -double tmp25 = tmp8 + tmp9 + tmp16 + tmp12; -double tmp26 = -(tmp4*tmp25); -double tmp27 = x(8+_OFF); -double tmp28 = tmp21 + tmp24 + tmp26 + tmp27; -double tmp29 = std::pow(tmp28,2); -double tmp30 = -(tmp0*tmp3); -double tmp31 = tmp5 + tmp30; -double tmp32 = -2*tmp4*tmp31; -double tmp33 = tmp0*tmp2; -double tmp34 = tmp33 + tmp14; -double tmp35 = -2*tmp6*tmp34; -double tmp36 = tmp8 + tmp15 + tmp10 + tmp12; -double tmp37 = -(tmp7*tmp36); -double tmp38 = x(9+_OFF); -double tmp39 = tmp32 + tmp35 + tmp37 + tmp38; -double tmp40 = std::pow(tmp39,2); -double tmp41 = tmp0*tmp1; -double tmp42 = tmp41 + tmp13; -double tmp43 = -2*tmp4*tmp42; -double tmp44 = -(tmp0*tmp2); -double tmp45 = tmp44 + tmp14; -double tmp46 = -2*tmp7*tmp45; -double tmp47 = tmp8 + tmp15 + tmp16 + tmp11; -double tmp48 = -(tmp6*tmp47); -double tmp49 = x(10+_OFF); -double tmp50 = tmp43 + tmp46 + tmp48 + tmp49; -double tmp51 = std::pow(tmp50,2); -double tmp52 = tmp29 + tmp40 + tmp51; -double tmp53 = 1/M_PI; -double tmp54 = fThrust(2+_OFF); -double tmp55 = std::sqrt(tmp52); -double tmp56 = std::pow(tmp17,2); -double tmp57 = fThrust(1+_OFF); -double tmp58 = std::pow(tmp18,2); -double tmp59 = cp(4+_OFF); -double tmp60 = 1/tmp59; -double tmp61 = 2*tmp4*tmp42; -double tmp62 = 2*tmp7*tmp45; -double tmp63 = tmp6*tmp47; -double tmp64 = -tmp49; -double tmp65 = tmp61 + tmp62 + tmp63 + tmp64; -double tmp66 = std::pow(tmp65,2); -double tmp67 = tmp29 + tmp66; -double tmp68 = 1/std::sqrt(tmp67); -double tmp69 = std::atan2(tmp65,tmp28); -double tmp70 = 1/std::sqrt(tmp52); -double tmp71 = tmp39*tmp70; -double tmp72 = std::asin(tmp71); -double tmp73 = -(tmp4*tmp8); -double tmp74 = -(tmp4*tmp9); -double tmp75 = 2*tmp6*tmp0*tmp1; -double tmp76 = -2*tmp7*tmp2*tmp1; -double tmp77 = tmp4*tmp10; -double tmp78 = -2*tmp7*tmp0*tmp3; -double tmp79 = -2*tmp6*tmp2*tmp3; -double tmp80 = tmp4*tmp11; -double tmp81 = tmp73 + tmp74 + tmp75 + tmp76 + tmp77 + tmp78 + tmp79 + tmp80 + tmp27; -double tmp82 = fLat(1+_OFF); -double tmp83 = tmp7*tmp8; -double tmp84 = 2*tmp6*tmp0*tmp2; -double tmp85 = -(tmp7*tmp9); -double tmp86 = 2*tmp4*tmp2*tmp1; -double tmp87 = tmp7*tmp10; -double tmp88 = -2*tmp4*tmp0*tmp3; -double tmp89 = 2*tmp6*tmp1*tmp3; -double tmp90 = -(tmp7*tmp11); -double tmp91 = -tmp38; -double tmp92 = tmp83 + tmp84 + tmp85 + tmp86 + tmp87 + tmp88 + tmp89 + tmp90 + tmp91; -double tmp93 = cp(2+_OFF); -double tmp94 = fLift(1+_OFF); -double tmp95 = fLift(2+_OFF); -double tmp96 = tmp69*tmp95; -double tmp97 = tmp94 + tmp96; -double tmp98 = tmp6*tmp8; -double tmp99 = -2*tmp7*tmp0*tmp2; -double tmp100 = -(tmp6*tmp9); -double tmp101 = 2*tmp4*tmp0*tmp1; -double tmp102 = -(tmp6*tmp10); -double tmp103 = 2*tmp4*tmp2*tmp3; -double tmp104 = 2*tmp7*tmp1*tmp3; -double tmp105 = tmp6*tmp11; -double tmp106 = tmp98 + tmp99 + tmp100 + tmp101 + tmp102 + tmp103 + tmp104 + tmp105 + tmp64; -double tmp107 = fDrag(1+_OFF); -double tmp108 = fDrag(2+_OFF); -double tmp109 = tmp69*tmp108; -double tmp110 = std::pow(tmp69,2); -double tmp111 = fDrag(3+_OFF); -double tmp112 = tmp110*tmp111; -double tmp113 = std::pow(tmp72,2); -double tmp114 = fDrag(4+_OFF); -double tmp115 = tmp113*tmp114; -double tmp116 = tmp107 + tmp109 + tmp112 + tmp115; -double tmp117 = 1/tmp52; -double tmp118 = -(tmp40*tmp117); -double tmp119 = 1 + tmp118; -double tmp120 = std::sqrt(tmp119); -double tmp121 = std::pow(M_PI,-2); -double tmp122 = fThrust(3+_OFF); -double tmp123 = tmp121*tmp122*tmp52; -double tmp124 = tmp53*tmp17*tmp54*tmp55*tmp18; -double tmp125 = tmp56*tmp57*tmp58; -double tmp126 = tmp123 + tmp124 + tmp125; -double tmp127 = std::pow(tmp59,-2); -double tmp128 = -(tmp7*tmp8); -double tmp129 = -2*tmp6*tmp0*tmp2; -double tmp130 = tmp7*tmp9; -double tmp131 = -2*tmp4*tmp2*tmp1; -double tmp132 = -(tmp7*tmp10); -double tmp133 = 2*tmp4*tmp0*tmp3; -double tmp134 = -2*tmp6*tmp1*tmp3; -double tmp135 = tmp7*tmp11; -double tmp136 = tmp128 + tmp129 + tmp130 + tmp131 + tmp132 + tmp133 + tmp134 + tmp135 + tmp38; -double tmp137 = -(tmp6*tmp8); -double tmp138 = 2*tmp7*tmp0*tmp2; -double tmp139 = tmp6*tmp9; -double tmp140 = -2*tmp4*tmp0*tmp1; -double tmp141 = tmp6*tmp10; -double tmp142 = -2*tmp4*tmp2*tmp3; -double tmp143 = -2*tmp7*tmp1*tmp3; -double tmp144 = -(tmp6*tmp11); -double tmp145 = tmp137 + tmp138 + tmp139 + tmp140 + tmp141 + tmp142 + tmp143 + tmp144 + tmp49; -double tmp146 = ibd(2+_OFF); -double tmp147 = ibd(3+_OFF); -double tmp148 = ibod(1+_OFF); -double tmp149 = ibod(2+_OFF); -double tmp150 = ibod(3+_OFF); -double tmp151 = ibd(1+_OFF); -double tmp152 = std::pow(tmp150,2); -double tmp153 = cp(3+_OFF); -double tmp154 = tmp151*tmp146*tmp147; -double tmp155 = std::pow(tmp148,2); -double tmp156 = -(tmp146*tmp155); -double tmp157 = std::pow(tmp149,2); -double tmp158 = -(tmp147*tmp157); -double tmp159 = 2*tmp148*tmp149*tmp150; -double tmp160 = -(tmp151*tmp152); -double tmp161 = tmp154 + tmp156 + tmp158 + tmp159 + tmp160; -double tmp162 = 1/tmp161; -double tmp163 = cBar(1+_OFF); -double tmp164 = x(13+_OFF); -double tmp165 = tmp146*tmp147; -double tmp166 = -tmp152; -double tmp167 = tmp165 + tmp166; -double tmp168 = mRoll(3+_OFF); -double tmp169 = x(11+_OFF); -double tmp170 = mRoll(4+_OFF); -double tmp171 = mRoll(2+_OFF); -double tmp172 = -(tmp72*tmp171); -double tmp173 = (tmp153*tmp168*tmp70*tmp169)/2.; -double tmp174 = -(tmp153*tmp170*tmp70*tmp164)/2.; -double tmp175 = mRoll(1+_OFF); -double tmp176 = z(2+_OFF); -double tmp177 = -(tmp175*tmp176); -double tmp178 = tmp172 + tmp173 + tmp174 + tmp177; -double tmp179 = -(tmp146*tmp148); -double tmp180 = tmp149*tmp150; -double tmp181 = tmp179 + tmp180; -double tmp182 = mYaw(3+_OFF); -double tmp183 = mYaw(2+_OFF); -double tmp184 = tmp72*tmp183; -double tmp185 = (tmp153*tmp182*tmp70*tmp164)/2.; -double tmp186 = mYaw(1+_OFF); -double tmp187 = z(4+_OFF); -double tmp188 = tmp186*tmp187; -double tmp189 = tmp184 + tmp185 + tmp188; -double tmp190 = -(tmp147*tmp149); -double tmp191 = tmp148*tmp150; -double tmp192 = tmp190 + tmp191; -double tmp193 = mPitch(1+_OFF); -double tmp194 = -tmp193; -double tmp195 = mPitch(3+_OFF); -double tmp196 = -(tmp69*tmp195); -double tmp197 = mPitch(4+_OFF); -double tmp198 = x(12+_OFF); -double tmp199 = (tmp163*tmp197*tmp70*tmp198)/2.; -double tmp200 = mPitch(2+_OFF); -double tmp201 = z(3+_OFF); -double tmp202 = -(tmp200*tmp201); -double tmp203 = tmp194 + tmp196 + tmp199 + tmp202; -double tmp204 = (tmp168*tmp70*tmp169)/2.; -double tmp205 = -(tmp170*tmp70*tmp164)/2.; -double tmp206 = tmp204 + tmp205; -double tmp207 = 0.5*tmp93*tmp153*tmp52*tmp206; -double tmp208 = 0.5*tmp93*tmp52*tmp178; -double tmp209 = tmp207 + tmp208; -double tmp210 = tmp148*tmp149; -double tmp211 = -(tmp151*tmp150); -double tmp212 = tmp210 + tmp211; -double tmp213 = 0.25*tmp93*tmp153*tmp182*tmp55*tmp164; -double tmp214 = 0.5*tmp93*tmp52*tmp189; -double tmp215 = tmp213 + tmp214; -double tmp216 = tmp151*tmp146; -double tmp217 = -tmp157; -double tmp218 = tmp216 + tmp217; -J(1+_OFF,1+_OFF) = -(tmp60*(1.*tmp56*(tmp53*tmp54*tmp55*tmp18 + 2*tmp17*tmp57*tmp58) + 2.*tmp17*tmp126)); -J(1+_OFF,2+_OFF) = -(tmp60*(0.5*tmp72*tmp82*tmp81*tmp92*tmp68*tmp55 - 0.5*tmp97*tmp68*tmp106*tmp52 + 0.5*tmp116*tmp81*tmp68*tmp52*tmp120)); +double tmp4 = x(30+_OFF); +double tmp5 = x(32+_OFF); +double tmp6 = x(31+_OFF); +double tmp7 = x(33+_OFF); +double tmp8 = tmp6*tmp5; +double tmp9 = tmp4*tmp7; +double tmp10 = tmp8 + tmp9; +double tmp11 = -(tmp4*tmp5); +double tmp12 = tmp6*tmp7; +double tmp13 = tmp11 + tmp12; +double tmp14 = std::pow(tmp4,2); +double tmp15 = std::pow(tmp6,2); +double tmp16 = std::pow(tmp5,2); +double tmp17 = -tmp16; +double tmp18 = std::pow(tmp7,2); +double tmp19 = -tmp18; +double tmp20 = tmp14 + tmp15 + tmp17 + tmp19; +double tmp21 = wind(1+_OFF); +double tmp22 = wind(3+_OFF); +double tmp23 = wind(2+_OFF); +double tmp24 = tmp5*tmp7; +double tmp25 = -tmp15; +double tmp26 = cp(1+_OFF); +double tmp27 = z(1+_OFF); +double tmp28 = -2*tmp23*tmp10; +double tmp29 = -2*tmp22*tmp13; +double tmp30 = -(tmp21*tmp20); +double tmp31 = x(34+_OFF); +double tmp32 = tmp28 + tmp29 + tmp30 + tmp31; +double tmp33 = std::pow(tmp32,2); +double tmp34 = -(tmp4*tmp7); +double tmp35 = tmp8 + tmp34; +double tmp36 = -2*tmp21*tmp35; +double tmp37 = tmp4*tmp6; +double tmp38 = tmp37 + tmp24; +double tmp39 = -2*tmp22*tmp38; +double tmp40 = tmp14 + tmp25 + tmp16 + tmp19; +double tmp41 = -(tmp23*tmp40); +double tmp42 = x(35+_OFF); +double tmp43 = tmp36 + tmp39 + tmp41 + tmp42; +double tmp44 = std::pow(tmp43,2); +double tmp45 = tmp4*tmp5; +double tmp46 = tmp45 + tmp12; +double tmp47 = -2*tmp21*tmp46; +double tmp48 = -(tmp4*tmp6); +double tmp49 = tmp48 + tmp24; +double tmp50 = -2*tmp23*tmp49; +double tmp51 = tmp14 + tmp25 + tmp17 + tmp18; +double tmp52 = -(tmp22*tmp51); +double tmp53 = x(36+_OFF); +double tmp54 = tmp47 + tmp50 + tmp52 + tmp53; +double tmp55 = std::pow(tmp54,2); +double tmp56 = tmp33 + tmp44 + tmp55; +double tmp57 = 1/M_PI; +double tmp58 = fThrust(2+_OFF); +double tmp59 = std::sqrt(tmp56); +double tmp60 = std::pow(tmp26,2); +double tmp61 = fThrust(1+_OFF); +double tmp62 = std::pow(tmp27,2); +double tmp63 = cp(4+_OFF); +double tmp64 = 1/tmp63; +double tmp65 = std::pow(tmp0,2); +double tmp66 = std::pow(tmp2,2); +double tmp67 = std::pow(tmp1,2); +double tmp68 = -tmp67; +double tmp69 = std::pow(tmp3,2); +double tmp70 = -tmp69; +double tmp71 = tmp65 + tmp66 + tmp68 + tmp70; +double tmp72 = -(tmp0*tmp1); +double tmp73 = tmp2*tmp3; +double tmp74 = tmp72 + tmp73; +double tmp75 = tmp2*tmp1; +double tmp76 = tmp0*tmp3; +double tmp77 = tmp75 + tmp76; +double tmp78 = std::atan2(tmp54,tmp32); +double tmp79 = 1/std::sqrt(tmp56); +double tmp80 = tmp43*tmp79; +double tmp81 = std::asin(tmp80); +double tmp82 = 4*tmp77*tmp10; +double tmp83 = 4*tmp74*tmp13; +double tmp84 = tmp71*tmp20; +double tmp85 = tmp82 + tmp83 + tmp84; +double tmp86 = fLat(1+_OFF); +double tmp87 = tmp33 + tmp55; +double tmp88 = 1/std::sqrt(tmp87); +double tmp89 = fDrag(1+_OFF); +double tmp90 = fDrag(2+_OFF); +double tmp91 = -(tmp78*tmp90); +double tmp92 = std::pow(tmp78,2); +double tmp93 = fDrag(3+_OFF); +double tmp94 = tmp92*tmp93; +double tmp95 = std::pow(tmp81,2); +double tmp96 = fDrag(4+_OFF); +double tmp97 = tmp95*tmp96; +double tmp98 = tmp89 + tmp91 + tmp94 + tmp97; +double tmp99 = 1/tmp56; +double tmp100 = -(tmp44*tmp99); +double tmp101 = 1 + tmp100; +double tmp102 = std::sqrt(tmp101); +double tmp103 = fLift(1+_OFF); +double tmp104 = -tmp103; +double tmp105 = fLift(2+_OFF); +double tmp106 = tmp78*tmp105; +double tmp107 = tmp104 + tmp106; +double tmp108 = 2*tmp71*tmp35; +double tmp109 = 4*tmp74*tmp38; +double tmp110 = 2*tmp77*tmp40; +double tmp111 = tmp108 + tmp109 + tmp110; +double tmp112 = cp(2+_OFF); +double tmp113 = 2*tmp71*tmp46; +double tmp114 = 4*tmp77*tmp49; +double tmp115 = 2*tmp74*tmp51; +double tmp116 = tmp113 + tmp114 + tmp115; +double tmp117 = std::pow(M_PI,-2); +double tmp118 = fThrust(3+_OFF); +double tmp119 = tmp117*tmp118*tmp56; +double tmp120 = tmp57*tmp26*tmp58*tmp59*tmp27; +double tmp121 = tmp60*tmp61*tmp62; +double tmp122 = tmp119 + tmp120 + tmp121; +double tmp123 = tmp57*tmp58*tmp59*tmp27; +double tmp124 = 2*tmp26*tmp61*tmp62; +double tmp125 = tmp123 + tmp124; +double tmp126 = 1.268*tmp60*tmp125; +double tmp127 = 2.536*tmp26*tmp122; +double tmp128 = tmp126 + tmp127; +double tmp129 = -(tmp0*tmp3); +double tmp130 = tmp75 + tmp129; +double tmp131 = tmp0*tmp2; +double tmp132 = tmp1*tmp3; +double tmp133 = tmp131 + tmp132; +double tmp134 = -tmp66; +double tmp135 = tmp65 + tmp134 + tmp67 + tmp70; +double tmp136 = 0.634*tmp98*tmp43*tmp59; +double tmp137 = 0.634*tmp81*tmp86*tmp56*tmp102; +double tmp138 = tmp136 + tmp137; +double tmp139 = 2*tmp135*tmp10; +double tmp140 = 4*tmp133*tmp13; +double tmp141 = 2*tmp130*tmp20; +double tmp142 = tmp139 + tmp140 + tmp141; +double tmp143 = -0.634*tmp81*tmp86*tmp32*tmp43*tmp88*tmp59; +double tmp144 = -0.634*tmp107*tmp54*tmp88*tmp56; +double tmp145 = 0.634*tmp98*tmp32*tmp88*tmp56*tmp102; +double tmp146 = tmp143 + tmp144 + tmp145; +double tmp147 = -0.634*tmp81*tmp86*tmp43*tmp54*tmp88*tmp59; +double tmp148 = 0.634*tmp107*tmp32*tmp88*tmp56; +double tmp149 = 0.634*tmp98*tmp54*tmp88*tmp56*tmp102; +double tmp150 = tmp147 + tmp148 + tmp149; +double tmp151 = std::pow(tmp63,-2); +double tmp152 = 4*tmp130*tmp35; +double tmp153 = 4*tmp133*tmp38; +double tmp154 = tmp135*tmp40; +double tmp155 = tmp152 + tmp153 + tmp154; +double tmp156 = 0.634*tmp112*tmp98*tmp43*tmp59; +double tmp157 = 0.634*tmp81*tmp112*tmp86*tmp56*tmp102; +double tmp158 = 0. + tmp156 + tmp157; +double tmp159 = 4*tmp130*tmp46; +double tmp160 = 2*tmp135*tmp49; +double tmp161 = 2*tmp133*tmp51; +double tmp162 = tmp159 + tmp160 + tmp161; +double tmp163 = -0.634*tmp81*tmp112*tmp86*tmp43*tmp54*tmp88*tmp59; +double tmp164 = 0.634*tmp112*tmp107*tmp32*tmp88*tmp56; +double tmp165 = 0.634*tmp112*tmp98*tmp54*tmp88*tmp56*tmp102; +double tmp166 = tmp163 + tmp164 + tmp165; +double tmp167 = -0.634*tmp81*tmp112*tmp86*tmp32*tmp43*tmp88*tmp59; +double tmp168 = -0.634*tmp112*tmp107*tmp54*tmp88*tmp56; +double tmp169 = 0.634*tmp112*tmp98*tmp32*tmp88*tmp56*tmp102; +double tmp170 = 1.268*tmp60*tmp122; +double tmp171 = tmp167 + tmp168 + tmp169 + tmp170; +double tmp172 = tmp0*tmp1; +double tmp173 = tmp172 + tmp73; +double tmp174 = tmp65 + tmp134 + tmp68 + tmp69; +double tmp175 = -(tmp0*tmp2); +double tmp176 = tmp175 + tmp132; +double tmp177 = 4*tmp176*tmp10; +double tmp178 = 2*tmp174*tmp13; +double tmp179 = 2*tmp173*tmp20; +double tmp180 = tmp177 + tmp178 + tmp179; +double tmp181 = 4*tmp173*tmp35; +double tmp182 = 2*tmp174*tmp38; +double tmp183 = 2*tmp176*tmp40; +double tmp184 = tmp181 + tmp182 + tmp183; +double tmp185 = 4*tmp173*tmp46; +double tmp186 = 4*tmp176*tmp49; +double tmp187 = tmp174*tmp51; +double tmp188 = tmp185 + tmp186 + tmp187; +double tmp189 = ibd(2+_OFF); +double tmp190 = ibd(3+_OFF); +double tmp191 = ibod(1+_OFF); +double tmp192 = ibod(2+_OFF); +double tmp193 = ibod(3+_OFF); +double tmp194 = ibd(1+_OFF); +double tmp195 = std::pow(tmp193,2); +double tmp196 = cp(3+_OFF); +double tmp197 = cBar(1+_OFF); +double tmp198 = x(39+_OFF); +double tmp199 = tmp194*tmp189*tmp190; +double tmp200 = std::pow(tmp191,2); +double tmp201 = -(tmp189*tmp200); +double tmp202 = std::pow(tmp192,2); +double tmp203 = -(tmp190*tmp202); +double tmp204 = 2*tmp191*tmp192*tmp193; +double tmp205 = -(tmp194*tmp195); +double tmp206 = tmp199 + tmp201 + tmp203 + tmp204 + tmp205; +double tmp207 = 1/tmp206; +double tmp208 = mRoll(2+_OFF); +double tmp209 = -(tmp81*tmp208); +double tmp210 = mRoll(3+_OFF); +double tmp211 = x(37+_OFF); +double tmp212 = (tmp196*tmp210*tmp79*tmp211)/2.; +double tmp213 = mRoll(4+_OFF); +double tmp214 = -0.5*(tmp196*tmp213*tmp79*tmp198); +double tmp215 = mRoll(1+_OFF); +double tmp216 = z(2+_OFF); +double tmp217 = tmp215*tmp216; +double tmp218 = tmp209 + tmp212 + tmp214 + tmp217; +double tmp219 = mPitch(1+_OFF); +double tmp220 = -tmp219; +double tmp221 = mPitch(3+_OFF); +double tmp222 = tmp78*tmp221; +double tmp223 = mPitch(4+_OFF); +double tmp224 = x(38+_OFF); +double tmp225 = (tmp197*tmp223*tmp79*tmp224)/2.; +double tmp226 = mPitch(2+_OFF); +double tmp227 = z(3+_OFF); +double tmp228 = -(tmp226*tmp227); +double tmp229 = tmp220 + tmp222 + tmp225 + tmp228; +double tmp230 = mYaw(2+_OFF); +double tmp231 = tmp81*tmp230; +double tmp232 = mYaw(3+_OFF); +double tmp233 = (tmp196*tmp232*tmp79*tmp198)/2.; +double tmp234 = mYaw(1+_OFF); +double tmp235 = z(4+_OFF); +double tmp236 = -(tmp234*tmp235); +double tmp237 = tmp231 + tmp233 + tmp236; +double tmp238 = tmp189*tmp190; +double tmp239 = -tmp195; +double tmp240 = tmp238 + tmp239; +double tmp241 = -(tmp190*tmp192); +double tmp242 = tmp191*tmp193; +double tmp243 = tmp241 + tmp242; +double tmp244 = (tmp210*tmp79*tmp211)/2.; +double tmp245 = -0.5*(tmp213*tmp79*tmp198); +double tmp246 = tmp244 + tmp245; +double tmp247 = -(tmp189*tmp191); +double tmp248 = tmp192*tmp193; +double tmp249 = tmp247 + tmp248; +double tmp250 = 0.634*tmp196*tmp85*tmp56*tmp218; +double tmp251 = 0.634*tmp197*tmp111*tmp56*tmp229; +double tmp252 = 0.634*tmp196*tmp116*tmp56*tmp237; +double tmp253 = tmp250 + tmp251 + tmp252; +double tmp254 = 0.634*tmp196*tmp142*tmp56*tmp218; +double tmp255 = 0.634*tmp197*tmp155*tmp56*tmp229; +double tmp256 = 0.634*tmp196*tmp162*tmp56*tmp237; +double tmp257 = tmp254 + tmp255 + tmp256; +double tmp258 = 0.634*tmp196*tmp180*tmp56*tmp218; +double tmp259 = 0.634*tmp197*tmp184*tmp56*tmp229; +double tmp260 = 0.634*tmp196*tmp188*tmp56*tmp237; +double tmp261 = tmp258 + tmp259 + tmp260; +double tmp262 = 0.317*tmp112*tmp196*tmp232*tmp116*tmp59*tmp198; +double tmp263 = 0.634*tmp112*tmp196*tmp85*tmp56*tmp246; +double tmp264 = 0.634*tmp112*tmp85*tmp56*tmp218; +double tmp265 = 0.634*tmp112*tmp116*tmp56*tmp237; +double tmp266 = tmp262 + tmp263 + tmp264 + tmp265; +double tmp267 = tmp194*tmp190; +double tmp268 = -tmp200; +double tmp269 = tmp267 + tmp268; +double tmp270 = 0.317*tmp112*tmp196*tmp232*tmp162*tmp59*tmp198; +double tmp271 = 0.634*tmp112*tmp196*tmp142*tmp56*tmp246; +double tmp272 = 0.634*tmp112*tmp142*tmp56*tmp218; +double tmp273 = 0.634*tmp112*tmp162*tmp56*tmp237; +double tmp274 = tmp270 + tmp271 + tmp272 + tmp273; +double tmp275 = tmp191*tmp192; +double tmp276 = -(tmp194*tmp193); +double tmp277 = tmp275 + tmp276; +double tmp278 = 0.317*tmp112*tmp196*tmp232*tmp188*tmp59*tmp198; +double tmp279 = 0.634*tmp112*tmp196*tmp180*tmp56*tmp246; +double tmp280 = 0.634*tmp112*tmp180*tmp56*tmp218; +double tmp281 = 0.634*tmp112*tmp188*tmp56*tmp237; +double tmp282 = tmp278 + tmp279 + tmp280 + tmp281; +double tmp283 = tmp194*tmp189; +double tmp284 = -tmp202; +double tmp285 = tmp283 + tmp284; +J(1+_OFF,1+_OFF) = -(tmp64*tmp85*tmp128); +J(1+_OFF,2+_OFF) = -(tmp64*(tmp111*tmp138 + tmp85*tmp146 + tmp116*tmp150)); J(1+_OFF,3+_OFF) = 0; -J(1+_OFF,4+_OFF) = tmp127*(0.5*tmp72*tmp93*tmp82*tmp81*tmp92*tmp68*tmp55 - 0.5*tmp93*tmp97*tmp68*tmp106*tmp52 + 0.5*tmp93*tmp116*tmp81*tmp68*tmp52*tmp120 + 1.*tmp56*tmp126); -J(2+_OFF,1+_OFF) = 0; -J(2+_OFF,2+_OFF) = -(tmp60*(0.5*tmp116*tmp136*tmp55 + 0.5*tmp72*tmp82*tmp52*tmp120)); +J(1+_OFF,4+_OFF) = tmp151*(tmp111*tmp158 + tmp116*tmp166 + tmp85*tmp171); +J(2+_OFF,1+_OFF) = -(tmp64*tmp142*tmp128); +J(2+_OFF,2+_OFF) = -(tmp64*(tmp155*tmp138 + tmp142*tmp146 + tmp162*tmp150)); J(2+_OFF,3+_OFF) = 0; -J(2+_OFF,4+_OFF) = tmp127*(0. + 0.5*tmp93*tmp116*tmp136*tmp55 + 0.5*tmp72*tmp93*tmp82*tmp52*tmp120); -J(3+_OFF,1+_OFF) = 0; -J(3+_OFF,2+_OFF) = -(tmp60*(0.5*tmp72*tmp82*tmp92*tmp68*tmp145*tmp55 - 0.5*tmp97*tmp81*tmp68*tmp52 + 0.5*tmp116*tmp68*tmp145*tmp52*tmp120)); +J(2+_OFF,4+_OFF) = tmp151*(tmp155*tmp158 + tmp162*tmp166 + tmp142*tmp171); +J(3+_OFF,1+_OFF) = -(tmp64*tmp180*tmp128); +J(3+_OFF,2+_OFF) = -(tmp64*(tmp184*tmp138 + tmp180*tmp146 + tmp188*tmp150)); J(3+_OFF,3+_OFF) = 0; -J(3+_OFF,4+_OFF) = tmp127*(0.5*tmp72*tmp93*tmp82*tmp92*tmp68*tmp145*tmp55 - 0.5*tmp93*tmp97*tmp81*tmp68*tmp52 + 0.5*tmp93*tmp116*tmp68*tmp145*tmp52*tmp120); +J(3+_OFF,4+_OFF) = tmp151*(tmp184*tmp158 + tmp188*tmp166 + tmp180*tmp171); J(4+_OFF,1+_OFF) = 0; -J(4+_OFF,2+_OFF) = -0.5*tmp153*tmp167*tmp162*tmp52*tmp178 - 0.5*tmp163*tmp192*tmp162*tmp52*tmp203 - 0.5*tmp153*tmp181*tmp162*tmp52*tmp189; -J(4+_OFF,3+_OFF) = -(tmp167*tmp162*tmp209) - tmp181*tmp162*tmp215; +J(4+_OFF,2+_OFF) = -(tmp240*tmp207*tmp253) - tmp243*tmp207*tmp257 - tmp249*tmp207*tmp261; +J(4+_OFF,3+_OFF) = -(tmp240*tmp207*tmp266) - tmp243*tmp207*tmp274 - tmp249*tmp207*tmp282; J(4+_OFF,4+_OFF) = 0; J(5+_OFF,1+_OFF) = 0; -J(5+_OFF,2+_OFF) = -0.5*tmp153*tmp192*tmp162*tmp52*tmp178 - 0.5*tmp163*(tmp151*tmp147 - tmp155)*tmp162*tmp52*tmp203 - 0.5*tmp153*tmp212*tmp162*tmp52*tmp189; -J(5+_OFF,3+_OFF) = -(tmp192*tmp162*tmp209) - tmp212*tmp162*tmp215; +J(5+_OFF,2+_OFF) = -(tmp243*tmp207*tmp253) - tmp269*tmp207*tmp257 - tmp277*tmp207*tmp261; +J(5+_OFF,3+_OFF) = -(tmp243*tmp207*tmp266) - tmp269*tmp207*tmp274 - tmp277*tmp207*tmp282; J(5+_OFF,4+_OFF) = 0; J(6+_OFF,1+_OFF) = 0; -J(6+_OFF,2+_OFF) = -0.5*tmp153*tmp181*tmp162*tmp52*tmp178 - 0.5*tmp163*tmp212*tmp162*tmp52*tmp203 - 0.5*tmp153*tmp218*tmp162*tmp52*tmp189; -J(6+_OFF,3+_OFF) = -(tmp181*tmp162*tmp209) - tmp218*tmp162*tmp215; +J(6+_OFF,2+_OFF) = -(tmp249*tmp207*tmp253) - tmp277*tmp207*tmp257 - tmp285*tmp207*tmp261; +J(6+_OFF,3+_OFF) = -(tmp249*tmp207*tmp266) - tmp277*tmp207*tmp274 - tmp285*tmp207*tmp282; J(6+_OFF,4+_OFF) = 0; diff --git a/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrFDrag.cppready b/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrFDrag.cppready index 5424096..e90eedb 100644 --- a/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrFDrag.cppready +++ b/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrFDrag.cppready @@ -1,115 +1,147 @@ -double tmp0 = wind(1+_OFF); -double tmp1 = x(4+_OFF); +double tmp0 = x(4+_OFF); +double tmp1 = x(6+_OFF); double tmp2 = x(5+_OFF); -double tmp3 = x(6+_OFF); -double tmp4 = wind(2+_OFF); -double tmp5 = wind(3+_OFF); -double tmp6 = x(7+_OFF); -double tmp7 = std::pow(tmp1,2); -double tmp8 = std::pow(tmp2,2); -double tmp9 = std::pow(tmp3,2); -double tmp10 = std::pow(tmp6,2); -double tmp11 = x(8+_OFF); -double tmp12 = tmp2*tmp6; -double tmp13 = -tmp9; -double tmp14 = tmp2*tmp3; -double tmp15 = tmp1*tmp6; -double tmp16 = tmp14 + tmp15; -double tmp17 = -2*tmp4*tmp16; -double tmp18 = -(tmp1*tmp3); -double tmp19 = tmp18 + tmp12; -double tmp20 = -2*tmp5*tmp19; -double tmp21 = -tmp10; -double tmp22 = tmp7 + tmp8 + tmp13 + tmp21; -double tmp23 = -(tmp0*tmp22); -double tmp24 = tmp17 + tmp20 + tmp23 + tmp11; -double tmp25 = std::pow(tmp24,2); -double tmp26 = tmp3*tmp6; -double tmp27 = -tmp8; -double tmp28 = tmp1*tmp3; -double tmp29 = tmp28 + tmp12; -double tmp30 = -(tmp1*tmp2); -double tmp31 = tmp30 + tmp26; -double tmp32 = tmp7 + tmp27 + tmp13 + tmp10; -double tmp33 = x(10+_OFF); -double tmp34 = -(tmp1*tmp6); -double tmp35 = tmp14 + tmp34; -double tmp36 = -2*tmp0*tmp35; -double tmp37 = tmp1*tmp2; -double tmp38 = tmp37 + tmp26; -double tmp39 = -2*tmp5*tmp38; -double tmp40 = tmp7 + tmp27 + tmp9 + tmp21; -double tmp41 = -(tmp4*tmp40); -double tmp42 = x(9+_OFF); -double tmp43 = tmp36 + tmp39 + tmp41 + tmp42; -double tmp44 = std::pow(tmp43,2); -double tmp45 = -2*tmp0*tmp29; -double tmp46 = -2*tmp4*tmp31; -double tmp47 = -(tmp5*tmp32); -double tmp48 = tmp45 + tmp46 + tmp47 + tmp33; -double tmp49 = std::pow(tmp48,2); -double tmp50 = tmp25 + tmp44 + tmp49; -double tmp51 = 2*tmp0*tmp29; -double tmp52 = 2*tmp4*tmp31; -double tmp53 = tmp5*tmp32; -double tmp54 = -tmp33; -double tmp55 = tmp51 + tmp52 + tmp53 + tmp54; -double tmp56 = cp(2+_OFF); -double tmp57 = cp(4+_OFF); -double tmp58 = 1/tmp57; -double tmp59 = -(tmp0*tmp7); -double tmp60 = -(tmp0*tmp8); -double tmp61 = 2*tmp5*tmp1*tmp3; -double tmp62 = -2*tmp4*tmp2*tmp3; -double tmp63 = tmp0*tmp9; -double tmp64 = -2*tmp4*tmp1*tmp6; -double tmp65 = -2*tmp5*tmp2*tmp6; -double tmp66 = tmp0*tmp10; -double tmp67 = tmp59 + tmp60 + tmp61 + tmp62 + tmp63 + tmp64 + tmp65 + tmp66 + tmp11; -double tmp68 = std::pow(tmp55,2); -double tmp69 = tmp25 + tmp68; +double tmp3 = x(7+_OFF); +double tmp4 = x(30+_OFF); +double tmp5 = x(31+_OFF); +double tmp6 = x(32+_OFF); +double tmp7 = x(33+_OFF); +double tmp8 = tmp5*tmp6; +double tmp9 = -(tmp4*tmp7); +double tmp10 = tmp8 + tmp9; +double tmp11 = tmp4*tmp5; +double tmp12 = tmp6*tmp7; +double tmp13 = tmp11 + tmp12; +double tmp14 = std::pow(tmp4,2); +double tmp15 = std::pow(tmp5,2); +double tmp16 = -tmp15; +double tmp17 = std::pow(tmp6,2); +double tmp18 = std::pow(tmp7,2); +double tmp19 = -tmp18; +double tmp20 = tmp14 + tmp16 + tmp17 + tmp19; +double tmp21 = wind(2+_OFF); +double tmp22 = wind(3+_OFF); +double tmp23 = wind(1+_OFF); +double tmp24 = -2*tmp23*tmp10; +double tmp25 = -2*tmp22*tmp13; +double tmp26 = -(tmp21*tmp20); +double tmp27 = x(35+_OFF); +double tmp28 = tmp24 + tmp25 + tmp26 + tmp27; +double tmp29 = tmp5*tmp7; +double tmp30 = -tmp17; +double tmp31 = cp(2+_OFF); +double tmp32 = tmp2*tmp1; +double tmp33 = tmp0*tmp3; +double tmp34 = tmp32 + tmp33; +double tmp35 = tmp4*tmp7; +double tmp36 = tmp8 + tmp35; +double tmp37 = -(tmp0*tmp1); +double tmp38 = tmp2*tmp3; +double tmp39 = tmp37 + tmp38; +double tmp40 = -(tmp4*tmp6); +double tmp41 = tmp40 + tmp29; +double tmp42 = std::pow(tmp0,2); +double tmp43 = std::pow(tmp2,2); +double tmp44 = std::pow(tmp1,2); +double tmp45 = -tmp44; +double tmp46 = std::pow(tmp3,2); +double tmp47 = -tmp46; +double tmp48 = tmp42 + tmp43 + tmp45 + tmp47; +double tmp49 = tmp14 + tmp15 + tmp30 + tmp19; +double tmp50 = -2*tmp21*tmp36; +double tmp51 = -2*tmp22*tmp41; +double tmp52 = -(tmp23*tmp49); +double tmp53 = x(34+_OFF); +double tmp54 = tmp50 + tmp51 + tmp52 + tmp53; +double tmp55 = std::pow(tmp54,2); +double tmp56 = tmp4*tmp6; +double tmp57 = tmp56 + tmp29; +double tmp58 = -2*tmp23*tmp57; +double tmp59 = -(tmp4*tmp5); +double tmp60 = tmp59 + tmp12; +double tmp61 = -2*tmp21*tmp60; +double tmp62 = tmp14 + tmp16 + tmp30 + tmp18; +double tmp63 = -(tmp22*tmp62); +double tmp64 = x(36+_OFF); +double tmp65 = tmp58 + tmp61 + tmp63 + tmp64; +double tmp66 = std::pow(tmp65,2); +double tmp67 = std::pow(tmp28,2); +double tmp68 = tmp55 + tmp67 + tmp66; +double tmp69 = tmp55 + tmp66; double tmp70 = 1/std::sqrt(tmp69); -double tmp71 = 1/tmp50; -double tmp72 = -(tmp44*tmp71); +double tmp71 = 1/tmp68; +double tmp72 = -(tmp67*tmp71); double tmp73 = 1 + tmp72; double tmp74 = std::sqrt(tmp73); -double tmp75 = std::atan2(tmp55,tmp24); -double tmp76 = -(tmp4*tmp7); -double tmp77 = -2*tmp5*tmp1*tmp2; -double tmp78 = tmp4*tmp8; -double tmp79 = -2*tmp0*tmp2*tmp3; -double tmp80 = -(tmp4*tmp9); -double tmp81 = 2*tmp0*tmp1*tmp6; -double tmp82 = -2*tmp5*tmp3*tmp6; -double tmp83 = tmp4*tmp10; -double tmp84 = tmp76 + tmp77 + tmp78 + tmp79 + tmp80 + tmp81 + tmp82 + tmp83 + tmp42; -double tmp85 = std::sqrt(tmp50); -double tmp86 = std::pow(tmp75,2); -double tmp87 = 1/std::sqrt(tmp50); -double tmp88 = tmp43*tmp87; -double tmp89 = std::asin(tmp88); -double tmp90 = std::pow(tmp89,2); -double tmp91 = -(tmp5*tmp7); -double tmp92 = 2*tmp4*tmp1*tmp2; -double tmp93 = tmp5*tmp8; -double tmp94 = -2*tmp0*tmp1*tmp3; -double tmp95 = tmp5*tmp9; -double tmp96 = -2*tmp0*tmp2*tmp6; -double tmp97 = -2*tmp4*tmp3*tmp6; -double tmp98 = -(tmp5*tmp10); -double tmp99 = tmp91 + tmp92 + tmp93 + tmp94 + tmp95 + tmp96 + tmp97 + tmp98 + tmp33; -J(1+_OFF,1+_OFF) = -0.5*tmp56*tmp58*tmp67*tmp70*tmp50*tmp74; -J(1+_OFF,2+_OFF) = -0.5*tmp75*tmp56*tmp58*tmp67*tmp70*tmp50*tmp74; -J(1+_OFF,3+_OFF) = -0.5*tmp86*tmp56*tmp58*tmp67*tmp70*tmp50*tmp74; -J(1+_OFF,4+_OFF) = -0.5*tmp90*tmp56*tmp58*tmp67*tmp70*tmp50*tmp74; -J(2+_OFF,1+_OFF) = -0.5*tmp56*tmp58*tmp84*tmp85; -J(2+_OFF,2+_OFF) = -0.5*tmp75*tmp56*tmp58*tmp84*tmp85; -J(2+_OFF,3+_OFF) = -0.5*tmp86*tmp56*tmp58*tmp84*tmp85; -J(2+_OFF,4+_OFF) = -0.5*tmp90*tmp56*tmp58*tmp84*tmp85; -J(3+_OFF,1+_OFF) = -0.5*tmp56*tmp58*tmp70*tmp99*tmp50*tmp74; -J(3+_OFF,2+_OFF) = -0.5*tmp75*tmp56*tmp58*tmp70*tmp99*tmp50*tmp74; -J(3+_OFF,3+_OFF) = -0.5*tmp86*tmp56*tmp58*tmp70*tmp99*tmp50*tmp74; -J(3+_OFF,4+_OFF) = -0.5*tmp90*tmp56*tmp58*tmp70*tmp99*tmp50*tmp74; +double tmp75 = cp(4+_OFF); +double tmp76 = 1/tmp75; +double tmp77 = 2*tmp48*tmp10; +double tmp78 = 4*tmp39*tmp13; +double tmp79 = 2*tmp34*tmp20; +double tmp80 = tmp77 + tmp78 + tmp79; +double tmp81 = std::sqrt(tmp68); +double tmp82 = std::atan2(tmp65,tmp54); +double tmp83 = 4*tmp34*tmp36; +double tmp84 = 4*tmp39*tmp41; +double tmp85 = tmp48*tmp49; +double tmp86 = tmp83 + tmp84 + tmp85; +double tmp87 = 2*tmp48*tmp57; +double tmp88 = 4*tmp34*tmp60; +double tmp89 = 2*tmp39*tmp62; +double tmp90 = tmp87 + tmp88 + tmp89; +double tmp91 = std::pow(tmp82,2); +double tmp92 = 1/std::sqrt(tmp68); +double tmp93 = tmp28*tmp92; +double tmp94 = std::asin(tmp93); +double tmp95 = std::pow(tmp94,2); +double tmp96 = -tmp43; +double tmp97 = tmp42 + tmp96 + tmp44 + tmp47; +double tmp98 = tmp0*tmp2; +double tmp99 = tmp1*tmp3; +double tmp100 = tmp98 + tmp99; +double tmp101 = -(tmp0*tmp3); +double tmp102 = tmp32 + tmp101; +double tmp103 = 4*tmp102*tmp10; +double tmp104 = 4*tmp100*tmp13; +double tmp105 = tmp97*tmp20; +double tmp106 = tmp103 + tmp104 + tmp105; +double tmp107 = 2*tmp97*tmp36; +double tmp108 = 4*tmp100*tmp41; +double tmp109 = 2*tmp102*tmp49; +double tmp110 = tmp107 + tmp108 + tmp109; +double tmp111 = 4*tmp102*tmp57; +double tmp112 = 2*tmp97*tmp60; +double tmp113 = 2*tmp100*tmp62; +double tmp114 = tmp111 + tmp112 + tmp113; +double tmp115 = -(tmp0*tmp2); +double tmp116 = tmp115 + tmp99; +double tmp117 = tmp42 + tmp96 + tmp45 + tmp46; +double tmp118 = tmp0*tmp1; +double tmp119 = tmp118 + tmp38; +double tmp120 = 4*tmp119*tmp10; +double tmp121 = 2*tmp117*tmp13; +double tmp122 = 2*tmp116*tmp20; +double tmp123 = tmp120 + tmp121 + tmp122; +double tmp124 = 4*tmp116*tmp36; +double tmp125 = 2*tmp117*tmp41; +double tmp126 = 2*tmp119*tmp49; +double tmp127 = tmp124 + tmp125 + tmp126; +double tmp128 = 4*tmp119*tmp57; +double tmp129 = 4*tmp116*tmp60; +double tmp130 = tmp117*tmp62; +double tmp131 = tmp128 + tmp129 + tmp130; +J(1+_OFF,1+_OFF) = -(tmp76*(0.634*tmp31*tmp80*tmp28*tmp81 + 0.634*tmp31*tmp86*tmp54*tmp70*tmp68*tmp74 + 0.634*tmp31*tmp90*tmp65*tmp70*tmp68*tmp74)); +J(1+_OFF,2+_OFF) = -(tmp76*(-0.634*tmp82*tmp31*tmp80*tmp28*tmp81 - 0.634*tmp82*tmp31*tmp86*tmp54*tmp70*tmp68*tmp74 - 0.634*tmp82*tmp31*tmp90*tmp65*tmp70*tmp68*tmp74)); +J(1+_OFF,3+_OFF) = -(tmp76*(0.634*tmp91*tmp31*tmp80*tmp28*tmp81 + 0.634*tmp91*tmp31*tmp86*tmp54*tmp70*tmp68*tmp74 + 0.634*tmp91*tmp31*tmp90*tmp65*tmp70*tmp68*tmp74)); +J(1+_OFF,4+_OFF) = -(tmp76*(0.634*tmp95*tmp31*tmp80*tmp28*tmp81 + 0.634*tmp95*tmp31*tmp86*tmp54*tmp70*tmp68*tmp74 + 0.634*tmp95*tmp31*tmp90*tmp65*tmp70*tmp68*tmp74)); +J(2+_OFF,1+_OFF) = -(tmp76*(0.634*tmp31*tmp106*tmp28*tmp81 + 0.634*tmp31*tmp110*tmp54*tmp70*tmp68*tmp74 + 0.634*tmp31*tmp114*tmp65*tmp70*tmp68*tmp74)); +J(2+_OFF,2+_OFF) = -(tmp76*(-0.634*tmp82*tmp31*tmp106*tmp28*tmp81 - 0.634*tmp82*tmp31*tmp110*tmp54*tmp70*tmp68*tmp74 - 0.634*tmp82*tmp31*tmp114*tmp65*tmp70*tmp68*tmp74)); +J(2+_OFF,3+_OFF) = -(tmp76*(0.634*tmp91*tmp31*tmp106*tmp28*tmp81 + 0.634*tmp91*tmp31*tmp110*tmp54*tmp70*tmp68*tmp74 + 0.634*tmp91*tmp31*tmp114*tmp65*tmp70*tmp68*tmp74)); +J(2+_OFF,4+_OFF) = -(tmp76*(0.634*tmp95*tmp31*tmp106*tmp28*tmp81 + 0.634*tmp95*tmp31*tmp110*tmp54*tmp70*tmp68*tmp74 + 0.634*tmp95*tmp31*tmp114*tmp65*tmp70*tmp68*tmp74)); +J(3+_OFF,1+_OFF) = -(tmp76*(0.634*tmp31*tmp123*tmp28*tmp81 + 0.634*tmp31*tmp127*tmp54*tmp70*tmp68*tmp74 + 0.634*tmp31*tmp131*tmp65*tmp70*tmp68*tmp74)); +J(3+_OFF,2+_OFF) = -(tmp76*(-0.634*tmp82*tmp31*tmp123*tmp28*tmp81 - 0.634*tmp82*tmp31*tmp127*tmp54*tmp70*tmp68*tmp74 - 0.634*tmp82*tmp31*tmp131*tmp65*tmp70*tmp68*tmp74)); +J(3+_OFF,3+_OFF) = -(tmp76*(0.634*tmp91*tmp31*tmp123*tmp28*tmp81 + 0.634*tmp91*tmp31*tmp127*tmp54*tmp70*tmp68*tmp74 + 0.634*tmp91*tmp31*tmp131*tmp65*tmp70*tmp68*tmp74)); +J(3+_OFF,4+_OFF) = -(tmp76*(0.634*tmp95*tmp31*tmp123*tmp28*tmp81 + 0.634*tmp95*tmp31*tmp127*tmp54*tmp70*tmp68*tmp74 + 0.634*tmp95*tmp31*tmp131*tmp65*tmp70*tmp68*tmp74)); J(4+_OFF,1+_OFF) = 0; J(4+_OFF,2+_OFF) = 0; J(4+_OFF,3+_OFF) = 0; diff --git a/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrFLat.cppready b/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrFLat.cppready index 7823b2b..53907ed 100644 --- a/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrFLat.cppready +++ b/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrFLat.cppready @@ -1,7 +1,7 @@ -double tmp0 = x(4+_OFF); -double tmp1 = x(5+_OFF); -double tmp2 = x(6+_OFF); -double tmp3 = x(7+_OFF); +double tmp0 = x(30+_OFF); +double tmp1 = x(31+_OFF); +double tmp2 = x(32+_OFF); +double tmp3 = x(33+_OFF); double tmp4 = wind(2+_OFF); double tmp5 = tmp1*tmp2; double tmp6 = wind(3+_OFF); @@ -21,62 +21,79 @@ double tmp19 = -2*tmp6*tmp18; double tmp20 = -tmp9; double tmp21 = tmp8 + tmp20 + tmp10 + tmp12; double tmp22 = -(tmp4*tmp21); -double tmp23 = x(9+_OFF); +double tmp23 = x(35+_OFF); double tmp24 = tmp15 + tmp19 + tmp22 + tmp23; double tmp25 = tmp1*tmp3; double tmp26 = -tmp10; -double tmp27 = x(8+_OFF); -double tmp28 = tmp0*tmp3; -double tmp29 = tmp5 + tmp28; -double tmp30 = -2*tmp4*tmp29; -double tmp31 = -(tmp0*tmp2); -double tmp32 = tmp31 + tmp25; -double tmp33 = -2*tmp6*tmp32; -double tmp34 = tmp8 + tmp9 + tmp26 + tmp12; -double tmp35 = -(tmp7*tmp34); -double tmp36 = tmp30 + tmp33 + tmp35 + tmp27; -double tmp37 = std::pow(tmp36,2); -double tmp38 = tmp0*tmp2; -double tmp39 = tmp38 + tmp25; -double tmp40 = -(tmp0*tmp1); -double tmp41 = tmp40 + tmp17; -double tmp42 = tmp8 + tmp20 + tmp26 + tmp11; -double tmp43 = x(10+_OFF); -double tmp44 = std::pow(tmp24,2); -double tmp45 = -2*tmp7*tmp39; -double tmp46 = -2*tmp4*tmp41; -double tmp47 = -(tmp6*tmp42); -double tmp48 = tmp45 + tmp46 + tmp47 + tmp43; -double tmp49 = std::pow(tmp48,2); -double tmp50 = tmp37 + tmp44 + tmp49; -double tmp51 = 1/std::sqrt(tmp50); -double tmp52 = tmp24*tmp51; -double tmp53 = std::asin(tmp52); -double tmp54 = cp(2+_OFF); -double tmp55 = cp(4+_OFF); -double tmp56 = 1/tmp55; -double tmp57 = tmp4*tmp8; -double tmp58 = 2*tmp6*tmp0*tmp1; -double tmp59 = -(tmp4*tmp9); -double tmp60 = 2*tmp7*tmp1*tmp2; -double tmp61 = tmp4*tmp10; -double tmp62 = -2*tmp7*tmp0*tmp3; -double tmp63 = 2*tmp6*tmp2*tmp3; -double tmp64 = -(tmp4*tmp11); -double tmp65 = -tmp23; -double tmp66 = tmp57 + tmp58 + tmp59 + tmp60 + tmp61 + tmp62 + tmp63 + tmp64 + tmp65; -double tmp67 = 2*tmp7*tmp39; -double tmp68 = 2*tmp4*tmp41; -double tmp69 = tmp6*tmp42; -double tmp70 = -tmp43; -double tmp71 = tmp67 + tmp68 + tmp69 + tmp70; -double tmp72 = std::pow(tmp71,2); -double tmp73 = tmp37 + tmp72; -double tmp74 = 1/std::sqrt(tmp73); -double tmp75 = std::sqrt(tmp50); -J(1+_OFF,1+_OFF) = -0.5*tmp53*tmp54*tmp56*(-(tmp7*tmp8) - tmp7*tmp9 + 2*tmp6*tmp0*tmp2 - 2*tmp4*tmp1*tmp2 + tmp7*tmp10 - 2*tmp4*tmp0*tmp3 - 2*tmp6*tmp1*tmp3 + tmp7*tmp11 + tmp27)*tmp66*tmp74*tmp75; -J(2+_OFF,1+_OFF) = -0.5*tmp53*tmp54*tmp56*tmp50*std::sqrt(1 - tmp44/tmp50); -J(3+_OFF,1+_OFF) = -0.5*tmp53*tmp54*tmp56*tmp66*tmp74*(-(tmp6*tmp8) + 2*tmp4*tmp0*tmp1 + tmp6*tmp9 - 2*tmp7*tmp0*tmp2 + tmp6*tmp10 - 2*tmp7*tmp1*tmp3 - 2*tmp4*tmp2*tmp3 - tmp6*tmp11 + tmp43)*tmp75; +double tmp27 = tmp0*tmp3; +double tmp28 = tmp5 + tmp27; +double tmp29 = x(4+_OFF); +double tmp30 = x(6+_OFF); +double tmp31 = x(5+_OFF); +double tmp32 = x(7+_OFF); +double tmp33 = -(tmp0*tmp2); +double tmp34 = tmp33 + tmp25; +double tmp35 = tmp8 + tmp9 + tmp26 + tmp12; +double tmp36 = -2*tmp4*tmp28; +double tmp37 = -2*tmp6*tmp34; +double tmp38 = -(tmp7*tmp35); +double tmp39 = x(34+_OFF); +double tmp40 = tmp36 + tmp37 + tmp38 + tmp39; +double tmp41 = std::pow(tmp40,2); +double tmp42 = tmp0*tmp2; +double tmp43 = tmp42 + tmp25; +double tmp44 = -2*tmp7*tmp43; +double tmp45 = -(tmp0*tmp1); +double tmp46 = tmp45 + tmp17; +double tmp47 = -2*tmp4*tmp46; +double tmp48 = tmp8 + tmp20 + tmp26 + tmp11; +double tmp49 = -(tmp6*tmp48); +double tmp50 = x(36+_OFF); +double tmp51 = tmp44 + tmp47 + tmp49 + tmp50; +double tmp52 = std::pow(tmp51,2); +double tmp53 = std::pow(tmp24,2); +double tmp54 = tmp41 + tmp53 + tmp52; +double tmp55 = 1/std::sqrt(tmp54); +double tmp56 = tmp24*tmp55; +double tmp57 = std::asin(tmp56); +double tmp58 = cp(2+_OFF); +double tmp59 = std::pow(tmp29,2); +double tmp60 = std::pow(tmp31,2); +double tmp61 = std::pow(tmp30,2); +double tmp62 = -tmp61; +double tmp63 = std::pow(tmp32,2); +double tmp64 = -tmp63; +double tmp65 = tmp59 + tmp60 + tmp62 + tmp64; +double tmp66 = tmp31*tmp30; +double tmp67 = tmp29*tmp32; +double tmp68 = tmp66 + tmp67; +double tmp69 = -(tmp29*tmp30); +double tmp70 = tmp31*tmp32; +double tmp71 = tmp69 + tmp70; +double tmp72 = tmp41 + tmp52; +double tmp73 = 1/std::sqrt(tmp72); +double tmp74 = std::sqrt(tmp54); +double tmp75 = cp(4+_OFF); +double tmp76 = 1/tmp75; +double tmp77 = -(tmp29*tmp32); +double tmp78 = tmp66 + tmp77; +double tmp79 = -tmp60; +double tmp80 = tmp59 + tmp79 + tmp61 + tmp64; +double tmp81 = tmp29*tmp31; +double tmp82 = tmp30*tmp32; +double tmp83 = tmp81 + tmp82; +double tmp84 = 1/tmp54; +double tmp85 = -(tmp53*tmp84); +double tmp86 = 1 + tmp85; +double tmp87 = std::sqrt(tmp86); +double tmp88 = tmp29*tmp30; +double tmp89 = tmp88 + tmp70; +double tmp90 = -(tmp29*tmp31); +double tmp91 = tmp90 + tmp82; +double tmp92 = tmp59 + tmp79 + tmp62 + tmp63; +J(1+_OFF,1+_OFF) = -(tmp76*(-0.634*tmp57*tmp58*(4*tmp68*tmp28 + 4*tmp71*tmp34 + tmp65*tmp35)*tmp40*tmp24*tmp73*tmp74 - 0.634*tmp57*tmp58*(2*tmp65*tmp43 + 4*tmp68*tmp46 + 2*tmp71*tmp48)*tmp24*tmp51*tmp73*tmp74 + 0.634*tmp57*tmp58*(2*tmp65*tmp14 + 4*tmp71*tmp18 + 2*tmp68*tmp21)*tmp54*tmp87)); +J(2+_OFF,1+_OFF) = -(tmp76*(-0.634*tmp57*tmp58*(2*tmp80*tmp28 + 4*tmp83*tmp34 + 2*tmp78*tmp35)*tmp40*tmp24*tmp73*tmp74 - 0.634*tmp57*tmp58*(4*tmp78*tmp43 + 2*tmp80*tmp46 + 2*tmp83*tmp48)*tmp24*tmp51*tmp73*tmp74 + 0.634*tmp57*tmp58*(4*tmp78*tmp14 + 4*tmp83*tmp18 + tmp80*tmp21)*tmp54*tmp87)); +J(3+_OFF,1+_OFF) = -(tmp76*(-0.634*tmp57*tmp58*(4*tmp91*tmp28 + 2*tmp92*tmp34 + 2*tmp89*tmp35)*tmp40*tmp24*tmp73*tmp74 - 0.634*tmp57*tmp58*(4*tmp89*tmp43 + 4*tmp91*tmp46 + tmp92*tmp48)*tmp24*tmp51*tmp73*tmp74 + 0.634*tmp57*tmp58*(4*tmp89*tmp14 + 2*tmp92*tmp18 + 2*tmp91*tmp21)*tmp54*tmp87)); J(4+_OFF,1+_OFF) = 0; J(5+_OFF,1+_OFF) = 0; J(6+_OFF,1+_OFF) = 0; diff --git a/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrFLift.cppready b/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrFLift.cppready index ca6efb8..9ceb459 100644 --- a/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrFLift.cppready +++ b/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrFLift.cppready @@ -1,90 +1,119 @@ -double tmp0 = x(4+_OFF); +double tmp0 = x(5+_OFF); double tmp1 = x(6+_OFF); -double tmp2 = x(5+_OFF); +double tmp2 = x(4+_OFF); double tmp3 = x(7+_OFF); -double tmp4 = wind(1+_OFF); -double tmp5 = tmp2*tmp3; -double tmp6 = wind(2+_OFF); -double tmp7 = wind(3+_OFF); -double tmp8 = std::pow(tmp0,2); -double tmp9 = std::pow(tmp2,2); -double tmp10 = std::pow(tmp1,2); -double tmp11 = -tmp10; -double tmp12 = std::pow(tmp3,2); -double tmp13 = x(10+_OFF); -double tmp14 = -tmp13; -double tmp15 = tmp2*tmp1; -double tmp16 = tmp0*tmp3; +double tmp4 = x(30+_OFF); +double tmp5 = x(31+_OFF); +double tmp6 = x(32+_OFF); +double tmp7 = x(33+_OFF); +double tmp8 = tmp5*tmp7; +double tmp9 = std::pow(tmp4,2); +double tmp10 = std::pow(tmp5,2); +double tmp11 = std::pow(tmp6,2); +double tmp12 = -tmp11; +double tmp13 = std::pow(tmp7,2); +double tmp14 = wind(2+_OFF); +double tmp15 = tmp5*tmp6; +double tmp16 = tmp4*tmp7; double tmp17 = tmp15 + tmp16; -double tmp18 = -2*tmp6*tmp17; -double tmp19 = -(tmp0*tmp1); -double tmp20 = tmp19 + tmp5; -double tmp21 = -2*tmp7*tmp20; -double tmp22 = -tmp12; -double tmp23 = tmp8 + tmp9 + tmp11 + tmp22; -double tmp24 = -(tmp4*tmp23); -double tmp25 = x(8+_OFF); -double tmp26 = tmp18 + tmp21 + tmp24 + tmp25; -double tmp27 = std::pow(tmp26,2); -double tmp28 = tmp1*tmp3; -double tmp29 = -tmp9; -double tmp30 = tmp0*tmp1; -double tmp31 = tmp30 + tmp5; -double tmp32 = -(tmp0*tmp2); -double tmp33 = tmp32 + tmp28; -double tmp34 = tmp8 + tmp29 + tmp11 + tmp12; -double tmp35 = 2*tmp4*tmp31; -double tmp36 = 2*tmp6*tmp33; -double tmp37 = tmp7*tmp34; -double tmp38 = tmp35 + tmp36 + tmp37 + tmp14; -double tmp39 = cp(2+_OFF); -double tmp40 = cp(4+_OFF); -double tmp41 = 1/tmp40; -double tmp42 = std::pow(tmp38,2); -double tmp43 = tmp27 + tmp42; -double tmp44 = 1/std::sqrt(tmp43); -double tmp45 = tmp7*tmp8; -double tmp46 = -2*tmp6*tmp0*tmp2; -double tmp47 = -(tmp7*tmp9); -double tmp48 = 2*tmp4*tmp0*tmp1; -double tmp49 = -(tmp7*tmp10); -double tmp50 = 2*tmp4*tmp2*tmp3; -double tmp51 = 2*tmp6*tmp1*tmp3; -double tmp52 = tmp7*tmp12; -double tmp53 = tmp45 + tmp46 + tmp47 + tmp48 + tmp49 + tmp50 + tmp51 + tmp52 + tmp14; -double tmp54 = -(tmp0*tmp3); -double tmp55 = tmp15 + tmp54; -double tmp56 = -2*tmp4*tmp55; -double tmp57 = tmp0*tmp2; -double tmp58 = tmp57 + tmp28; -double tmp59 = -2*tmp7*tmp58; -double tmp60 = tmp8 + tmp29 + tmp10 + tmp22; -double tmp61 = -(tmp6*tmp60); -double tmp62 = x(9+_OFF); -double tmp63 = tmp56 + tmp59 + tmp61 + tmp62; -double tmp64 = std::pow(tmp63,2); -double tmp65 = -2*tmp4*tmp31; -double tmp66 = -2*tmp6*tmp33; -double tmp67 = -(tmp7*tmp34); -double tmp68 = tmp65 + tmp66 + tmp67 + tmp13; +double tmp18 = -2*tmp14*tmp17; +double tmp19 = wind(3+_OFF); +double tmp20 = -(tmp4*tmp6); +double tmp21 = tmp20 + tmp8; +double tmp22 = -2*tmp19*tmp21; +double tmp23 = wind(1+_OFF); +double tmp24 = -tmp13; +double tmp25 = tmp9 + tmp10 + tmp12 + tmp24; +double tmp26 = -(tmp23*tmp25); +double tmp27 = x(34+_OFF); +double tmp28 = tmp18 + tmp22 + tmp26 + tmp27; +double tmp29 = tmp4*tmp6; +double tmp30 = tmp29 + tmp8; +double tmp31 = -(tmp4*tmp5); +double tmp32 = tmp6*tmp7; +double tmp33 = tmp31 + tmp32; +double tmp34 = -tmp10; +double tmp35 = tmp9 + tmp34 + tmp12 + tmp13; +double tmp36 = std::pow(tmp28,2); +double tmp37 = -2*tmp23*tmp30; +double tmp38 = -2*tmp14*tmp33; +double tmp39 = -(tmp19*tmp35); +double tmp40 = x(36+_OFF); +double tmp41 = tmp37 + tmp38 + tmp39 + tmp40; +double tmp42 = std::pow(tmp41,2); +double tmp43 = cp(2+_OFF); +double tmp44 = tmp0*tmp1; +double tmp45 = tmp2*tmp3; +double tmp46 = tmp44 + tmp45; +double tmp47 = -(tmp2*tmp1); +double tmp48 = tmp0*tmp3; +double tmp49 = tmp47 + tmp48; +double tmp50 = std::pow(tmp2,2); +double tmp51 = std::pow(tmp0,2); +double tmp52 = std::pow(tmp1,2); +double tmp53 = -tmp52; +double tmp54 = std::pow(tmp3,2); +double tmp55 = -tmp54; +double tmp56 = tmp50 + tmp51 + tmp53 + tmp55; +double tmp57 = tmp36 + tmp42; +double tmp58 = 1/std::sqrt(tmp57); +double tmp59 = -(tmp4*tmp7); +double tmp60 = tmp15 + tmp59; +double tmp61 = -2*tmp23*tmp60; +double tmp62 = tmp4*tmp5; +double tmp63 = tmp62 + tmp32; +double tmp64 = -2*tmp19*tmp63; +double tmp65 = tmp9 + tmp34 + tmp11 + tmp24; +double tmp66 = -(tmp14*tmp65); +double tmp67 = x(35+_OFF); +double tmp68 = tmp61 + tmp64 + tmp66 + tmp67; double tmp69 = std::pow(tmp68,2); -double tmp70 = tmp27 + tmp64 + tmp69; -double tmp71 = std::atan2(tmp38,tmp26); -double tmp72 = -(tmp4*tmp8); -double tmp73 = -(tmp4*tmp9); -double tmp74 = 2*tmp7*tmp0*tmp1; -double tmp75 = -2*tmp6*tmp2*tmp1; -double tmp76 = tmp4*tmp10; -double tmp77 = -2*tmp6*tmp0*tmp3; -double tmp78 = -2*tmp7*tmp2*tmp3; -double tmp79 = tmp4*tmp12; -double tmp80 = tmp72 + tmp73 + tmp74 + tmp75 + tmp76 + tmp77 + tmp78 + tmp79 + tmp25; -J(1+_OFF,1+_OFF) = 0.5*tmp39*tmp41*tmp44*tmp53*tmp70; -J(1+_OFF,2+_OFF) = 0.5*tmp71*tmp39*tmp41*tmp44*tmp53*tmp70; -J(2+_OFF,1+_OFF) = 0; -J(2+_OFF,2+_OFF) = 0; -J(3+_OFF,1+_OFF) = 0.5*tmp39*tmp41*tmp80*tmp44*tmp70; -J(3+_OFF,2+_OFF) = 0.5*tmp71*tmp39*tmp41*tmp80*tmp44*tmp70; +double tmp70 = tmp36 + tmp69 + tmp42; +double tmp71 = cp(4+_OFF); +double tmp72 = 1/tmp71; +double tmp73 = 2*tmp56*tmp30; +double tmp74 = 4*tmp46*tmp33; +double tmp75 = 2*tmp49*tmp35; +double tmp76 = tmp73 + tmp74 + tmp75; +double tmp77 = std::atan2(tmp41,tmp28); +double tmp78 = 4*tmp46*tmp17; +double tmp79 = 4*tmp49*tmp21; +double tmp80 = tmp56*tmp25; +double tmp81 = tmp78 + tmp79 + tmp80; +double tmp82 = -tmp51; +double tmp83 = tmp50 + tmp82 + tmp52 + tmp55; +double tmp84 = tmp2*tmp0; +double tmp85 = tmp1*tmp3; +double tmp86 = tmp84 + tmp85; +double tmp87 = -(tmp2*tmp3); +double tmp88 = tmp44 + tmp87; +double tmp89 = 4*tmp88*tmp30; +double tmp90 = 2*tmp83*tmp33; +double tmp91 = 2*tmp86*tmp35; +double tmp92 = tmp89 + tmp90 + tmp91; +double tmp93 = 2*tmp83*tmp17; +double tmp94 = 4*tmp86*tmp21; +double tmp95 = 2*tmp88*tmp25; +double tmp96 = tmp93 + tmp94 + tmp95; +double tmp97 = -(tmp2*tmp0); +double tmp98 = tmp97 + tmp85; +double tmp99 = tmp50 + tmp82 + tmp53 + tmp54; +double tmp100 = tmp2*tmp1; +double tmp101 = tmp100 + tmp48; +double tmp102 = 4*tmp101*tmp30; +double tmp103 = 4*tmp98*tmp33; +double tmp104 = tmp99*tmp35; +double tmp105 = tmp102 + tmp103 + tmp104; +double tmp106 = 4*tmp98*tmp17; +double tmp107 = 2*tmp99*tmp21; +double tmp108 = 2*tmp101*tmp25; +double tmp109 = tmp106 + tmp107 + tmp108; +J(1+_OFF,1+_OFF) = -(tmp72*(-0.634*tmp43*tmp76*tmp28*tmp58*tmp70 + 0.634*tmp43*tmp81*tmp41*tmp58*tmp70)); +J(1+_OFF,2+_OFF) = -(tmp72*(0.634*tmp77*tmp43*tmp76*tmp28*tmp58*tmp70 - 0.634*tmp77*tmp43*tmp81*tmp41*tmp58*tmp70)); +J(2+_OFF,1+_OFF) = -(tmp72*(-0.634*tmp43*tmp92*tmp28*tmp58*tmp70 + 0.634*tmp43*tmp96*tmp41*tmp58*tmp70)); +J(2+_OFF,2+_OFF) = -(tmp72*(0.634*tmp77*tmp43*tmp92*tmp28*tmp58*tmp70 - 0.634*tmp77*tmp43*tmp96*tmp41*tmp58*tmp70)); +J(3+_OFF,1+_OFF) = -(tmp72*(-0.634*tmp43*tmp105*tmp28*tmp58*tmp70 + 0.634*tmp43*tmp109*tmp41*tmp58*tmp70)); +J(3+_OFF,2+_OFF) = -(tmp72*(0.634*tmp77*tmp43*tmp105*tmp28*tmp58*tmp70 - 0.634*tmp77*tmp43*tmp109*tmp41*tmp58*tmp70)); J(4+_OFF,1+_OFF) = 0; J(4+_OFF,2+_OFF) = 0; J(5+_OFF,1+_OFF) = 0; diff --git a/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrFThrust.cppready b/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrFThrust.cppready index 6ab2414..0481512 100644 --- a/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrFThrust.cppready +++ b/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrFThrust.cppready @@ -1,67 +1,113 @@ -double tmp0 = cp(1+_OFF); -double tmp1 = cp(4+_OFF); -double tmp2 = 1/tmp1; -double tmp3 = x(4+_OFF); -double tmp4 = x(6+_OFF); -double tmp5 = x(5+_OFF); -double tmp6 = x(7+_OFF); -double tmp7 = wind(1+_OFF); -double tmp8 = tmp5*tmp4; -double tmp9 = wind(3+_OFF); -double tmp10 = wind(2+_OFF); -double tmp11 = std::pow(tmp3,2); -double tmp12 = std::pow(tmp5,2); -double tmp13 = std::pow(tmp4,2); -double tmp14 = std::pow(tmp6,2); -double tmp15 = -tmp14; -double tmp16 = tmp5*tmp6; -double tmp17 = tmp4*tmp6; -double tmp18 = -tmp12; -double tmp19 = -tmp13; -double tmp20 = z(1+_OFF); -double tmp21 = tmp3*tmp6; -double tmp22 = tmp8 + tmp21; -double tmp23 = -2*tmp10*tmp22; -double tmp24 = -(tmp3*tmp4); -double tmp25 = tmp24 + tmp16; -double tmp26 = -2*tmp9*tmp25; -double tmp27 = tmp11 + tmp12 + tmp19 + tmp15; -double tmp28 = -(tmp7*tmp27); -double tmp29 = x(8+_OFF); -double tmp30 = tmp23 + tmp26 + tmp28 + tmp29; -double tmp31 = std::pow(tmp30,2); -double tmp32 = -(tmp3*tmp6); -double tmp33 = tmp8 + tmp32; -double tmp34 = -2*tmp7*tmp33; -double tmp35 = tmp3*tmp5; -double tmp36 = tmp35 + tmp17; -double tmp37 = -2*tmp9*tmp36; -double tmp38 = tmp11 + tmp18 + tmp13 + tmp15; -double tmp39 = -(tmp10*tmp38); -double tmp40 = x(9+_OFF); -double tmp41 = tmp34 + tmp37 + tmp39 + tmp40; -double tmp42 = std::pow(tmp41,2); -double tmp43 = tmp3*tmp4; -double tmp44 = tmp43 + tmp16; -double tmp45 = -2*tmp7*tmp44; -double tmp46 = -(tmp3*tmp5); -double tmp47 = tmp46 + tmp17; -double tmp48 = -2*tmp10*tmp47; -double tmp49 = tmp11 + tmp18 + tmp19 + tmp14; -double tmp50 = -(tmp9*tmp49); -double tmp51 = x(10+_OFF); -double tmp52 = tmp45 + tmp48 + tmp50 + tmp51; -double tmp53 = std::pow(tmp52,2); -double tmp54 = tmp31 + tmp42 + tmp53; -J(1+_OFF,1+_OFF) = -1.*std::pow(tmp0,4)*tmp2*std::pow(tmp20,2); -J(1+_OFF,2+_OFF) = -0.3183098861837907*std::pow(tmp0,3)*tmp2*std::sqrt(tmp54)*tmp20; -J(1+_OFF,3+_OFF) = -0.10132118364233778*std::pow(tmp0,2)*tmp2*tmp54; -J(2+_OFF,1+_OFF) = 0; -J(2+_OFF,2+_OFF) = 0; -J(2+_OFF,3+_OFF) = 0; -J(3+_OFF,1+_OFF) = 0; -J(3+_OFF,2+_OFF) = 0; -J(3+_OFF,3+_OFF) = 0; +double tmp0 = x(4+_OFF); +double tmp1 = x(6+_OFF); +double tmp2 = x(5+_OFF); +double tmp3 = x(7+_OFF); +double tmp4 = x(30+_OFF); +double tmp5 = x(32+_OFF); +double tmp6 = x(31+_OFF); +double tmp7 = x(33+_OFF); +double tmp8 = cp(1+_OFF); +double tmp9 = cp(4+_OFF); +double tmp10 = 1/tmp9; +double tmp11 = tmp2*tmp1; +double tmp12 = tmp0*tmp3; +double tmp13 = tmp11 + tmp12; +double tmp14 = tmp6*tmp5; +double tmp15 = tmp4*tmp7; +double tmp16 = tmp14 + tmp15; +double tmp17 = 4*tmp13*tmp16; +double tmp18 = -(tmp0*tmp1); +double tmp19 = tmp2*tmp3; +double tmp20 = tmp18 + tmp19; +double tmp21 = -(tmp4*tmp5); +double tmp22 = tmp6*tmp7; +double tmp23 = tmp21 + tmp22; +double tmp24 = 4*tmp20*tmp23; +double tmp25 = std::pow(tmp0,2); +double tmp26 = std::pow(tmp2,2); +double tmp27 = std::pow(tmp1,2); +double tmp28 = -tmp27; +double tmp29 = std::pow(tmp3,2); +double tmp30 = -tmp29; +double tmp31 = tmp25 + tmp26 + tmp28 + tmp30; +double tmp32 = std::pow(tmp4,2); +double tmp33 = std::pow(tmp6,2); +double tmp34 = std::pow(tmp5,2); +double tmp35 = -tmp34; +double tmp36 = std::pow(tmp7,2); +double tmp37 = -tmp36; +double tmp38 = tmp32 + tmp33 + tmp35 + tmp37; +double tmp39 = tmp31*tmp38; +double tmp40 = tmp17 + tmp24 + tmp39; +double tmp41 = wind(1+_OFF); +double tmp42 = wind(3+_OFF); +double tmp43 = wind(2+_OFF); +double tmp44 = tmp5*tmp7; +double tmp45 = -tmp33; +double tmp46 = z(1+_OFF); +double tmp47 = -2*tmp43*tmp16; +double tmp48 = -2*tmp42*tmp23; +double tmp49 = -(tmp41*tmp38); +double tmp50 = x(34+_OFF); +double tmp51 = tmp47 + tmp48 + tmp49 + tmp50; +double tmp52 = std::pow(tmp51,2); +double tmp53 = -(tmp4*tmp7); +double tmp54 = tmp14 + tmp53; +double tmp55 = -2*tmp41*tmp54; +double tmp56 = tmp4*tmp6; +double tmp57 = tmp56 + tmp44; +double tmp58 = -2*tmp42*tmp57; +double tmp59 = tmp32 + tmp45 + tmp34 + tmp37; +double tmp60 = -(tmp43*tmp59); +double tmp61 = x(35+_OFF); +double tmp62 = tmp55 + tmp58 + tmp60 + tmp61; +double tmp63 = std::pow(tmp62,2); +double tmp64 = tmp4*tmp5; +double tmp65 = tmp64 + tmp22; +double tmp66 = -2*tmp41*tmp65; +double tmp67 = -(tmp4*tmp6); +double tmp68 = tmp67 + tmp44; +double tmp69 = -2*tmp43*tmp68; +double tmp70 = tmp32 + tmp45 + tmp35 + tmp36; +double tmp71 = -(tmp42*tmp70); +double tmp72 = x(36+_OFF); +double tmp73 = tmp66 + tmp69 + tmp71 + tmp72; +double tmp74 = std::pow(tmp73,2); +double tmp75 = tmp52 + tmp63 + tmp74; +double tmp76 = std::pow(tmp8,4); +double tmp77 = std::pow(tmp46,2); +double tmp78 = std::pow(tmp8,3); +double tmp79 = -tmp26; +double tmp80 = tmp25 + tmp79 + tmp27 + tmp30; +double tmp81 = 2*tmp80*tmp16; +double tmp82 = tmp0*tmp2; +double tmp83 = tmp1*tmp3; +double tmp84 = tmp82 + tmp83; +double tmp85 = 4*tmp84*tmp23; +double tmp86 = -(tmp0*tmp3); +double tmp87 = tmp11 + tmp86; +double tmp88 = 2*tmp87*tmp38; +double tmp89 = tmp81 + tmp85 + tmp88; +double tmp90 = std::sqrt(tmp75); +double tmp91 = std::pow(tmp8,2); +double tmp92 = -(tmp0*tmp2); +double tmp93 = tmp92 + tmp83; +double tmp94 = 4*tmp93*tmp16; +double tmp95 = tmp25 + tmp79 + tmp28 + tmp29; +double tmp96 = 2*tmp95*tmp23; +double tmp97 = tmp0*tmp1; +double tmp98 = tmp97 + tmp19; +double tmp99 = 2*tmp98*tmp38; +double tmp100 = tmp94 + tmp96 + tmp99; +J(1+_OFF,1+_OFF) = -1.268*tmp76*tmp10*tmp40*tmp77; +J(1+_OFF,2+_OFF) = -0.4036169356810466*tmp78*tmp10*tmp40*tmp90*tmp46; +J(1+_OFF,3+_OFF) = -0.1284752608584843*tmp91*tmp10*tmp40*tmp75; +J(2+_OFF,1+_OFF) = -1.268*tmp76*tmp10*tmp89*tmp77; +J(2+_OFF,2+_OFF) = -0.4036169356810466*tmp78*tmp10*tmp89*tmp90*tmp46; +J(2+_OFF,3+_OFF) = -0.1284752608584843*tmp91*tmp10*tmp89*tmp75; +J(3+_OFF,1+_OFF) = -1.268*tmp76*tmp10*tmp100*tmp77; +J(3+_OFF,2+_OFF) = -0.4036169356810466*tmp78*tmp10*tmp100*tmp90*tmp46; +J(3+_OFF,3+_OFF) = -0.1284752608584843*tmp91*tmp10*tmp100*tmp75; J(4+_OFF,1+_OFF) = 0; J(4+_OFF,2+_OFF) = 0; J(4+_OFF,3+_OFF) = 0; diff --git a/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrIbd.cppready b/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrIbd.cppready index 637af48..798bf66 100644 --- a/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrIbd.cppready +++ b/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrIbd.cppready @@ -21,139 +21,208 @@ double tmp19 = x(4+_OFF); double tmp20 = x(6+_OFF); double tmp21 = x(5+_OFF); double tmp22 = x(7+_OFF); -double tmp23 = wind(1+_OFF); -double tmp24 = tmp21*tmp20; -double tmp25 = wind(3+_OFF); -double tmp26 = wind(2+_OFF); -double tmp27 = std::pow(tmp19,2); -double tmp28 = std::pow(tmp21,2); -double tmp29 = std::pow(tmp20,2); -double tmp30 = std::pow(tmp22,2); -double tmp31 = -tmp30; -double tmp32 = tmp21*tmp22; -double tmp33 = tmp20*tmp22; -double tmp34 = -tmp28; -double tmp35 = -tmp29; -double tmp36 = -(tmp19*tmp22); -double tmp37 = tmp24 + tmp36; -double tmp38 = -2*tmp23*tmp37; -double tmp39 = tmp19*tmp21; -double tmp40 = tmp39 + tmp33; -double tmp41 = -2*tmp25*tmp40; -double tmp42 = tmp27 + tmp34 + tmp29 + tmp31; -double tmp43 = -(tmp26*tmp42); -double tmp44 = x(9+_OFF); -double tmp45 = tmp38 + tmp41 + tmp43 + tmp44; -double tmp46 = tmp19*tmp22; -double tmp47 = tmp24 + tmp46; -double tmp48 = -2*tmp26*tmp47; -double tmp49 = -(tmp19*tmp20); -double tmp50 = tmp49 + tmp32; -double tmp51 = -2*tmp25*tmp50; -double tmp52 = tmp27 + tmp28 + tmp35 + tmp31; -double tmp53 = -(tmp23*tmp52); -double tmp54 = x(8+_OFF); -double tmp55 = tmp48 + tmp51 + tmp53 + tmp54; -double tmp56 = std::pow(tmp55,2); -double tmp57 = std::pow(tmp45,2); -double tmp58 = tmp19*tmp20; -double tmp59 = tmp58 + tmp32; -double tmp60 = -2*tmp23*tmp59; -double tmp61 = -(tmp19*tmp21); -double tmp62 = tmp61 + tmp33; -double tmp63 = -2*tmp26*tmp62; -double tmp64 = tmp27 + tmp34 + tmp35 + tmp30; -double tmp65 = -(tmp25*tmp64); -double tmp66 = x(10+_OFF); -double tmp67 = tmp60 + tmp63 + tmp65 + tmp66; -double tmp68 = std::pow(tmp67,2); -double tmp69 = tmp56 + tmp57 + tmp68; -double tmp70 = cp(3+_OFF); -double tmp71 = 1/std::sqrt(tmp69); -double tmp72 = -(tmp2*tmp3); -double tmp73 = tmp1*tmp4; -double tmp74 = tmp72 + tmp73; -double tmp75 = tmp0*tmp2; -double tmp76 = -tmp12; -double tmp77 = tmp75 + tmp76; -double tmp78 = std::pow(tmp14,-2); -double tmp79 = std::pow(tmp18,2); -double tmp80 = cp(2+_OFF); -double tmp81 = cBar(1+_OFF); -double tmp82 = -(tmp0*tmp1); -double tmp83 = tmp3*tmp4; -double tmp84 = tmp82 + tmp83; -double tmp85 = std::pow(tmp16,2); -double tmp86 = std::pow(tmp17,2); -double tmp87 = tmp45*tmp71; -double tmp88 = std::asin(tmp87); -double tmp89 = -(tmp1*tmp16*tmp17); -double tmp90 = -(tmp4*tmp86); -double tmp91 = tmp3*tmp16*tmp18; -double tmp92 = tmp0*tmp17*tmp18; -double tmp93 = -(tmp2*tmp17*tmp18); -double tmp94 = tmp4*tmp79; -double tmp95 = mRoll(2+_OFF); -double tmp96 = -(tmp88*tmp95); -double tmp97 = mRoll(3+_OFF); -double tmp98 = (tmp70*tmp97*tmp71*tmp16)/2.; -double tmp99 = mRoll(4+_OFF); -double tmp100 = -(tmp70*tmp99*tmp71*tmp18)/2.; -double tmp101 = mRoll(1+_OFF); -double tmp102 = z(2+_OFF); -double tmp103 = -(tmp101*tmp102); -double tmp104 = tmp96 + tmp98 + tmp100 + tmp103; -double tmp105 = 0.5*tmp80*tmp70*tmp69*tmp104; -double tmp106 = tmp89 + tmp90 + tmp91 + tmp92 + tmp93 + tmp94 + tmp105; -double tmp107 = tmp5*tmp2; -double tmp108 = -tmp7; -double tmp109 = tmp107 + tmp108; -double tmp110 = tmp1*tmp85; -double tmp111 = tmp4*tmp16*tmp17; -double tmp112 = -(tmp5*tmp16*tmp18); -double tmp113 = tmp2*tmp16*tmp18; -double tmp114 = -(tmp3*tmp17*tmp18); -double tmp115 = -(tmp1*tmp79); -double tmp116 = mPitch(1+_OFF); -double tmp117 = -tmp116; -double tmp118 = 2*tmp23*tmp59; -double tmp119 = 2*tmp26*tmp62; -double tmp120 = tmp25*tmp64; -double tmp121 = -tmp66; -double tmp122 = tmp118 + tmp119 + tmp120 + tmp121; -double tmp123 = std::atan2(tmp122,tmp55); -double tmp124 = mPitch(3+_OFF); -double tmp125 = -(tmp123*tmp124); -double tmp126 = mPitch(4+_OFF); -double tmp127 = (tmp81*tmp126*tmp71*tmp17)/2.; +double tmp23 = x(30+_OFF); +double tmp24 = x(32+_OFF); +double tmp25 = x(31+_OFF); +double tmp26 = x(33+_OFF); +double tmp27 = tmp25*tmp24; +double tmp28 = tmp23*tmp26; +double tmp29 = tmp27 + tmp28; +double tmp30 = -(tmp23*tmp24); +double tmp31 = tmp25*tmp26; +double tmp32 = tmp30 + tmp31; +double tmp33 = std::pow(tmp23,2); +double tmp34 = std::pow(tmp25,2); +double tmp35 = std::pow(tmp24,2); +double tmp36 = -tmp35; +double tmp37 = std::pow(tmp26,2); +double tmp38 = -tmp37; +double tmp39 = tmp33 + tmp34 + tmp36 + tmp38; +double tmp40 = wind(1+_OFF); +double tmp41 = wind(3+_OFF); +double tmp42 = wind(2+_OFF); +double tmp43 = tmp24*tmp26; +double tmp44 = -tmp34; +double tmp45 = -(tmp23*tmp26); +double tmp46 = tmp27 + tmp45; +double tmp47 = -2*tmp40*tmp46; +double tmp48 = tmp23*tmp25; +double tmp49 = tmp48 + tmp43; +double tmp50 = -2*tmp41*tmp49; +double tmp51 = tmp33 + tmp44 + tmp35 + tmp38; +double tmp52 = -(tmp42*tmp51); +double tmp53 = x(35+_OFF); +double tmp54 = tmp47 + tmp50 + tmp52 + tmp53; +double tmp55 = -2*tmp42*tmp29; +double tmp56 = -2*tmp41*tmp32; +double tmp57 = -(tmp40*tmp39); +double tmp58 = x(34+_OFF); +double tmp59 = tmp55 + tmp56 + tmp57 + tmp58; +double tmp60 = std::pow(tmp59,2); +double tmp61 = std::pow(tmp54,2); +double tmp62 = tmp23*tmp24; +double tmp63 = tmp62 + tmp31; +double tmp64 = -2*tmp40*tmp63; +double tmp65 = -(tmp23*tmp25); +double tmp66 = tmp65 + tmp43; +double tmp67 = -2*tmp42*tmp66; +double tmp68 = tmp33 + tmp44 + tmp36 + tmp37; +double tmp69 = -(tmp41*tmp68); +double tmp70 = x(36+_OFF); +double tmp71 = tmp64 + tmp67 + tmp69 + tmp70; +double tmp72 = std::pow(tmp71,2); +double tmp73 = tmp60 + tmp61 + tmp72; +double tmp74 = cp(3+_OFF); +double tmp75 = 1/std::sqrt(tmp73); +double tmp76 = cp(2+_OFF); +double tmp77 = std::pow(tmp19,2); +double tmp78 = std::pow(tmp21,2); +double tmp79 = std::pow(tmp20,2); +double tmp80 = -tmp79; +double tmp81 = std::pow(tmp22,2); +double tmp82 = -tmp81; +double tmp83 = tmp77 + tmp78 + tmp80 + tmp82; +double tmp84 = -(tmp19*tmp20); +double tmp85 = tmp21*tmp22; +double tmp86 = tmp84 + tmp85; +double tmp87 = tmp21*tmp20; +double tmp88 = tmp19*tmp22; +double tmp89 = tmp87 + tmp88; +double tmp90 = cBar(1+_OFF); +double tmp91 = tmp54*tmp75; +double tmp92 = std::asin(tmp91); +double tmp93 = x(39+_OFF); +double tmp94 = -(tmp2*tmp3); +double tmp95 = tmp1*tmp4; +double tmp96 = tmp94 + tmp95; +double tmp97 = tmp0*tmp2; +double tmp98 = -tmp12; +double tmp99 = tmp97 + tmp98; +double tmp100 = std::pow(tmp14,-2); +double tmp101 = std::pow(tmp18,2); +double tmp102 = mRoll(2+_OFF); +double tmp103 = -(tmp92*tmp102); +double tmp104 = mRoll(3+_OFF); +double tmp105 = x(37+_OFF); +double tmp106 = (tmp74*tmp104*tmp75*tmp105)/2.; +double tmp107 = mRoll(4+_OFF); +double tmp108 = -0.5*(tmp74*tmp107*tmp75*tmp93); +double tmp109 = mRoll(1+_OFF); +double tmp110 = z(2+_OFF); +double tmp111 = tmp109*tmp110; +double tmp112 = tmp103 + tmp106 + tmp108 + tmp111; +double tmp113 = -(tmp19*tmp22); +double tmp114 = tmp87 + tmp113; +double tmp115 = tmp19*tmp21; +double tmp116 = tmp20*tmp22; +double tmp117 = tmp115 + tmp116; +double tmp118 = -tmp78; +double tmp119 = tmp77 + tmp118 + tmp79 + tmp82; +double tmp120 = mPitch(1+_OFF); +double tmp121 = -tmp120; +double tmp122 = std::atan2(tmp71,tmp59); +double tmp123 = mPitch(3+_OFF); +double tmp124 = tmp122*tmp123; +double tmp125 = mPitch(4+_OFF); +double tmp126 = x(38+_OFF); +double tmp127 = (tmp90*tmp125*tmp75*tmp126)/2.; double tmp128 = mPitch(2+_OFF); double tmp129 = z(3+_OFF); double tmp130 = -(tmp128*tmp129); -double tmp131 = tmp117 + tmp125 + tmp127 + tmp130; -double tmp132 = 0.5*tmp81*tmp80*tmp69*tmp131; -double tmp133 = tmp110 + tmp111 + tmp112 + tmp113 + tmp114 + tmp115 + tmp132; -double tmp134 = -(tmp3*tmp85); -double tmp135 = tmp5*tmp16*tmp17; -double tmp136 = -(tmp0*tmp16*tmp17); -double tmp137 = tmp3*tmp86; -double tmp138 = -(tmp4*tmp16*tmp18); -double tmp139 = tmp1*tmp17*tmp18; -double tmp140 = mYaw(2+_OFF); -double tmp141 = tmp88*tmp140; -double tmp142 = mYaw(3+_OFF); -double tmp143 = (tmp70*tmp142*tmp71*tmp18)/2.; -double tmp144 = mYaw(1+_OFF); -double tmp145 = z(4+_OFF); -double tmp146 = tmp144*tmp145; -double tmp147 = tmp141 + tmp143 + tmp146; -double tmp148 = 0.5*tmp80*tmp70*tmp69*tmp147; -double tmp149 = tmp134 + tmp135 + tmp136 + tmp137 + tmp138 + tmp139 + tmp148; -double tmp150 = tmp5*tmp0; -double tmp151 = -tmp9; -double tmp152 = tmp150 + tmp151; -double tmp153 = tmp1*tmp3; -double tmp154 = -(tmp5*tmp4); -double tmp155 = tmp153 + tmp154; +double tmp131 = tmp121 + tmp124 + tmp127 + tmp130; +double tmp132 = mYaw(2+_OFF); +double tmp133 = tmp92*tmp132; +double tmp134 = mYaw(3+_OFF); +double tmp135 = (tmp74*tmp134*tmp75*tmp93)/2.; +double tmp136 = mYaw(1+_OFF); +double tmp137 = z(4+_OFF); +double tmp138 = -(tmp136*tmp137); +double tmp139 = tmp133 + tmp135 + tmp138; +double tmp140 = -(tmp0*tmp1); +double tmp141 = tmp3*tmp4; +double tmp142 = tmp140 + tmp141; +double tmp143 = std::pow(tmp16,2); +double tmp144 = std::pow(tmp17,2); +double tmp145 = tmp19*tmp20; +double tmp146 = tmp145 + tmp85; +double tmp147 = tmp77 + tmp118 + tmp80 + tmp81; +double tmp148 = -(tmp19*tmp21); +double tmp149 = tmp148 + tmp116; +double tmp150 = -(tmp1*tmp16*tmp17); +double tmp151 = -(tmp4*tmp144); +double tmp152 = tmp3*tmp16*tmp18; +double tmp153 = tmp0*tmp17*tmp18; +double tmp154 = -(tmp2*tmp17*tmp18); +double tmp155 = tmp4*tmp101; +double tmp156 = 4*tmp89*tmp29; +double tmp157 = 4*tmp86*tmp32; +double tmp158 = tmp83*tmp39; +double tmp159 = tmp156 + tmp157 + tmp158; +double tmp160 = 0.634*tmp76*tmp74*tmp159*tmp73*tmp112; +double tmp161 = 2*tmp83*tmp46; +double tmp162 = 4*tmp86*tmp49; +double tmp163 = 2*tmp89*tmp51; +double tmp164 = tmp161 + tmp162 + tmp163; +double tmp165 = 0.634*tmp90*tmp76*tmp164*tmp73*tmp131; +double tmp166 = 2*tmp83*tmp63; +double tmp167 = 4*tmp89*tmp66; +double tmp168 = 2*tmp86*tmp68; +double tmp169 = tmp166 + tmp167 + tmp168; +double tmp170 = 0.634*tmp76*tmp74*tmp169*tmp73*tmp139; +double tmp171 = tmp150 + tmp151 + tmp152 + tmp153 + tmp154 + tmp155 + tmp160 + tmp165 + tmp170; +double tmp172 = tmp5*tmp2; +double tmp173 = -tmp7; +double tmp174 = tmp172 + tmp173; +double tmp175 = tmp1*tmp143; +double tmp176 = tmp4*tmp16*tmp17; +double tmp177 = -(tmp5*tmp16*tmp18); +double tmp178 = tmp2*tmp16*tmp18; +double tmp179 = -(tmp3*tmp17*tmp18); +double tmp180 = -(tmp1*tmp101); +double tmp181 = 2*tmp119*tmp29; +double tmp182 = 4*tmp117*tmp32; +double tmp183 = 2*tmp114*tmp39; +double tmp184 = tmp181 + tmp182 + tmp183; +double tmp185 = 0.634*tmp76*tmp74*tmp184*tmp73*tmp112; +double tmp186 = 4*tmp114*tmp46; +double tmp187 = 4*tmp117*tmp49; +double tmp188 = tmp119*tmp51; +double tmp189 = tmp186 + tmp187 + tmp188; +double tmp190 = 0.634*tmp90*tmp76*tmp189*tmp73*tmp131; +double tmp191 = 4*tmp114*tmp63; +double tmp192 = 2*tmp119*tmp66; +double tmp193 = 2*tmp117*tmp68; +double tmp194 = tmp191 + tmp192 + tmp193; +double tmp195 = 0.634*tmp76*tmp74*tmp194*tmp73*tmp139; +double tmp196 = tmp175 + tmp176 + tmp177 + tmp178 + tmp179 + tmp180 + tmp185 + tmp190 + tmp195; +double tmp197 = -(tmp3*tmp143); +double tmp198 = tmp5*tmp16*tmp17; +double tmp199 = -(tmp0*tmp16*tmp17); +double tmp200 = tmp3*tmp144; +double tmp201 = -(tmp4*tmp16*tmp18); +double tmp202 = tmp1*tmp17*tmp18; +double tmp203 = 4*tmp149*tmp29; +double tmp204 = 2*tmp147*tmp32; +double tmp205 = 2*tmp146*tmp39; +double tmp206 = tmp203 + tmp204 + tmp205; +double tmp207 = 0.634*tmp76*tmp74*tmp206*tmp73*tmp112; +double tmp208 = 4*tmp146*tmp46; +double tmp209 = 2*tmp147*tmp49; +double tmp210 = 2*tmp149*tmp51; +double tmp211 = tmp208 + tmp209 + tmp210; +double tmp212 = 0.634*tmp90*tmp76*tmp211*tmp73*tmp131; +double tmp213 = 4*tmp146*tmp63; +double tmp214 = 4*tmp149*tmp66; +double tmp215 = tmp147*tmp68; +double tmp216 = tmp213 + tmp214 + tmp215; +double tmp217 = 0.634*tmp76*tmp74*tmp216*tmp73*tmp139; +double tmp218 = tmp197 + tmp198 + tmp199 + tmp200 + tmp201 + tmp202 + tmp207 + tmp212 + tmp217; +double tmp219 = tmp5*tmp0; +double tmp220 = -tmp9; +double tmp221 = tmp219 + tmp220; +double tmp222 = tmp1*tmp3; +double tmp223 = -(tmp5*tmp4); +double tmp224 = tmp222 + tmp223; J(1+_OFF,1+_OFF) = 0; J(1+_OFF,2+_OFF) = 0; J(1+_OFF,3+_OFF) = 0; @@ -163,12 +232,12 @@ J(2+_OFF,3+_OFF) = 0; J(3+_OFF,1+_OFF) = 0; J(3+_OFF,2+_OFF) = 0; J(3+_OFF,3+_OFF) = 0; -J(4+_OFF,1+_OFF) = -(tmp84*tmp15*tmp16*tmp17) + tmp74*tmp15*tmp16*tmp18 + std::pow(tmp77,2)*tmp78*tmp106 + tmp74*tmp77*tmp78*tmp133 + tmp84*tmp77*tmp78*tmp149; -J(4+_OFF,2+_OFF) = tmp84*tmp15*tmp16*tmp17 - tmp77*tmp15*tmp17*tmp18 + tmp109*tmp77*tmp78*tmp106 - tmp2*tmp15*tmp106 + tmp109*tmp74*tmp78*tmp133 + tmp109*tmp84*tmp78*tmp149 + tmp1*tmp15*tmp149; -J(4+_OFF,3+_OFF) = -(tmp74*tmp15*tmp16*tmp18) + tmp77*tmp15*tmp17*tmp18 + tmp152*tmp77*tmp78*tmp106 - tmp0*tmp15*tmp106 + tmp152*tmp74*tmp78*tmp133 + tmp3*tmp15*tmp133 + tmp152*tmp84*tmp78*tmp149; -J(5+_OFF,1+_OFF) = -(tmp155*tmp15*tmp16*tmp17) + tmp109*tmp15*tmp16*tmp18 + tmp74*tmp77*tmp78*tmp106 + tmp109*tmp77*tmp78*tmp133 - tmp2*tmp15*tmp133 + tmp155*tmp77*tmp78*tmp149 + tmp4*tmp15*tmp149; -J(5+_OFF,2+_OFF) = tmp155*tmp15*tmp16*tmp17 - tmp74*tmp15*tmp17*tmp18 + tmp109*tmp74*tmp78*tmp106 + std::pow(tmp109,2)*tmp78*tmp133 + tmp109*tmp155*tmp78*tmp149; -J(5+_OFF,3+_OFF) = -(tmp109*tmp15*tmp16*tmp18) + tmp74*tmp15*tmp17*tmp18 + tmp152*tmp74*tmp78*tmp106 + tmp3*tmp15*tmp106 + tmp109*tmp152*tmp78*tmp133 - tmp5*tmp15*tmp133 + tmp152*tmp155*tmp78*tmp149; -J(6+_OFF,1+_OFF) = -(tmp152*tmp15*tmp16*tmp17) + tmp155*tmp15*tmp16*tmp18 + tmp84*tmp77*tmp78*tmp106 + tmp155*tmp77*tmp78*tmp133 + tmp4*tmp15*tmp133 + tmp152*tmp77*tmp78*tmp149 - tmp0*tmp15*tmp149; -J(6+_OFF,2+_OFF) = tmp152*tmp15*tmp16*tmp17 - tmp84*tmp15*tmp17*tmp18 + tmp109*tmp84*tmp78*tmp106 + tmp1*tmp15*tmp106 + tmp109*tmp155*tmp78*tmp133 + tmp109*tmp152*tmp78*tmp149 - tmp5*tmp15*tmp149; -J(6+_OFF,3+_OFF) = -(tmp155*tmp15*tmp16*tmp18) + tmp84*tmp15*tmp17*tmp18 + tmp152*tmp84*tmp78*tmp106 + tmp152*tmp155*tmp78*tmp133 + std::pow(tmp152,2)*tmp78*tmp149; +J(4+_OFF,1+_OFF) = -(tmp142*tmp15*tmp16*tmp17) + tmp96*tmp15*tmp16*tmp18 + std::pow(tmp99,2)*tmp100*tmp171 + tmp96*tmp99*tmp100*tmp196 + tmp142*tmp99*tmp100*tmp218; +J(4+_OFF,2+_OFF) = tmp142*tmp15*tmp16*tmp17 - tmp99*tmp15*tmp17*tmp18 + tmp174*tmp99*tmp100*tmp171 - tmp2*tmp15*tmp171 + tmp174*tmp96*tmp100*tmp196 + tmp174*tmp142*tmp100*tmp218 + tmp1*tmp15*tmp218; +J(4+_OFF,3+_OFF) = -(tmp96*tmp15*tmp16*tmp18) + tmp99*tmp15*tmp17*tmp18 + tmp221*tmp99*tmp100*tmp171 - tmp0*tmp15*tmp171 + tmp221*tmp96*tmp100*tmp196 + tmp3*tmp15*tmp196 + tmp221*tmp142*tmp100*tmp218; +J(5+_OFF,1+_OFF) = -(tmp224*tmp15*tmp16*tmp17) + tmp174*tmp15*tmp16*tmp18 + tmp96*tmp99*tmp100*tmp171 + tmp174*tmp99*tmp100*tmp196 - tmp2*tmp15*tmp196 + tmp224*tmp99*tmp100*tmp218 + tmp4*tmp15*tmp218; +J(5+_OFF,2+_OFF) = tmp224*tmp15*tmp16*tmp17 - tmp96*tmp15*tmp17*tmp18 + tmp174*tmp96*tmp100*tmp171 + std::pow(tmp174,2)*tmp100*tmp196 + tmp174*tmp224*tmp100*tmp218; +J(5+_OFF,3+_OFF) = -(tmp174*tmp15*tmp16*tmp18) + tmp96*tmp15*tmp17*tmp18 + tmp221*tmp96*tmp100*tmp171 + tmp3*tmp15*tmp171 + tmp174*tmp221*tmp100*tmp196 - tmp5*tmp15*tmp196 + tmp221*tmp224*tmp100*tmp218; +J(6+_OFF,1+_OFF) = -(tmp221*tmp15*tmp16*tmp17) + tmp224*tmp15*tmp16*tmp18 + tmp142*tmp99*tmp100*tmp171 + tmp224*tmp99*tmp100*tmp196 + tmp4*tmp15*tmp196 + tmp221*tmp99*tmp100*tmp218 - tmp0*tmp15*tmp218; +J(6+_OFF,2+_OFF) = tmp221*tmp15*tmp16*tmp17 - tmp142*tmp15*tmp17*tmp18 + tmp174*tmp142*tmp100*tmp171 + tmp1*tmp15*tmp171 + tmp174*tmp224*tmp100*tmp196 + tmp174*tmp221*tmp100*tmp218 - tmp5*tmp15*tmp218; +J(6+_OFF,3+_OFF) = -(tmp224*tmp15*tmp16*tmp18) + tmp142*tmp15*tmp17*tmp18 + tmp221*tmp142*tmp100*tmp171 + tmp221*tmp224*tmp100*tmp196 + std::pow(tmp221,2)*tmp100*tmp218; diff --git a/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrIbod.cppready b/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrIbod.cppready index 35e6169..3829b50 100644 --- a/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrIbod.cppready +++ b/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrIbod.cppready @@ -25,153 +25,222 @@ double tmp23 = x(4+_OFF); double tmp24 = x(6+_OFF); double tmp25 = x(5+_OFF); double tmp26 = x(7+_OFF); -double tmp27 = wind(1+_OFF); -double tmp28 = tmp25*tmp24; -double tmp29 = wind(3+_OFF); -double tmp30 = wind(2+_OFF); -double tmp31 = std::pow(tmp23,2); -double tmp32 = std::pow(tmp25,2); -double tmp33 = std::pow(tmp24,2); -double tmp34 = std::pow(tmp26,2); -double tmp35 = -tmp34; -double tmp36 = tmp25*tmp26; -double tmp37 = tmp24*tmp26; -double tmp38 = -tmp32; -double tmp39 = -tmp33; -double tmp40 = -(tmp23*tmp26); -double tmp41 = tmp28 + tmp40; -double tmp42 = -2*tmp27*tmp41; -double tmp43 = tmp23*tmp25; -double tmp44 = tmp43 + tmp37; -double tmp45 = -2*tmp29*tmp44; -double tmp46 = tmp31 + tmp38 + tmp33 + tmp35; -double tmp47 = -(tmp30*tmp46); -double tmp48 = x(9+_OFF); -double tmp49 = tmp42 + tmp45 + tmp47 + tmp48; -double tmp50 = tmp23*tmp26; -double tmp51 = tmp28 + tmp50; -double tmp52 = -2*tmp30*tmp51; -double tmp53 = -(tmp23*tmp24); -double tmp54 = tmp53 + tmp36; -double tmp55 = -2*tmp29*tmp54; -double tmp56 = tmp31 + tmp32 + tmp39 + tmp35; -double tmp57 = -(tmp27*tmp56); -double tmp58 = x(8+_OFF); -double tmp59 = tmp52 + tmp55 + tmp57 + tmp58; -double tmp60 = std::pow(tmp59,2); -double tmp61 = std::pow(tmp49,2); -double tmp62 = tmp23*tmp24; -double tmp63 = tmp62 + tmp36; -double tmp64 = -2*tmp27*tmp63; -double tmp65 = -(tmp23*tmp25); -double tmp66 = tmp65 + tmp37; -double tmp67 = -2*tmp30*tmp66; -double tmp68 = tmp31 + tmp38 + tmp39 + tmp34; -double tmp69 = -(tmp29*tmp68); -double tmp70 = x(10+_OFF); -double tmp71 = tmp64 + tmp67 + tmp69 + tmp70; -double tmp72 = std::pow(tmp71,2); -double tmp73 = tmp60 + tmp61 + tmp72; -double tmp74 = cp(3+_OFF); -double tmp75 = 1/std::sqrt(tmp73); -double tmp76 = -(tmp1*tmp3); -double tmp77 = tmp2*tmp4; -double tmp78 = tmp76 + tmp77; -double tmp79 = -2*tmp0*tmp2; -double tmp80 = 2*tmp3*tmp4; -double tmp81 = tmp79 + tmp80; -double tmp82 = std::pow(tmp14,-2); -double tmp83 = std::pow(tmp17,2); -double tmp84 = cp(2+_OFF); -double tmp85 = cBar(1+_OFF); -double tmp86 = tmp2*tmp83; -double tmp87 = tmp4*tmp17*tmp16; -double tmp88 = -(tmp5*tmp17*tmp18); -double tmp89 = tmp1*tmp17*tmp18; -double tmp90 = -(tmp3*tmp16*tmp18); -double tmp91 = -(tmp2*tmp22); -double tmp92 = mPitch(1+_OFF); -double tmp93 = -tmp92; -double tmp94 = 2*tmp27*tmp63; -double tmp95 = 2*tmp30*tmp66; -double tmp96 = tmp29*tmp68; -double tmp97 = -tmp70; -double tmp98 = tmp94 + tmp95 + tmp96 + tmp97; -double tmp99 = std::atan2(tmp98,tmp59); -double tmp100 = mPitch(3+_OFF); -double tmp101 = -(tmp99*tmp100); -double tmp102 = mPitch(4+_OFF); -double tmp103 = (tmp85*tmp102*tmp75*tmp16)/2.; -double tmp104 = mPitch(2+_OFF); -double tmp105 = z(3+_OFF); -double tmp106 = -(tmp104*tmp105); -double tmp107 = tmp93 + tmp101 + tmp103 + tmp106; -double tmp108 = 0.5*tmp85*tmp84*tmp73*tmp107; -double tmp109 = tmp86 + tmp87 + tmp88 + tmp89 + tmp90 + tmp91 + tmp108; -double tmp110 = -(tmp0*tmp2); -double tmp111 = tmp3*tmp4; -double tmp112 = tmp110 + tmp111; -double tmp113 = std::pow(tmp16,2); -double tmp114 = tmp49*tmp75; -double tmp115 = std::asin(tmp114); -double tmp116 = -(tmp3*tmp83); -double tmp117 = tmp5*tmp17*tmp16; -double tmp118 = -(tmp0*tmp17*tmp16); -double tmp119 = tmp3*tmp113; -double tmp120 = -(tmp4*tmp17*tmp18); -double tmp121 = tmp2*tmp16*tmp18; -double tmp122 = mYaw(2+_OFF); -double tmp123 = tmp115*tmp122; -double tmp124 = mYaw(3+_OFF); -double tmp125 = (tmp74*tmp124*tmp75*tmp18)/2.; -double tmp126 = mYaw(1+_OFF); -double tmp127 = z(4+_OFF); +double tmp27 = x(30+_OFF); +double tmp28 = x(32+_OFF); +double tmp29 = x(31+_OFF); +double tmp30 = x(33+_OFF); +double tmp31 = tmp29*tmp28; +double tmp32 = tmp27*tmp30; +double tmp33 = tmp31 + tmp32; +double tmp34 = -(tmp27*tmp28); +double tmp35 = tmp29*tmp30; +double tmp36 = tmp34 + tmp35; +double tmp37 = std::pow(tmp27,2); +double tmp38 = std::pow(tmp29,2); +double tmp39 = std::pow(tmp28,2); +double tmp40 = -tmp39; +double tmp41 = std::pow(tmp30,2); +double tmp42 = -tmp41; +double tmp43 = tmp37 + tmp38 + tmp40 + tmp42; +double tmp44 = wind(1+_OFF); +double tmp45 = wind(3+_OFF); +double tmp46 = wind(2+_OFF); +double tmp47 = tmp28*tmp30; +double tmp48 = -tmp38; +double tmp49 = -(tmp27*tmp30); +double tmp50 = tmp31 + tmp49; +double tmp51 = -2*tmp44*tmp50; +double tmp52 = tmp27*tmp29; +double tmp53 = tmp52 + tmp47; +double tmp54 = -2*tmp45*tmp53; +double tmp55 = tmp37 + tmp48 + tmp39 + tmp42; +double tmp56 = -(tmp46*tmp55); +double tmp57 = x(35+_OFF); +double tmp58 = tmp51 + tmp54 + tmp56 + tmp57; +double tmp59 = -2*tmp46*tmp33; +double tmp60 = -2*tmp45*tmp36; +double tmp61 = -(tmp44*tmp43); +double tmp62 = x(34+_OFF); +double tmp63 = tmp59 + tmp60 + tmp61 + tmp62; +double tmp64 = std::pow(tmp63,2); +double tmp65 = std::pow(tmp58,2); +double tmp66 = tmp27*tmp28; +double tmp67 = tmp66 + tmp35; +double tmp68 = -2*tmp44*tmp67; +double tmp69 = -(tmp27*tmp29); +double tmp70 = tmp69 + tmp47; +double tmp71 = -2*tmp46*tmp70; +double tmp72 = tmp37 + tmp48 + tmp40 + tmp41; +double tmp73 = -(tmp45*tmp72); +double tmp74 = x(36+_OFF); +double tmp75 = tmp68 + tmp71 + tmp73 + tmp74; +double tmp76 = std::pow(tmp75,2); +double tmp77 = tmp64 + tmp65 + tmp76; +double tmp78 = cp(3+_OFF); +double tmp79 = 1/std::sqrt(tmp77); +double tmp80 = cp(2+_OFF); +double tmp81 = std::pow(tmp23,2); +double tmp82 = std::pow(tmp25,2); +double tmp83 = std::pow(tmp24,2); +double tmp84 = -tmp83; +double tmp85 = std::pow(tmp26,2); +double tmp86 = -tmp85; +double tmp87 = tmp81 + tmp82 + tmp84 + tmp86; +double tmp88 = -(tmp23*tmp24); +double tmp89 = tmp25*tmp26; +double tmp90 = tmp88 + tmp89; +double tmp91 = tmp25*tmp24; +double tmp92 = tmp23*tmp26; +double tmp93 = tmp91 + tmp92; +double tmp94 = cBar(1+_OFF); +double tmp95 = tmp58*tmp79; +double tmp96 = std::asin(tmp95); +double tmp97 = x(39+_OFF); +double tmp98 = -(tmp1*tmp3); +double tmp99 = tmp2*tmp4; +double tmp100 = tmp98 + tmp99; +double tmp101 = -2*tmp0*tmp2; +double tmp102 = 2*tmp3*tmp4; +double tmp103 = tmp101 + tmp102; +double tmp104 = std::pow(tmp14,-2); +double tmp105 = std::pow(tmp17,2); +double tmp106 = mRoll(2+_OFF); +double tmp107 = -(tmp96*tmp106); +double tmp108 = mRoll(3+_OFF); +double tmp109 = x(37+_OFF); +double tmp110 = (tmp78*tmp108*tmp79*tmp109)/2.; +double tmp111 = mRoll(4+_OFF); +double tmp112 = -0.5*(tmp78*tmp111*tmp79*tmp97); +double tmp113 = mRoll(1+_OFF); +double tmp114 = z(2+_OFF); +double tmp115 = tmp113*tmp114; +double tmp116 = tmp107 + tmp110 + tmp112 + tmp115; +double tmp117 = -(tmp23*tmp26); +double tmp118 = tmp91 + tmp117; +double tmp119 = tmp23*tmp25; +double tmp120 = tmp24*tmp26; +double tmp121 = tmp119 + tmp120; +double tmp122 = -tmp82; +double tmp123 = tmp81 + tmp122 + tmp83 + tmp86; +double tmp124 = mPitch(1+_OFF); +double tmp125 = -tmp124; +double tmp126 = std::atan2(tmp75,tmp63); +double tmp127 = mPitch(3+_OFF); double tmp128 = tmp126*tmp127; -double tmp129 = tmp123 + tmp125 + tmp128; -double tmp130 = 0.5*tmp84*tmp74*tmp73*tmp129; -double tmp131 = tmp116 + tmp117 + tmp118 + tmp119 + tmp120 + tmp121 + tmp130; -double tmp132 = -(tmp2*tmp17*tmp16); -double tmp133 = -(tmp4*tmp113); -double tmp134 = tmp3*tmp17*tmp18; -double tmp135 = tmp0*tmp16*tmp18; -double tmp136 = -(tmp1*tmp16*tmp18); -double tmp137 = tmp4*tmp22; -double tmp138 = mRoll(2+_OFF); -double tmp139 = -(tmp115*tmp138); -double tmp140 = mRoll(3+_OFF); -double tmp141 = (tmp74*tmp140*tmp75*tmp17)/2.; -double tmp142 = mRoll(4+_OFF); -double tmp143 = -(tmp74*tmp142*tmp75*tmp18)/2.; -double tmp144 = mRoll(1+_OFF); -double tmp145 = z(2+_OFF); -double tmp146 = -(tmp144*tmp145); -double tmp147 = tmp139 + tmp141 + tmp143 + tmp146; -double tmp148 = 0.5*tmp84*tmp74*tmp73*tmp147; -double tmp149 = tmp132 + tmp133 + tmp134 + tmp135 + tmp136 + tmp137 + tmp148; -double tmp150 = -2*tmp1*tmp3; -double tmp151 = 2*tmp2*tmp4; -double tmp152 = tmp150 + tmp151; -double tmp153 = 2*tmp2*tmp3; -double tmp154 = -2*tmp5*tmp4; -double tmp155 = tmp153 + tmp154; -double tmp156 = -tmp22; -double tmp157 = tmp83 + tmp156; -double tmp158 = tmp5*tmp1; -double tmp159 = -tmp8; -double tmp160 = tmp158 + tmp159; -double tmp161 = tmp2*tmp3; -double tmp162 = -(tmp5*tmp4); -double tmp163 = tmp161 + tmp162; -double tmp164 = -(tmp3*tmp15*tmp131); -double tmp165 = -tmp83; -double tmp166 = tmp165 + tmp113; -double tmp167 = -tmp113; -double tmp168 = tmp167 + tmp22; -double tmp169 = tmp5*tmp0; -double tmp170 = -tmp10; -double tmp171 = tmp169 + tmp170; -double tmp172 = -(tmp4*tmp15*tmp149); -double tmp173 = -(tmp2*tmp15*tmp109); +double tmp129 = mPitch(4+_OFF); +double tmp130 = x(38+_OFF); +double tmp131 = (tmp94*tmp129*tmp79*tmp130)/2.; +double tmp132 = mPitch(2+_OFF); +double tmp133 = z(3+_OFF); +double tmp134 = -(tmp132*tmp133); +double tmp135 = tmp125 + tmp128 + tmp131 + tmp134; +double tmp136 = mYaw(2+_OFF); +double tmp137 = tmp96*tmp136; +double tmp138 = mYaw(3+_OFF); +double tmp139 = (tmp78*tmp138*tmp79*tmp97)/2.; +double tmp140 = mYaw(1+_OFF); +double tmp141 = z(4+_OFF); +double tmp142 = -(tmp140*tmp141); +double tmp143 = tmp137 + tmp139 + tmp142; +double tmp144 = tmp2*tmp105; +double tmp145 = tmp4*tmp17*tmp16; +double tmp146 = -(tmp5*tmp17*tmp18); +double tmp147 = tmp1*tmp17*tmp18; +double tmp148 = -(tmp3*tmp16*tmp18); +double tmp149 = -(tmp2*tmp22); +double tmp150 = 2*tmp123*tmp33; +double tmp151 = 4*tmp121*tmp36; +double tmp152 = 2*tmp118*tmp43; +double tmp153 = tmp150 + tmp151 + tmp152; +double tmp154 = 0.634*tmp80*tmp78*tmp153*tmp77*tmp116; +double tmp155 = 4*tmp118*tmp50; +double tmp156 = 4*tmp121*tmp53; +double tmp157 = tmp123*tmp55; +double tmp158 = tmp155 + tmp156 + tmp157; +double tmp159 = 0.634*tmp94*tmp80*tmp158*tmp77*tmp135; +double tmp160 = 4*tmp118*tmp67; +double tmp161 = 2*tmp123*tmp70; +double tmp162 = 2*tmp121*tmp72; +double tmp163 = tmp160 + tmp161 + tmp162; +double tmp164 = 0.634*tmp80*tmp78*tmp163*tmp77*tmp143; +double tmp165 = tmp144 + tmp145 + tmp146 + tmp147 + tmp148 + tmp149 + tmp154 + tmp159 + tmp164; +double tmp166 = -(tmp0*tmp2); +double tmp167 = tmp3*tmp4; +double tmp168 = tmp166 + tmp167; +double tmp169 = std::pow(tmp16,2); +double tmp170 = tmp23*tmp24; +double tmp171 = tmp170 + tmp89; +double tmp172 = tmp81 + tmp122 + tmp84 + tmp85; +double tmp173 = -(tmp23*tmp25); +double tmp174 = tmp173 + tmp120; +double tmp175 = -(tmp3*tmp105); +double tmp176 = tmp5*tmp17*tmp16; +double tmp177 = -(tmp0*tmp17*tmp16); +double tmp178 = tmp3*tmp169; +double tmp179 = -(tmp4*tmp17*tmp18); +double tmp180 = tmp2*tmp16*tmp18; +double tmp181 = 4*tmp174*tmp33; +double tmp182 = 2*tmp172*tmp36; +double tmp183 = 2*tmp171*tmp43; +double tmp184 = tmp181 + tmp182 + tmp183; +double tmp185 = 0.634*tmp80*tmp78*tmp184*tmp77*tmp116; +double tmp186 = 4*tmp171*tmp50; +double tmp187 = 2*tmp172*tmp53; +double tmp188 = 2*tmp174*tmp55; +double tmp189 = tmp186 + tmp187 + tmp188; +double tmp190 = 0.634*tmp94*tmp80*tmp189*tmp77*tmp135; +double tmp191 = 4*tmp171*tmp67; +double tmp192 = 4*tmp174*tmp70; +double tmp193 = tmp172*tmp72; +double tmp194 = tmp191 + tmp192 + tmp193; +double tmp195 = 0.634*tmp80*tmp78*tmp194*tmp77*tmp143; +double tmp196 = tmp175 + tmp176 + tmp177 + tmp178 + tmp179 + tmp180 + tmp185 + tmp190 + tmp195; +double tmp197 = -(tmp2*tmp17*tmp16); +double tmp198 = -(tmp4*tmp169); +double tmp199 = tmp3*tmp17*tmp18; +double tmp200 = tmp0*tmp16*tmp18; +double tmp201 = -(tmp1*tmp16*tmp18); +double tmp202 = tmp4*tmp22; +double tmp203 = 4*tmp93*tmp33; +double tmp204 = 4*tmp90*tmp36; +double tmp205 = tmp87*tmp43; +double tmp206 = tmp203 + tmp204 + tmp205; +double tmp207 = 0.634*tmp80*tmp78*tmp206*tmp77*tmp116; +double tmp208 = 2*tmp87*tmp50; +double tmp209 = 4*tmp90*tmp53; +double tmp210 = 2*tmp93*tmp55; +double tmp211 = tmp208 + tmp209 + tmp210; +double tmp212 = 0.634*tmp94*tmp80*tmp211*tmp77*tmp135; +double tmp213 = 2*tmp87*tmp67; +double tmp214 = 4*tmp93*tmp70; +double tmp215 = 2*tmp90*tmp72; +double tmp216 = tmp213 + tmp214 + tmp215; +double tmp217 = 0.634*tmp80*tmp78*tmp216*tmp77*tmp143; +double tmp218 = tmp197 + tmp198 + tmp199 + tmp200 + tmp201 + tmp202 + tmp207 + tmp212 + tmp217; +double tmp219 = -2*tmp1*tmp3; +double tmp220 = 2*tmp2*tmp4; +double tmp221 = tmp219 + tmp220; +double tmp222 = 2*tmp2*tmp3; +double tmp223 = -2*tmp5*tmp4; +double tmp224 = tmp222 + tmp223; +double tmp225 = -tmp22; +double tmp226 = tmp105 + tmp225; +double tmp227 = tmp5*tmp1; +double tmp228 = -tmp8; +double tmp229 = tmp227 + tmp228; +double tmp230 = tmp2*tmp3; +double tmp231 = -(tmp5*tmp4); +double tmp232 = tmp230 + tmp231; +double tmp233 = -(tmp3*tmp15*tmp196); +double tmp234 = -tmp105; +double tmp235 = tmp234 + tmp169; +double tmp236 = -tmp169; +double tmp237 = tmp236 + tmp22; +double tmp238 = tmp5*tmp0; +double tmp239 = -tmp10; +double tmp240 = tmp238 + tmp239; +double tmp241 = -(tmp4*tmp15*tmp218); +double tmp242 = -(tmp2*tmp15*tmp165); J(1+_OFF,1+_OFF) = 0; J(1+_OFF,2+_OFF) = 0; J(1+_OFF,3+_OFF) = 0; @@ -181,12 +250,12 @@ J(2+_OFF,3+_OFF) = 0; J(3+_OFF,1+_OFF) = 0; J(3+_OFF,2+_OFF) = 0; J(3+_OFF,3+_OFF) = 0; -J(4+_OFF,1+_OFF) = tmp21*tmp15*tmp17*tmp16 - tmp112*tmp15*tmp16*tmp18 - tmp78*tmp15*tmp157 + tmp81*tmp21*tmp82*tmp149 + tmp78*tmp81*tmp82*tmp109 - tmp4*tmp15*tmp109 + tmp112*tmp81*tmp82*tmp131 + tmp0*tmp15*tmp131; -J(4+_OFF,2+_OFF) = -(tmp112*tmp15*tmp166) - tmp21*tmp15*tmp17*tmp18 + tmp78*tmp15*tmp16*tmp18 + tmp152*tmp21*tmp82*tmp149 + tmp78*tmp152*tmp82*tmp109 + tmp1*tmp15*tmp109 + tmp152*tmp112*tmp82*tmp131 - tmp4*tmp15*tmp131; -J(4+_OFF,3+_OFF) = -(tmp78*tmp15*tmp17*tmp16) + tmp112*tmp15*tmp17*tmp18 - tmp21*tmp15*tmp168 + tmp155*tmp21*tmp82*tmp149 + 2*tmp4*tmp15*tmp149 + tmp155*tmp78*tmp82*tmp109 + tmp173 + tmp155*tmp112*tmp82*tmp131 + tmp164; -J(5+_OFF,1+_OFF) = tmp78*tmp15*tmp17*tmp16 - tmp163*tmp15*tmp16*tmp18 - tmp160*tmp15*tmp157 + tmp78*tmp81*tmp82*tmp149 + tmp172 + tmp160*tmp81*tmp82*tmp109 + 2*tmp2*tmp15*tmp109 + tmp163*tmp81*tmp82*tmp131 + tmp164; -J(5+_OFF,2+_OFF) = -(tmp163*tmp15*tmp166) - tmp78*tmp15*tmp17*tmp18 + tmp160*tmp15*tmp16*tmp18 + tmp78*tmp152*tmp82*tmp149 + tmp1*tmp15*tmp149 + tmp160*tmp152*tmp82*tmp109 + tmp163*tmp152*tmp82*tmp131 - tmp2*tmp15*tmp131; -J(5+_OFF,3+_OFF) = -(tmp160*tmp15*tmp17*tmp16) + tmp163*tmp15*tmp17*tmp18 - tmp78*tmp15*tmp168 + tmp155*tmp78*tmp82*tmp149 - tmp2*tmp15*tmp149 + tmp160*tmp155*tmp82*tmp109 + tmp155*tmp163*tmp82*tmp131 + tmp5*tmp15*tmp131; -J(6+_OFF,1+_OFF) = tmp112*tmp15*tmp17*tmp16 - tmp171*tmp15*tmp16*tmp18 - tmp163*tmp15*tmp157 + tmp112*tmp81*tmp82*tmp149 + tmp0*tmp15*tmp149 + tmp163*tmp81*tmp82*tmp109 - tmp3*tmp15*tmp109 + tmp171*tmp81*tmp82*tmp131; -J(6+_OFF,2+_OFF) = -(tmp171*tmp15*tmp166) - tmp112*tmp15*tmp17*tmp18 + tmp163*tmp15*tmp16*tmp18 + tmp152*tmp112*tmp82*tmp149 + tmp172 + tmp163*tmp152*tmp82*tmp109 + tmp173 + tmp171*tmp152*tmp82*tmp131 + 2*tmp3*tmp15*tmp131; -J(6+_OFF,3+_OFF) = -(tmp163*tmp15*tmp17*tmp16) + tmp171*tmp15*tmp17*tmp18 - tmp112*tmp15*tmp168 + tmp155*tmp112*tmp82*tmp149 - tmp3*tmp15*tmp149 + tmp155*tmp163*tmp82*tmp109 + tmp5*tmp15*tmp109 + tmp171*tmp155*tmp82*tmp131; +J(4+_OFF,1+_OFF) = tmp21*tmp15*tmp17*tmp16 - tmp168*tmp15*tmp16*tmp18 - tmp100*tmp15*tmp226 + tmp103*tmp21*tmp104*tmp218 + tmp100*tmp103*tmp104*tmp165 - tmp4*tmp15*tmp165 + tmp168*tmp103*tmp104*tmp196 + tmp0*tmp15*tmp196; +J(4+_OFF,2+_OFF) = -(tmp168*tmp15*tmp235) - tmp21*tmp15*tmp17*tmp18 + tmp100*tmp15*tmp16*tmp18 + tmp221*tmp21*tmp104*tmp218 + tmp100*tmp221*tmp104*tmp165 + tmp1*tmp15*tmp165 + tmp221*tmp168*tmp104*tmp196 - tmp4*tmp15*tmp196; +J(4+_OFF,3+_OFF) = -(tmp100*tmp15*tmp17*tmp16) + tmp168*tmp15*tmp17*tmp18 - tmp21*tmp15*tmp237 + tmp224*tmp21*tmp104*tmp218 + 2*tmp4*tmp15*tmp218 + tmp224*tmp100*tmp104*tmp165 + tmp242 + tmp224*tmp168*tmp104*tmp196 + tmp233; +J(5+_OFF,1+_OFF) = tmp100*tmp15*tmp17*tmp16 - tmp232*tmp15*tmp16*tmp18 - tmp229*tmp15*tmp226 + tmp100*tmp103*tmp104*tmp218 + tmp241 + tmp229*tmp103*tmp104*tmp165 + 2*tmp2*tmp15*tmp165 + tmp232*tmp103*tmp104*tmp196 + tmp233; +J(5+_OFF,2+_OFF) = -(tmp232*tmp15*tmp235) - tmp100*tmp15*tmp17*tmp18 + tmp229*tmp15*tmp16*tmp18 + tmp100*tmp221*tmp104*tmp218 + tmp1*tmp15*tmp218 + tmp229*tmp221*tmp104*tmp165 + tmp232*tmp221*tmp104*tmp196 - tmp2*tmp15*tmp196; +J(5+_OFF,3+_OFF) = -(tmp229*tmp15*tmp17*tmp16) + tmp232*tmp15*tmp17*tmp18 - tmp100*tmp15*tmp237 + tmp224*tmp100*tmp104*tmp218 - tmp2*tmp15*tmp218 + tmp229*tmp224*tmp104*tmp165 + tmp224*tmp232*tmp104*tmp196 + tmp5*tmp15*tmp196; +J(6+_OFF,1+_OFF) = tmp168*tmp15*tmp17*tmp16 - tmp240*tmp15*tmp16*tmp18 - tmp232*tmp15*tmp226 + tmp168*tmp103*tmp104*tmp218 + tmp0*tmp15*tmp218 + tmp232*tmp103*tmp104*tmp165 - tmp3*tmp15*tmp165 + tmp240*tmp103*tmp104*tmp196; +J(6+_OFF,2+_OFF) = -(tmp240*tmp15*tmp235) - tmp168*tmp15*tmp17*tmp18 + tmp232*tmp15*tmp16*tmp18 + tmp221*tmp168*tmp104*tmp218 + tmp241 + tmp232*tmp221*tmp104*tmp165 + tmp242 + tmp240*tmp221*tmp104*tmp196 + 2*tmp3*tmp15*tmp196; +J(6+_OFF,3+_OFF) = -(tmp232*tmp15*tmp17*tmp16) + tmp240*tmp15*tmp17*tmp18 - tmp168*tmp15*tmp237 + tmp224*tmp168*tmp104*tmp218 - tmp3*tmp15*tmp218 + tmp224*tmp232*tmp104*tmp165 + tmp5*tmp15*tmp165 + tmp240*tmp224*tmp104*tmp196; diff --git a/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrMPitch.cppready b/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrMPitch.cppready index d2e2fe6..fe71882 100644 --- a/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrMPitch.cppready +++ b/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrMPitch.cppready @@ -1,91 +1,136 @@ -double tmp0 = ibd(3+_OFF); -double tmp1 = ibd(2+_OFF); +double tmp0 = ibd(2+_OFF); +double tmp1 = ibd(3+_OFF); double tmp2 = ibod(1+_OFF); double tmp3 = ibod(2+_OFF); double tmp4 = ibod(3+_OFF); double tmp5 = ibd(1+_OFF); -double tmp6 = x(4+_OFF); -double tmp7 = x(6+_OFF); -double tmp8 = x(5+_OFF); -double tmp9 = x(7+_OFF); -double tmp10 = wind(1+_OFF); -double tmp11 = tmp8*tmp7; -double tmp12 = wind(3+_OFF); -double tmp13 = wind(2+_OFF); -double tmp14 = std::pow(tmp6,2); -double tmp15 = std::pow(tmp8,2); -double tmp16 = std::pow(tmp7,2); -double tmp17 = std::pow(tmp9,2); -double tmp18 = -tmp17; -double tmp19 = tmp8*tmp9; -double tmp20 = tmp7*tmp9; -double tmp21 = -tmp15; -double tmp22 = -tmp16; -double tmp23 = cBar(1+_OFF); -double tmp24 = cp(2+_OFF); -double tmp25 = -(tmp0*tmp3); -double tmp26 = tmp2*tmp4; +double tmp6 = std::pow(tmp4,2); +double tmp7 = x(4+_OFF); +double tmp8 = x(6+_OFF); +double tmp9 = x(5+_OFF); +double tmp10 = x(7+_OFF); +double tmp11 = x(30+_OFF); +double tmp12 = x(31+_OFF); +double tmp13 = x(32+_OFF); +double tmp14 = x(33+_OFF); +double tmp15 = tmp12*tmp13; +double tmp16 = std::pow(tmp11,2); +double tmp17 = std::pow(tmp12,2); +double tmp18 = std::pow(tmp13,2); +double tmp19 = std::pow(tmp14,2); +double tmp20 = -tmp19; +double tmp21 = wind(1+_OFF); +double tmp22 = -(tmp11*tmp14); +double tmp23 = tmp15 + tmp22; +double tmp24 = wind(3+_OFF); +double tmp25 = tmp11*tmp12; +double tmp26 = tmp13*tmp14; double tmp27 = tmp25 + tmp26; -double tmp28 = tmp5*tmp1*tmp0; -double tmp29 = std::pow(tmp2,2); -double tmp30 = -(tmp1*tmp29); -double tmp31 = std::pow(tmp3,2); -double tmp32 = -(tmp0*tmp31); -double tmp33 = 2*tmp2*tmp3*tmp4; -double tmp34 = std::pow(tmp4,2); -double tmp35 = -(tmp5*tmp34); -double tmp36 = tmp28 + tmp30 + tmp32 + tmp33 + tmp35; -double tmp37 = 1/tmp36; -double tmp38 = tmp6*tmp9; -double tmp39 = tmp11 + tmp38; -double tmp40 = -2*tmp13*tmp39; -double tmp41 = -(tmp6*tmp7); -double tmp42 = tmp41 + tmp19; -double tmp43 = -2*tmp12*tmp42; -double tmp44 = tmp14 + tmp15 + tmp22 + tmp18; -double tmp45 = -(tmp10*tmp44); -double tmp46 = x(8+_OFF); -double tmp47 = tmp40 + tmp43 + tmp45 + tmp46; -double tmp48 = std::pow(tmp47,2); -double tmp49 = -(tmp6*tmp9); -double tmp50 = tmp11 + tmp49; -double tmp51 = -2*tmp10*tmp50; -double tmp52 = tmp6*tmp8; -double tmp53 = tmp52 + tmp20; -double tmp54 = -2*tmp12*tmp53; -double tmp55 = tmp14 + tmp21 + tmp16 + tmp18; -double tmp56 = -(tmp13*tmp55); -double tmp57 = x(9+_OFF); -double tmp58 = tmp51 + tmp54 + tmp56 + tmp57; -double tmp59 = std::pow(tmp58,2); -double tmp60 = tmp6*tmp7; -double tmp61 = tmp60 + tmp19; -double tmp62 = -2*tmp10*tmp61; -double tmp63 = -(tmp6*tmp8); -double tmp64 = tmp63 + tmp20; -double tmp65 = -2*tmp13*tmp64; -double tmp66 = tmp14 + tmp21 + tmp22 + tmp17; -double tmp67 = -(tmp12*tmp66); -double tmp68 = x(10+_OFF); -double tmp69 = tmp62 + tmp65 + tmp67 + tmp68; -double tmp70 = std::pow(tmp69,2); -double tmp71 = tmp48 + tmp59 + tmp70; -double tmp72 = tmp5*tmp0; -double tmp73 = -tmp29; -double tmp74 = tmp72 + tmp73; -double tmp75 = z(3+_OFF); -double tmp76 = 2*tmp10*tmp61; -double tmp77 = 2*tmp13*tmp64; -double tmp78 = tmp12*tmp66; -double tmp79 = -tmp68; -double tmp80 = tmp76 + tmp77 + tmp78 + tmp79; -double tmp81 = std::atan2(tmp80,tmp47); -double tmp82 = std::pow(tmp23,2); -double tmp83 = std::sqrt(tmp71); -double tmp84 = x(12+_OFF); -double tmp85 = tmp2*tmp3; -double tmp86 = -(tmp5*tmp4); -double tmp87 = tmp85 + tmp86; +double tmp28 = wind(2+_OFF); +double tmp29 = -tmp17; +double tmp30 = tmp16 + tmp29 + tmp18 + tmp20; +double tmp31 = tmp12*tmp14; +double tmp32 = -tmp18; +double tmp33 = cBar(1+_OFF); +double tmp34 = cp(2+_OFF); +double tmp35 = tmp5*tmp0*tmp1; +double tmp36 = std::pow(tmp2,2); +double tmp37 = -(tmp0*tmp36); +double tmp38 = std::pow(tmp3,2); +double tmp39 = -(tmp1*tmp38); +double tmp40 = 2*tmp2*tmp3*tmp4; +double tmp41 = -(tmp5*tmp6); +double tmp42 = tmp35 + tmp37 + tmp39 + tmp40 + tmp41; +double tmp43 = 1/tmp42; +double tmp44 = tmp9*tmp10; +double tmp45 = std::pow(tmp7,2); +double tmp46 = std::pow(tmp9,2); +double tmp47 = std::pow(tmp8,2); +double tmp48 = -tmp47; +double tmp49 = std::pow(tmp10,2); +double tmp50 = tmp11*tmp14; +double tmp51 = tmp15 + tmp50; +double tmp52 = -2*tmp28*tmp51; +double tmp53 = -(tmp11*tmp13); +double tmp54 = tmp53 + tmp31; +double tmp55 = -2*tmp24*tmp54; +double tmp56 = tmp16 + tmp17 + tmp32 + tmp20; +double tmp57 = -(tmp21*tmp56); +double tmp58 = x(34+_OFF); +double tmp59 = tmp52 + tmp55 + tmp57 + tmp58; +double tmp60 = std::pow(tmp59,2); +double tmp61 = -2*tmp21*tmp23; +double tmp62 = -2*tmp24*tmp27; +double tmp63 = -(tmp28*tmp30); +double tmp64 = x(35+_OFF); +double tmp65 = tmp61 + tmp62 + tmp63 + tmp64; +double tmp66 = std::pow(tmp65,2); +double tmp67 = tmp11*tmp13; +double tmp68 = tmp67 + tmp31; +double tmp69 = -2*tmp21*tmp68; +double tmp70 = -(tmp11*tmp12); +double tmp71 = tmp70 + tmp26; +double tmp72 = -2*tmp28*tmp71; +double tmp73 = tmp16 + tmp29 + tmp32 + tmp19; +double tmp74 = -(tmp24*tmp73); +double tmp75 = x(36+_OFF); +double tmp76 = tmp69 + tmp72 + tmp74 + tmp75; +double tmp77 = std::pow(tmp76,2); +double tmp78 = tmp60 + tmp66 + tmp77; +double tmp79 = tmp9*tmp8; +double tmp80 = tmp8*tmp10; +double tmp81 = -tmp46; +double tmp82 = -tmp49; +double tmp83 = tmp0*tmp1; +double tmp84 = -tmp6; +double tmp85 = tmp83 + tmp84; +double tmp86 = tmp45 + tmp46 + tmp48 + tmp82; +double tmp87 = 2*tmp86*tmp23; +double tmp88 = -(tmp7*tmp8); +double tmp89 = tmp88 + tmp44; +double tmp90 = 4*tmp89*tmp27; +double tmp91 = tmp7*tmp10; +double tmp92 = tmp79 + tmp91; +double tmp93 = 2*tmp92*tmp30; +double tmp94 = tmp87 + tmp90 + tmp93; +double tmp95 = -(tmp0*tmp2); +double tmp96 = tmp3*tmp4; +double tmp97 = tmp95 + tmp96; +double tmp98 = tmp7*tmp8; +double tmp99 = tmp98 + tmp44; +double tmp100 = 4*tmp99*tmp23; +double tmp101 = tmp45 + tmp81 + tmp48 + tmp49; +double tmp102 = 2*tmp101*tmp27; +double tmp103 = -(tmp7*tmp9); +double tmp104 = tmp103 + tmp80; +double tmp105 = 2*tmp104*tmp30; +double tmp106 = tmp100 + tmp102 + tmp105; +double tmp107 = z(3+_OFF); +double tmp108 = -(tmp1*tmp3); +double tmp109 = tmp2*tmp4; +double tmp110 = tmp108 + tmp109; +double tmp111 = -(tmp7*tmp10); +double tmp112 = tmp79 + tmp111; +double tmp113 = 4*tmp112*tmp23; +double tmp114 = tmp7*tmp9; +double tmp115 = tmp114 + tmp80; +double tmp116 = 4*tmp115*tmp27; +double tmp117 = tmp45 + tmp81 + tmp47 + tmp82; +double tmp118 = tmp117*tmp30; +double tmp119 = tmp113 + tmp116 + tmp118; +double tmp120 = std::atan2(tmp76,tmp59); +double tmp121 = std::pow(tmp33,2); +double tmp122 = std::sqrt(tmp78); +double tmp123 = x(38+_OFF); +double tmp124 = tmp2*tmp3; +double tmp125 = -(tmp5*tmp4); +double tmp126 = tmp124 + tmp125; +double tmp127 = tmp5*tmp1; +double tmp128 = -tmp36; +double tmp129 = tmp127 + tmp128; +double tmp130 = tmp5*tmp0; +double tmp131 = -tmp38; +double tmp132 = tmp130 + tmp131; J(1+_OFF,1+_OFF) = 0; J(1+_OFF,2+_OFF) = 0; J(1+_OFF,3+_OFF) = 0; @@ -98,15 +143,15 @@ J(3+_OFF,1+_OFF) = 0; J(3+_OFF,2+_OFF) = 0; J(3+_OFF,3+_OFF) = 0; J(3+_OFF,4+_OFF) = 0; -J(4+_OFF,1+_OFF) = 0.5*tmp23*tmp24*tmp27*tmp37*tmp71; -J(4+_OFF,2+_OFF) = 0.5*tmp23*tmp24*tmp27*tmp37*tmp71*tmp75; -J(4+_OFF,3+_OFF) = 0.5*tmp81*tmp23*tmp24*tmp27*tmp37*tmp71; -J(4+_OFF,4+_OFF) = -0.25*tmp82*tmp24*tmp27*tmp37*tmp83*tmp84; -J(5+_OFF,1+_OFF) = 0.5*tmp23*tmp24*tmp74*tmp37*tmp71; -J(5+_OFF,2+_OFF) = 0.5*tmp23*tmp24*tmp74*tmp37*tmp71*tmp75; -J(5+_OFF,3+_OFF) = 0.5*tmp81*tmp23*tmp24*tmp74*tmp37*tmp71; -J(5+_OFF,4+_OFF) = -0.25*tmp82*tmp24*tmp74*tmp37*tmp83*tmp84; -J(6+_OFF,1+_OFF) = 0.5*tmp23*tmp24*tmp87*tmp37*tmp71; -J(6+_OFF,2+_OFF) = 0.5*tmp23*tmp24*tmp87*tmp37*tmp71*tmp75; -J(6+_OFF,3+_OFF) = 0.5*tmp81*tmp23*tmp24*tmp87*tmp37*tmp71; -J(6+_OFF,4+_OFF) = -0.25*tmp82*tmp24*tmp87*tmp37*tmp83*tmp84; +J(4+_OFF,1+_OFF) = 0.634*tmp33*tmp34*tmp85*tmp43*tmp94*tmp78 + 0.634*tmp33*tmp34*tmp97*tmp43*tmp106*tmp78 + 0.634*tmp33*tmp34*tmp110*tmp43*tmp119*tmp78; +J(4+_OFF,2+_OFF) = 0.634*tmp33*tmp34*tmp85*tmp43*tmp94*tmp78*tmp107 + 0.634*tmp33*tmp34*tmp97*tmp43*tmp106*tmp78*tmp107 + 0.634*tmp33*tmp34*tmp110*tmp43*tmp119*tmp78*tmp107; +J(4+_OFF,3+_OFF) = -0.634*tmp120*tmp33*tmp34*tmp85*tmp43*tmp94*tmp78 - 0.634*tmp120*tmp33*tmp34*tmp97*tmp43*tmp106*tmp78 - 0.634*tmp120*tmp33*tmp34*tmp110*tmp43*tmp119*tmp78; +J(4+_OFF,4+_OFF) = -0.317*tmp121*tmp34*tmp85*tmp43*tmp94*tmp122*tmp123 - 0.317*tmp121*tmp34*tmp97*tmp43*tmp106*tmp122*tmp123 - 0.317*tmp121*tmp34*tmp110*tmp43*tmp119*tmp122*tmp123; +J(5+_OFF,1+_OFF) = 0.634*tmp33*tmp34*tmp110*tmp43*tmp94*tmp78 + 0.634*tmp33*tmp34*tmp126*tmp43*tmp106*tmp78 + 0.634*tmp33*tmp34*tmp129*tmp43*tmp119*tmp78; +J(5+_OFF,2+_OFF) = 0.634*tmp33*tmp34*tmp110*tmp43*tmp94*tmp78*tmp107 + 0.634*tmp33*tmp34*tmp126*tmp43*tmp106*tmp78*tmp107 + 0.634*tmp33*tmp34*tmp129*tmp43*tmp119*tmp78*tmp107; +J(5+_OFF,3+_OFF) = -0.634*tmp120*tmp33*tmp34*tmp110*tmp43*tmp94*tmp78 - 0.634*tmp120*tmp33*tmp34*tmp126*tmp43*tmp106*tmp78 - 0.634*tmp120*tmp33*tmp34*tmp129*tmp43*tmp119*tmp78; +J(5+_OFF,4+_OFF) = -0.317*tmp121*tmp34*tmp110*tmp43*tmp94*tmp122*tmp123 - 0.317*tmp121*tmp34*tmp126*tmp43*tmp106*tmp122*tmp123 - 0.317*tmp121*tmp34*tmp129*tmp43*tmp119*tmp122*tmp123; +J(6+_OFF,1+_OFF) = 0.634*tmp33*tmp34*tmp97*tmp43*tmp94*tmp78 + 0.634*tmp33*tmp34*tmp132*tmp43*tmp106*tmp78 + 0.634*tmp33*tmp34*tmp126*tmp43*tmp119*tmp78; +J(6+_OFF,2+_OFF) = 0.634*tmp33*tmp34*tmp97*tmp43*tmp94*tmp78*tmp107 + 0.634*tmp33*tmp34*tmp132*tmp43*tmp106*tmp78*tmp107 + 0.634*tmp33*tmp34*tmp126*tmp43*tmp119*tmp78*tmp107; +J(6+_OFF,3+_OFF) = -0.634*tmp120*tmp33*tmp34*tmp97*tmp43*tmp94*tmp78 - 0.634*tmp120*tmp33*tmp34*tmp132*tmp43*tmp106*tmp78 - 0.634*tmp120*tmp33*tmp34*tmp126*tmp43*tmp119*tmp78; +J(6+_OFF,4+_OFF) = -0.317*tmp121*tmp34*tmp97*tmp43*tmp94*tmp122*tmp123 - 0.317*tmp121*tmp34*tmp132*tmp43*tmp106*tmp122*tmp123 - 0.317*tmp121*tmp34*tmp126*tmp43*tmp119*tmp122*tmp123; diff --git a/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrMRoll.cppready b/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrMRoll.cppready index 490fca7..ca64a1a 100644 --- a/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrMRoll.cppready +++ b/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrMRoll.cppready @@ -1,89 +1,139 @@ -double tmp0 = ibd(2+_OFF); -double tmp1 = ibd(3+_OFF); +double tmp0 = ibd(3+_OFF); +double tmp1 = ibd(2+_OFF); double tmp2 = ibod(1+_OFF); double tmp3 = ibod(2+_OFF); double tmp4 = ibod(3+_OFF); double tmp5 = ibd(1+_OFF); -double tmp6 = std::pow(tmp4,2); -double tmp7 = x(4+_OFF); +double tmp6 = x(4+_OFF); +double tmp7 = x(5+_OFF); double tmp8 = x(6+_OFF); -double tmp9 = x(5+_OFF); -double tmp10 = x(7+_OFF); -double tmp11 = wind(1+_OFF); -double tmp12 = tmp9*tmp8; -double tmp13 = wind(3+_OFF); -double tmp14 = wind(2+_OFF); -double tmp15 = std::pow(tmp7,2); -double tmp16 = std::pow(tmp9,2); -double tmp17 = std::pow(tmp8,2); -double tmp18 = std::pow(tmp10,2); -double tmp19 = -tmp18; -double tmp20 = tmp9*tmp10; -double tmp21 = tmp8*tmp10; -double tmp22 = -tmp16; -double tmp23 = -tmp17; -double tmp24 = -(tmp7*tmp10); -double tmp25 = tmp12 + tmp24; -double tmp26 = -2*tmp11*tmp25; -double tmp27 = tmp7*tmp9; -double tmp28 = tmp27 + tmp21; -double tmp29 = -2*tmp13*tmp28; -double tmp30 = tmp15 + tmp22 + tmp17 + tmp19; -double tmp31 = -(tmp14*tmp30); -double tmp32 = x(9+_OFF); -double tmp33 = tmp26 + tmp29 + tmp31 + tmp32; -double tmp34 = tmp7*tmp10; -double tmp35 = tmp12 + tmp34; -double tmp36 = -2*tmp14*tmp35; -double tmp37 = -(tmp7*tmp8); -double tmp38 = tmp37 + tmp20; -double tmp39 = -2*tmp13*tmp38; -double tmp40 = tmp15 + tmp16 + tmp23 + tmp19; -double tmp41 = -(tmp11*tmp40); -double tmp42 = x(8+_OFF); -double tmp43 = tmp36 + tmp39 + tmp41 + tmp42; -double tmp44 = std::pow(tmp43,2); -double tmp45 = std::pow(tmp33,2); -double tmp46 = tmp7*tmp8; -double tmp47 = tmp46 + tmp20; -double tmp48 = -2*tmp11*tmp47; -double tmp49 = -(tmp7*tmp9); -double tmp50 = tmp49 + tmp21; -double tmp51 = -2*tmp14*tmp50; -double tmp52 = tmp15 + tmp22 + tmp23 + tmp18; -double tmp53 = -(tmp13*tmp52); -double tmp54 = x(10+_OFF); -double tmp55 = tmp48 + tmp51 + tmp53 + tmp54; -double tmp56 = std::pow(tmp55,2); -double tmp57 = tmp44 + tmp45 + tmp56; -double tmp58 = cp(2+_OFF); -double tmp59 = cp(3+_OFF); -double tmp60 = tmp0*tmp1; -double tmp61 = -tmp6; -double tmp62 = tmp60 + tmp61; -double tmp63 = tmp5*tmp0*tmp1; -double tmp64 = std::pow(tmp2,2); -double tmp65 = -(tmp0*tmp64); -double tmp66 = std::pow(tmp3,2); -double tmp67 = -(tmp1*tmp66); -double tmp68 = 2*tmp2*tmp3*tmp4; -double tmp69 = -(tmp5*tmp6); -double tmp70 = tmp63 + tmp65 + tmp67 + tmp68 + tmp69; -double tmp71 = 1/tmp70; -double tmp72 = std::pow(tmp59,2); -double tmp73 = std::sqrt(tmp57); -double tmp74 = z(2+_OFF); -double tmp75 = 1/std::sqrt(tmp57); -double tmp76 = tmp33*tmp75; -double tmp77 = std::asin(tmp76); -double tmp78 = -(tmp1*tmp3); -double tmp79 = tmp2*tmp4; -double tmp80 = tmp78 + tmp79; -double tmp81 = x(11+_OFF); -double tmp82 = x(13+_OFF); -double tmp83 = -(tmp0*tmp2); -double tmp84 = tmp3*tmp4; -double tmp85 = tmp83 + tmp84; +double tmp9 = x(7+_OFF); +double tmp10 = x(30+_OFF); +double tmp11 = x(32+_OFF); +double tmp12 = x(31+_OFF); +double tmp13 = x(33+_OFF); +double tmp14 = tmp12*tmp11; +double tmp15 = tmp10*tmp13; +double tmp16 = tmp14 + tmp15; +double tmp17 = -(tmp10*tmp11); +double tmp18 = tmp12*tmp13; +double tmp19 = tmp17 + tmp18; +double tmp20 = std::pow(tmp10,2); +double tmp21 = std::pow(tmp12,2); +double tmp22 = std::pow(tmp11,2); +double tmp23 = -tmp22; +double tmp24 = std::pow(tmp13,2); +double tmp25 = -tmp24; +double tmp26 = tmp20 + tmp21 + tmp23 + tmp25; +double tmp27 = wind(1+_OFF); +double tmp28 = wind(3+_OFF); +double tmp29 = wind(2+_OFF); +double tmp30 = tmp11*tmp13; +double tmp31 = -tmp21; +double tmp32 = cp(2+_OFF); +double tmp33 = cp(3+_OFF); +double tmp34 = tmp5*tmp1*tmp0; +double tmp35 = std::pow(tmp2,2); +double tmp36 = -(tmp1*tmp35); +double tmp37 = std::pow(tmp3,2); +double tmp38 = -(tmp0*tmp37); +double tmp39 = 2*tmp2*tmp3*tmp4; +double tmp40 = std::pow(tmp4,2); +double tmp41 = -(tmp5*tmp40); +double tmp42 = tmp34 + tmp36 + tmp38 + tmp39 + tmp41; +double tmp43 = 1/tmp42; +double tmp44 = tmp8*tmp9; +double tmp45 = std::pow(tmp6,2); +double tmp46 = std::pow(tmp7,2); +double tmp47 = -tmp46; +double tmp48 = std::pow(tmp8,2); +double tmp49 = std::pow(tmp9,2); +double tmp50 = -2*tmp29*tmp16; +double tmp51 = -2*tmp28*tmp19; +double tmp52 = -(tmp27*tmp26); +double tmp53 = x(34+_OFF); +double tmp54 = tmp50 + tmp51 + tmp52 + tmp53; +double tmp55 = std::pow(tmp54,2); +double tmp56 = -(tmp10*tmp13); +double tmp57 = tmp14 + tmp56; +double tmp58 = -2*tmp27*tmp57; +double tmp59 = tmp10*tmp12; +double tmp60 = tmp59 + tmp30; +double tmp61 = -2*tmp28*tmp60; +double tmp62 = tmp20 + tmp31 + tmp22 + tmp25; +double tmp63 = -(tmp29*tmp62); +double tmp64 = x(35+_OFF); +double tmp65 = tmp58 + tmp61 + tmp63 + tmp64; +double tmp66 = std::pow(tmp65,2); +double tmp67 = tmp10*tmp11; +double tmp68 = tmp67 + tmp18; +double tmp69 = -2*tmp27*tmp68; +double tmp70 = -(tmp10*tmp12); +double tmp71 = tmp70 + tmp30; +double tmp72 = -2*tmp29*tmp71; +double tmp73 = tmp20 + tmp31 + tmp23 + tmp24; +double tmp74 = -(tmp28*tmp73); +double tmp75 = x(36+_OFF); +double tmp76 = tmp69 + tmp72 + tmp74 + tmp75; +double tmp77 = std::pow(tmp76,2); +double tmp78 = tmp55 + tmp66 + tmp77; +double tmp79 = z(2+_OFF); +double tmp80 = tmp7*tmp8; +double tmp81 = tmp7*tmp9; +double tmp82 = -tmp48; +double tmp83 = -tmp49; +double tmp84 = -(tmp0*tmp3); +double tmp85 = tmp2*tmp4; +double tmp86 = tmp84 + tmp85; +double tmp87 = tmp45 + tmp47 + tmp48 + tmp83; +double tmp88 = 2*tmp87*tmp16; +double tmp89 = tmp6*tmp7; +double tmp90 = tmp89 + tmp44; +double tmp91 = 4*tmp90*tmp19; +double tmp92 = -(tmp6*tmp9); +double tmp93 = tmp80 + tmp92; +double tmp94 = 2*tmp93*tmp26; +double tmp95 = tmp88 + tmp91 + tmp94; +double tmp96 = 1/std::sqrt(tmp78); +double tmp97 = tmp65*tmp96; +double tmp98 = std::asin(tmp97); +double tmp99 = -(tmp1*tmp2); +double tmp100 = tmp3*tmp4; +double tmp101 = tmp99 + tmp100; +double tmp102 = -(tmp6*tmp7); +double tmp103 = tmp102 + tmp44; +double tmp104 = 4*tmp103*tmp16; +double tmp105 = tmp45 + tmp47 + tmp82 + tmp49; +double tmp106 = 2*tmp105*tmp19; +double tmp107 = tmp6*tmp8; +double tmp108 = tmp107 + tmp81; +double tmp109 = 2*tmp108*tmp26; +double tmp110 = tmp104 + tmp106 + tmp109; +double tmp111 = tmp1*tmp0; +double tmp112 = -tmp40; +double tmp113 = tmp111 + tmp112; +double tmp114 = tmp6*tmp9; +double tmp115 = tmp80 + tmp114; +double tmp116 = 4*tmp115*tmp16; +double tmp117 = -(tmp6*tmp8); +double tmp118 = tmp117 + tmp81; +double tmp119 = 4*tmp118*tmp19; +double tmp120 = tmp45 + tmp46 + tmp82 + tmp83; +double tmp121 = tmp120*tmp26; +double tmp122 = tmp116 + tmp119 + tmp121; +double tmp123 = std::pow(tmp33,2); +double tmp124 = std::sqrt(tmp78); +double tmp125 = x(37+_OFF); +double tmp126 = x(39+_OFF); +double tmp127 = tmp5*tmp0; +double tmp128 = -tmp35; +double tmp129 = tmp127 + tmp128; +double tmp130 = tmp2*tmp3; +double tmp131 = -(tmp5*tmp4); +double tmp132 = tmp130 + tmp131; +double tmp133 = tmp5*tmp1; +double tmp134 = -tmp37; +double tmp135 = tmp133 + tmp134; J(1+_OFF,1+_OFF) = 0; J(1+_OFF,2+_OFF) = 0; J(1+_OFF,3+_OFF) = 0; @@ -96,15 +146,15 @@ J(3+_OFF,1+_OFF) = 0; J(3+_OFF,2+_OFF) = 0; J(3+_OFF,3+_OFF) = 0; J(3+_OFF,4+_OFF) = 0; -J(4+_OFF,1+_OFF) = 0.5*tmp58*tmp59*tmp62*tmp71*tmp57*tmp74; -J(4+_OFF,2+_OFF) = 0.5*tmp77*tmp58*tmp59*tmp62*tmp71*tmp57; -J(4+_OFF,3+_OFF) = -0.25*tmp58*tmp72*tmp62*tmp71*tmp73*tmp81; -J(4+_OFF,4+_OFF) = 0.25*tmp58*tmp72*tmp62*tmp71*tmp73*tmp82; -J(5+_OFF,1+_OFF) = 0.5*tmp58*tmp59*tmp80*tmp71*tmp57*tmp74; -J(5+_OFF,2+_OFF) = 0.5*tmp77*tmp58*tmp59*tmp80*tmp71*tmp57; -J(5+_OFF,3+_OFF) = -0.25*tmp58*tmp72*tmp80*tmp71*tmp73*tmp81; -J(5+_OFF,4+_OFF) = 0.25*tmp58*tmp72*tmp80*tmp71*tmp73*tmp82; -J(6+_OFF,1+_OFF) = 0.5*tmp58*tmp59*tmp85*tmp71*tmp57*tmp74; -J(6+_OFF,2+_OFF) = 0.5*tmp77*tmp58*tmp59*tmp85*tmp71*tmp57; -J(6+_OFF,3+_OFF) = -0.25*tmp58*tmp72*tmp85*tmp71*tmp73*tmp81; -J(6+_OFF,4+_OFF) = 0.25*tmp58*tmp72*tmp85*tmp71*tmp73*tmp82; +J(4+_OFF,1+_OFF) = -0.634*tmp32*tmp33*tmp86*tmp43*tmp95*tmp78*tmp79 - 0.634*tmp32*tmp33*tmp101*tmp43*tmp110*tmp78*tmp79 - 0.634*tmp32*tmp33*tmp113*tmp43*tmp122*tmp78*tmp79; +J(4+_OFF,2+_OFF) = 0.634*tmp98*tmp32*tmp33*tmp86*tmp43*tmp95*tmp78 + 0.634*tmp98*tmp32*tmp33*tmp101*tmp43*tmp110*tmp78 + 0.634*tmp98*tmp32*tmp33*tmp113*tmp43*tmp122*tmp78; +J(4+_OFF,3+_OFF) = -0.317*tmp32*tmp123*tmp86*tmp43*tmp95*tmp124*tmp125 - 0.317*tmp32*tmp123*tmp101*tmp43*tmp110*tmp124*tmp125 - 0.317*tmp32*tmp123*tmp113*tmp43*tmp122*tmp124*tmp125; +J(4+_OFF,4+_OFF) = 0.317*tmp32*tmp123*tmp86*tmp43*tmp95*tmp124*tmp126 + 0.317*tmp32*tmp123*tmp101*tmp43*tmp110*tmp124*tmp126 + 0.317*tmp32*tmp123*tmp113*tmp43*tmp122*tmp124*tmp126; +J(5+_OFF,1+_OFF) = -0.634*tmp32*tmp33*tmp129*tmp43*tmp95*tmp78*tmp79 - 0.634*tmp32*tmp33*tmp132*tmp43*tmp110*tmp78*tmp79 - 0.634*tmp32*tmp33*tmp86*tmp43*tmp122*tmp78*tmp79; +J(5+_OFF,2+_OFF) = 0.634*tmp98*tmp32*tmp33*tmp129*tmp43*tmp95*tmp78 + 0.634*tmp98*tmp32*tmp33*tmp132*tmp43*tmp110*tmp78 + 0.634*tmp98*tmp32*tmp33*tmp86*tmp43*tmp122*tmp78; +J(5+_OFF,3+_OFF) = -0.317*tmp32*tmp123*tmp129*tmp43*tmp95*tmp124*tmp125 - 0.317*tmp32*tmp123*tmp132*tmp43*tmp110*tmp124*tmp125 - 0.317*tmp32*tmp123*tmp86*tmp43*tmp122*tmp124*tmp125; +J(5+_OFF,4+_OFF) = 0.317*tmp32*tmp123*tmp129*tmp43*tmp95*tmp124*tmp126 + 0.317*tmp32*tmp123*tmp132*tmp43*tmp110*tmp124*tmp126 + 0.317*tmp32*tmp123*tmp86*tmp43*tmp122*tmp124*tmp126; +J(6+_OFF,1+_OFF) = -0.634*tmp32*tmp33*tmp132*tmp43*tmp95*tmp78*tmp79 - 0.634*tmp32*tmp33*tmp135*tmp43*tmp110*tmp78*tmp79 - 0.634*tmp32*tmp33*tmp101*tmp43*tmp122*tmp78*tmp79; +J(6+_OFF,2+_OFF) = 0.634*tmp98*tmp32*tmp33*tmp132*tmp43*tmp95*tmp78 + 0.634*tmp98*tmp32*tmp33*tmp135*tmp43*tmp110*tmp78 + 0.634*tmp98*tmp32*tmp33*tmp101*tmp43*tmp122*tmp78; +J(6+_OFF,3+_OFF) = -0.317*tmp32*tmp123*tmp132*tmp43*tmp95*tmp124*tmp125 - 0.317*tmp32*tmp123*tmp135*tmp43*tmp110*tmp124*tmp125 - 0.317*tmp32*tmp123*tmp101*tmp43*tmp122*tmp124*tmp125; +J(6+_OFF,4+_OFF) = 0.317*tmp32*tmp123*tmp132*tmp43*tmp95*tmp124*tmp126 + 0.317*tmp32*tmp123*tmp135*tmp43*tmp110*tmp124*tmp126 + 0.317*tmp32*tmp123*tmp101*tmp43*tmp122*tmp124*tmp126; diff --git a/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrMYaw.cppready b/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrMYaw.cppready index b0cc942..fd18df9 100644 --- a/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrMYaw.cppready +++ b/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrMYaw.cppready @@ -1,88 +1,138 @@ double tmp0 = ibd(2+_OFF); -double tmp1 = ibod(1+_OFF); -double tmp2 = ibd(3+_OFF); +double tmp1 = ibd(3+_OFF); +double tmp2 = ibod(1+_OFF); double tmp3 = ibod(2+_OFF); double tmp4 = ibod(3+_OFF); double tmp5 = ibd(1+_OFF); -double tmp6 = x(4+_OFF); -double tmp7 = x(6+_OFF); -double tmp8 = x(5+_OFF); -double tmp9 = x(7+_OFF); -double tmp10 = wind(1+_OFF); -double tmp11 = tmp8*tmp7; -double tmp12 = wind(3+_OFF); -double tmp13 = wind(2+_OFF); -double tmp14 = std::pow(tmp6,2); -double tmp15 = std::pow(tmp8,2); -double tmp16 = std::pow(tmp7,2); -double tmp17 = std::pow(tmp9,2); -double tmp18 = -tmp17; -double tmp19 = tmp8*tmp9; -double tmp20 = tmp7*tmp9; -double tmp21 = -tmp15; -double tmp22 = -tmp16; -double tmp23 = -(tmp6*tmp9); -double tmp24 = tmp11 + tmp23; -double tmp25 = -2*tmp10*tmp24; -double tmp26 = tmp6*tmp8; -double tmp27 = tmp26 + tmp20; -double tmp28 = -2*tmp12*tmp27; -double tmp29 = tmp14 + tmp21 + tmp16 + tmp18; -double tmp30 = -(tmp13*tmp29); -double tmp31 = x(9+_OFF); -double tmp32 = tmp25 + tmp28 + tmp30 + tmp31; -double tmp33 = tmp6*tmp9; -double tmp34 = tmp11 + tmp33; -double tmp35 = -2*tmp13*tmp34; -double tmp36 = -(tmp6*tmp7); -double tmp37 = tmp36 + tmp19; -double tmp38 = -2*tmp12*tmp37; -double tmp39 = tmp14 + tmp15 + tmp22 + tmp18; -double tmp40 = -(tmp10*tmp39); -double tmp41 = x(8+_OFF); -double tmp42 = tmp35 + tmp38 + tmp40 + tmp41; -double tmp43 = std::pow(tmp42,2); -double tmp44 = std::pow(tmp32,2); -double tmp45 = tmp6*tmp7; -double tmp46 = tmp45 + tmp19; -double tmp47 = -2*tmp10*tmp46; -double tmp48 = -(tmp6*tmp8); -double tmp49 = tmp48 + tmp20; -double tmp50 = -2*tmp13*tmp49; -double tmp51 = tmp14 + tmp21 + tmp22 + tmp17; -double tmp52 = -(tmp12*tmp51); -double tmp53 = x(10+_OFF); -double tmp54 = tmp47 + tmp50 + tmp52 + tmp53; -double tmp55 = std::pow(tmp54,2); -double tmp56 = tmp43 + tmp44 + tmp55; -double tmp57 = cp(2+_OFF); -double tmp58 = cp(3+_OFF); -double tmp59 = -(tmp0*tmp1); -double tmp60 = tmp3*tmp4; -double tmp61 = tmp59 + tmp60; -double tmp62 = tmp5*tmp0*tmp2; -double tmp63 = std::pow(tmp1,2); -double tmp64 = -(tmp0*tmp63); -double tmp65 = std::pow(tmp3,2); -double tmp66 = -(tmp2*tmp65); -double tmp67 = 2*tmp1*tmp3*tmp4; -double tmp68 = std::pow(tmp4,2); -double tmp69 = -(tmp5*tmp68); -double tmp70 = tmp62 + tmp64 + tmp66 + tmp67 + tmp69; -double tmp71 = 1/tmp70; -double tmp72 = z(4+_OFF); -double tmp73 = 1/std::sqrt(tmp56); -double tmp74 = tmp32*tmp73; -double tmp75 = std::asin(tmp74); -double tmp76 = tmp1*tmp3; -double tmp77 = -(tmp5*tmp4); -double tmp78 = tmp76 + tmp77; -double tmp79 = std::pow(tmp58,2); -double tmp80 = std::sqrt(tmp56); -double tmp81 = x(13+_OFF); -double tmp82 = tmp5*tmp0; -double tmp83 = -tmp65; -double tmp84 = tmp82 + tmp83; +double tmp6 = std::pow(tmp4,2); +double tmp7 = x(5+_OFF); +double tmp8 = x(6+_OFF); +double tmp9 = x(4+_OFF); +double tmp10 = x(7+_OFF); +double tmp11 = x(30+_OFF); +double tmp12 = x(31+_OFF); +double tmp13 = x(32+_OFF); +double tmp14 = x(33+_OFF); +double tmp15 = tmp12*tmp14; +double tmp16 = std::pow(tmp11,2); +double tmp17 = std::pow(tmp12,2); +double tmp18 = std::pow(tmp13,2); +double tmp19 = -tmp18; +double tmp20 = std::pow(tmp14,2); +double tmp21 = wind(1+_OFF); +double tmp22 = tmp12*tmp13; +double tmp23 = wind(3+_OFF); +double tmp24 = tmp13*tmp14; +double tmp25 = wind(2+_OFF); +double tmp26 = -tmp17; +double tmp27 = -tmp20; +double tmp28 = tmp11*tmp13; +double tmp29 = tmp28 + tmp15; +double tmp30 = -(tmp11*tmp12); +double tmp31 = tmp30 + tmp24; +double tmp32 = tmp16 + tmp26 + tmp19 + tmp20; +double tmp33 = cp(2+_OFF); +double tmp34 = cp(3+_OFF); +double tmp35 = tmp5*tmp0*tmp1; +double tmp36 = std::pow(tmp2,2); +double tmp37 = -(tmp0*tmp36); +double tmp38 = std::pow(tmp3,2); +double tmp39 = -(tmp1*tmp38); +double tmp40 = 2*tmp2*tmp3*tmp4; +double tmp41 = -(tmp5*tmp6); +double tmp42 = tmp35 + tmp37 + tmp39 + tmp40 + tmp41; +double tmp43 = 1/tmp42; +double tmp44 = tmp7*tmp8; +double tmp45 = std::pow(tmp9,2); +double tmp46 = std::pow(tmp7,2); +double tmp47 = std::pow(tmp8,2); +double tmp48 = std::pow(tmp10,2); +double tmp49 = -tmp48; +double tmp50 = tmp11*tmp14; +double tmp51 = tmp22 + tmp50; +double tmp52 = -2*tmp25*tmp51; +double tmp53 = -(tmp11*tmp13); +double tmp54 = tmp53 + tmp15; +double tmp55 = -2*tmp23*tmp54; +double tmp56 = tmp16 + tmp17 + tmp19 + tmp27; +double tmp57 = -(tmp21*tmp56); +double tmp58 = x(34+_OFF); +double tmp59 = tmp52 + tmp55 + tmp57 + tmp58; +double tmp60 = std::pow(tmp59,2); +double tmp61 = -(tmp11*tmp14); +double tmp62 = tmp22 + tmp61; +double tmp63 = -2*tmp21*tmp62; +double tmp64 = tmp11*tmp12; +double tmp65 = tmp64 + tmp24; +double tmp66 = -2*tmp23*tmp65; +double tmp67 = tmp16 + tmp26 + tmp18 + tmp27; +double tmp68 = -(tmp25*tmp67); +double tmp69 = x(35+_OFF); +double tmp70 = tmp63 + tmp66 + tmp68 + tmp69; +double tmp71 = std::pow(tmp70,2); +double tmp72 = -2*tmp21*tmp29; +double tmp73 = -2*tmp25*tmp31; +double tmp74 = -(tmp23*tmp32); +double tmp75 = x(36+_OFF); +double tmp76 = tmp72 + tmp73 + tmp74 + tmp75; +double tmp77 = std::pow(tmp76,2); +double tmp78 = tmp60 + tmp71 + tmp77; +double tmp79 = z(4+_OFF); +double tmp80 = tmp7*tmp10; +double tmp81 = tmp8*tmp10; +double tmp82 = -tmp46; +double tmp83 = -tmp47; +double tmp84 = tmp0*tmp1; +double tmp85 = -tmp6; +double tmp86 = tmp84 + tmp85; +double tmp87 = tmp45 + tmp46 + tmp83 + tmp49; +double tmp88 = 2*tmp87*tmp29; +double tmp89 = tmp9*tmp10; +double tmp90 = tmp44 + tmp89; +double tmp91 = 4*tmp90*tmp31; +double tmp92 = -(tmp9*tmp8); +double tmp93 = tmp92 + tmp80; +double tmp94 = 2*tmp93*tmp32; +double tmp95 = tmp88 + tmp91 + tmp94; +double tmp96 = 1/std::sqrt(tmp78); +double tmp97 = tmp70*tmp96; +double tmp98 = std::asin(tmp97); +double tmp99 = -(tmp1*tmp3); +double tmp100 = tmp2*tmp4; +double tmp101 = tmp99 + tmp100; +double tmp102 = -(tmp9*tmp10); +double tmp103 = tmp44 + tmp102; +double tmp104 = 4*tmp103*tmp29; +double tmp105 = tmp45 + tmp82 + tmp47 + tmp49; +double tmp106 = 2*tmp105*tmp31; +double tmp107 = tmp9*tmp7; +double tmp108 = tmp107 + tmp81; +double tmp109 = 2*tmp108*tmp32; +double tmp110 = tmp104 + tmp106 + tmp109; +double tmp111 = -(tmp0*tmp2); +double tmp112 = tmp3*tmp4; +double tmp113 = tmp111 + tmp112; +double tmp114 = tmp9*tmp8; +double tmp115 = tmp114 + tmp80; +double tmp116 = 4*tmp115*tmp29; +double tmp117 = -(tmp9*tmp7); +double tmp118 = tmp117 + tmp81; +double tmp119 = 4*tmp118*tmp31; +double tmp120 = tmp45 + tmp82 + tmp83 + tmp48; +double tmp121 = tmp120*tmp32; +double tmp122 = tmp116 + tmp119 + tmp121; +double tmp123 = std::pow(tmp34,2); +double tmp124 = std::sqrt(tmp78); +double tmp125 = x(39+_OFF); +double tmp126 = tmp5*tmp1; +double tmp127 = -tmp36; +double tmp128 = tmp126 + tmp127; +double tmp129 = tmp2*tmp3; +double tmp130 = -(tmp5*tmp4); +double tmp131 = tmp129 + tmp130; +double tmp132 = tmp5*tmp0; +double tmp133 = -tmp38; +double tmp134 = tmp132 + tmp133; J(1+_OFF,1+_OFF) = 0; J(1+_OFF,2+_OFF) = 0; J(1+_OFF,3+_OFF) = 0; @@ -92,12 +142,12 @@ J(2+_OFF,3+_OFF) = 0; J(3+_OFF,1+_OFF) = 0; J(3+_OFF,2+_OFF) = 0; J(3+_OFF,3+_OFF) = 0; -J(4+_OFF,1+_OFF) = -0.5*tmp57*tmp58*tmp61*tmp71*tmp56*tmp72; -J(4+_OFF,2+_OFF) = -0.5*tmp75*tmp57*tmp58*tmp61*tmp71*tmp56; -J(4+_OFF,3+_OFF) = -0.25*tmp57*tmp79*tmp61*tmp71*tmp80*tmp81; -J(5+_OFF,1+_OFF) = -0.5*tmp57*tmp58*tmp78*tmp71*tmp56*tmp72; -J(5+_OFF,2+_OFF) = -0.5*tmp75*tmp57*tmp58*tmp78*tmp71*tmp56; -J(5+_OFF,3+_OFF) = -0.25*tmp57*tmp79*tmp78*tmp71*tmp80*tmp81; -J(6+_OFF,1+_OFF) = -0.5*tmp57*tmp58*tmp84*tmp71*tmp56*tmp72; -J(6+_OFF,2+_OFF) = -0.5*tmp75*tmp57*tmp58*tmp84*tmp71*tmp56; -J(6+_OFF,3+_OFF) = -0.25*tmp57*tmp79*tmp84*tmp71*tmp80*tmp81; +J(4+_OFF,1+_OFF) = 0.634*tmp33*tmp34*tmp86*tmp43*tmp95*tmp78*tmp79 + 0.634*tmp33*tmp34*tmp101*tmp43*tmp110*tmp78*tmp79 + 0.634*tmp33*tmp34*tmp113*tmp43*tmp122*tmp78*tmp79; +J(4+_OFF,2+_OFF) = -0.634*tmp98*tmp33*tmp34*tmp86*tmp43*tmp95*tmp78 - 0.634*tmp98*tmp33*tmp34*tmp101*tmp43*tmp110*tmp78 - 0.634*tmp98*tmp33*tmp34*tmp113*tmp43*tmp122*tmp78; +J(4+_OFF,3+_OFF) = -0.317*tmp33*tmp123*tmp86*tmp43*tmp95*tmp124*tmp125 - 0.317*tmp33*tmp123*tmp101*tmp43*tmp110*tmp124*tmp125 - 0.317*tmp33*tmp123*tmp113*tmp43*tmp122*tmp124*tmp125; +J(5+_OFF,1+_OFF) = 0.634*tmp33*tmp34*tmp101*tmp43*tmp95*tmp78*tmp79 + 0.634*tmp33*tmp34*tmp128*tmp43*tmp110*tmp78*tmp79 + 0.634*tmp33*tmp34*tmp131*tmp43*tmp122*tmp78*tmp79; +J(5+_OFF,2+_OFF) = -0.634*tmp98*tmp33*tmp34*tmp101*tmp43*tmp95*tmp78 - 0.634*tmp98*tmp33*tmp34*tmp128*tmp43*tmp110*tmp78 - 0.634*tmp98*tmp33*tmp34*tmp131*tmp43*tmp122*tmp78; +J(5+_OFF,3+_OFF) = -0.317*tmp33*tmp123*tmp101*tmp43*tmp95*tmp124*tmp125 - 0.317*tmp33*tmp123*tmp128*tmp43*tmp110*tmp124*tmp125 - 0.317*tmp33*tmp123*tmp131*tmp43*tmp122*tmp124*tmp125; +J(6+_OFF,1+_OFF) = 0.634*tmp33*tmp34*tmp113*tmp43*tmp95*tmp78*tmp79 + 0.634*tmp33*tmp34*tmp131*tmp43*tmp110*tmp78*tmp79 + 0.634*tmp33*tmp34*tmp134*tmp43*tmp122*tmp78*tmp79; +J(6+_OFF,2+_OFF) = -0.634*tmp98*tmp33*tmp34*tmp113*tmp43*tmp95*tmp78 - 0.634*tmp98*tmp33*tmp34*tmp131*tmp43*tmp110*tmp78 - 0.634*tmp98*tmp33*tmp34*tmp134*tmp43*tmp122*tmp78; +J(6+_OFF,3+_OFF) = -0.317*tmp33*tmp123*tmp113*tmp43*tmp95*tmp124*tmp125 - 0.317*tmp33*tmp123*tmp131*tmp43*tmp110*tmp124*tmp125 - 0.317*tmp33*tmp123*tmp134*tmp43*tmp122*tmp124*tmp125; diff --git a/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrQ.cppready b/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrQ.cppready index b3cfa18..642d391 100644 --- a/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrQ.cppready +++ b/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrQ.cppready @@ -1,430 +1,348 @@ double tmp0 = x(6+_OFF); -double tmp1 = x(4+_OFF); -double tmp2 = x(5+_OFF); -double tmp3 = x(7+_OFF); -double tmp4 = wind(1+_OFF); +double tmp1 = x(30+_OFF); +double tmp2 = x(31+_OFF); +double tmp3 = x(32+_OFF); +double tmp4 = x(33+_OFF); double tmp5 = tmp2*tmp3; -double tmp6 = wind(2+_OFF); -double tmp7 = wind(3+_OFF); -double tmp8 = std::pow(tmp1,2); -double tmp9 = std::pow(tmp2,2); -double tmp10 = std::pow(tmp0,2); -double tmp11 = -tmp10; -double tmp12 = std::pow(tmp3,2); -double tmp13 = tmp2*tmp0; -double tmp14 = tmp1*tmp3; -double tmp15 = tmp13 + tmp14; -double tmp16 = -2*tmp6*tmp15; -double tmp17 = -(tmp1*tmp0); -double tmp18 = tmp17 + tmp5; -double tmp19 = -2*tmp7*tmp18; -double tmp20 = -tmp12; -double tmp21 = tmp8 + tmp9 + tmp11 + tmp20; -double tmp22 = -(tmp4*tmp21); -double tmp23 = x(8+_OFF); -double tmp24 = tmp16 + tmp19 + tmp22 + tmp23; -double tmp25 = tmp1*tmp0; -double tmp26 = tmp25 + tmp5; -double tmp27 = 2*tmp4*tmp26; -double tmp28 = -(tmp1*tmp2); -double tmp29 = tmp0*tmp3; -double tmp30 = tmp28 + tmp29; -double tmp31 = 2*tmp6*tmp30; -double tmp32 = -tmp9; -double tmp33 = tmp8 + tmp32 + tmp11 + tmp12; -double tmp34 = tmp7*tmp33; -double tmp35 = x(10+_OFF); -double tmp36 = -tmp35; -double tmp37 = tmp27 + tmp31 + tmp34 + tmp36; -double tmp38 = -(tmp1*tmp3); -double tmp39 = tmp13 + tmp38; -double tmp40 = -2*tmp4*tmp39; +double tmp6 = std::pow(tmp1,2); +double tmp7 = std::pow(tmp2,2); +double tmp8 = std::pow(tmp3,2); +double tmp9 = std::pow(tmp4,2); +double tmp10 = -tmp9; +double tmp11 = wind(1+_OFF); +double tmp12 = tmp2*tmp4; +double tmp13 = wind(2+_OFF); +double tmp14 = tmp3*tmp4; +double tmp15 = wind(3+_OFF); +double tmp16 = -tmp7; +double tmp17 = -tmp8; +double tmp18 = tmp1*tmp4; +double tmp19 = tmp5 + tmp18; +double tmp20 = -2*tmp13*tmp19; +double tmp21 = -(tmp1*tmp3); +double tmp22 = tmp21 + tmp12; +double tmp23 = -2*tmp15*tmp22; +double tmp24 = tmp6 + tmp7 + tmp17 + tmp10; +double tmp25 = -(tmp11*tmp24); +double tmp26 = x(34+_OFF); +double tmp27 = tmp20 + tmp23 + tmp25 + tmp26; +double tmp28 = tmp1*tmp3; +double tmp29 = tmp28 + tmp12; +double tmp30 = -2*tmp11*tmp29; +double tmp31 = -(tmp1*tmp2); +double tmp32 = tmp31 + tmp14; +double tmp33 = -2*tmp13*tmp32; +double tmp34 = tmp6 + tmp16 + tmp17 + tmp9; +double tmp35 = -(tmp15*tmp34); +double tmp36 = x(36+_OFF); +double tmp37 = tmp30 + tmp33 + tmp35 + tmp36; +double tmp38 = std::atan2(tmp37,tmp27); +double tmp39 = -(tmp1*tmp4); +double tmp40 = tmp5 + tmp39; double tmp41 = tmp1*tmp2; -double tmp42 = tmp41 + tmp29; -double tmp43 = -2*tmp7*tmp42; -double tmp44 = tmp8 + tmp32 + tmp10 + tmp20; -double tmp45 = -(tmp6*tmp44); -double tmp46 = x(9+_OFF); -double tmp47 = tmp40 + tmp43 + tmp45 + tmp46; -double tmp48 = std::pow(tmp24,2); -double tmp49 = -2*tmp4*tmp26; -double tmp50 = -2*tmp6*tmp30; -double tmp51 = -(tmp7*tmp33); -double tmp52 = tmp49 + tmp50 + tmp51 + tmp35; -double tmp53 = cp(2+_OFF); -double tmp54 = std::pow(tmp37,2); -double tmp55 = tmp48 + tmp54; -double tmp56 = 1/std::sqrt(tmp55); -double tmp57 = -2*tmp4*tmp1; -double tmp58 = 2*tmp7*tmp0; -double tmp59 = -2*tmp6*tmp3; -double tmp60 = tmp57 + tmp58 + tmp59; -double tmp61 = 2*tmp60*tmp24; -double tmp62 = -2*tmp6*tmp1; -double tmp63 = -2*tmp7*tmp2; -double tmp64 = 2*tmp4*tmp3; -double tmp65 = tmp62 + tmp63 + tmp64; -double tmp66 = 2*tmp65*tmp47; -double tmp67 = -2*tmp7*tmp1; -double tmp68 = 2*tmp6*tmp2; -double tmp69 = -2*tmp4*tmp0; -double tmp70 = tmp67 + tmp68 + tmp69; -double tmp71 = 2*tmp70*tmp52; -double tmp72 = tmp61 + tmp66 + tmp71; -double tmp73 = std::pow(tmp47,2); -double tmp74 = std::pow(tmp52,2); -double tmp75 = tmp48 + tmp73 + tmp74; -double tmp76 = 1/std::sqrt(tmp75); -double tmp77 = tmp47*tmp76; -double tmp78 = std::asin(tmp77); -double tmp79 = fLat(1+_OFF); -double tmp80 = -(tmp4*tmp8); -double tmp81 = -(tmp4*tmp9); -double tmp82 = 2*tmp7*tmp1*tmp0; -double tmp83 = -2*tmp6*tmp2*tmp0; -double tmp84 = tmp4*tmp10; -double tmp85 = -2*tmp6*tmp1*tmp3; -double tmp86 = -2*tmp7*tmp2*tmp3; -double tmp87 = tmp4*tmp12; -double tmp88 = tmp80 + tmp81 + tmp82 + tmp83 + tmp84 + tmp85 + tmp86 + tmp87 + tmp23; -double tmp89 = tmp6*tmp8; -double tmp90 = 2*tmp7*tmp1*tmp2; -double tmp91 = -(tmp6*tmp9); -double tmp92 = 2*tmp4*tmp2*tmp0; -double tmp93 = tmp6*tmp10; -double tmp94 = -2*tmp4*tmp1*tmp3; -double tmp95 = 2*tmp7*tmp0*tmp3; -double tmp96 = -(tmp6*tmp12); -double tmp97 = -tmp46; -double tmp98 = tmp89 + tmp90 + tmp91 + tmp92 + tmp93 + tmp94 + tmp95 + tmp96 + tmp97; -double tmp99 = std::sqrt(tmp75); -double tmp100 = fLift(1+_OFF); -double tmp101 = std::atan2(tmp37,tmp24); -double tmp102 = fLift(2+_OFF); -double tmp103 = tmp101*tmp102; -double tmp104 = tmp100 + tmp103; -double tmp105 = 2*tmp7*tmp1; -double tmp106 = -2*tmp6*tmp2; -double tmp107 = 2*tmp4*tmp0; -double tmp108 = tmp105 + tmp106 + tmp107; -double tmp109 = 2*tmp108*tmp37; -double tmp110 = tmp61 + tmp109; -double tmp111 = std::pow(tmp55,-1.5); -double tmp112 = tmp7*tmp8; -double tmp113 = -2*tmp6*tmp1*tmp2; -double tmp114 = -(tmp7*tmp9); -double tmp115 = 2*tmp4*tmp1*tmp0; -double tmp116 = -(tmp7*tmp10); -double tmp117 = 2*tmp4*tmp2*tmp3; -double tmp118 = 2*tmp6*tmp0*tmp3; -double tmp119 = tmp7*tmp12; -double tmp120 = tmp112 + tmp113 + tmp114 + tmp115 + tmp116 + tmp117 + tmp118 + tmp119 + tmp36; -double tmp121 = 1/tmp55; -double tmp122 = 1/tmp75; -double tmp123 = fDrag(1+_OFF); -double tmp124 = fDrag(2+_OFF); -double tmp125 = tmp101*tmp124; -double tmp126 = std::pow(tmp101,2); -double tmp127 = fDrag(3+_OFF); -double tmp128 = tmp126*tmp127; -double tmp129 = std::pow(tmp78,2); -double tmp130 = fDrag(4+_OFF); -double tmp131 = tmp129*tmp130; -double tmp132 = tmp123 + tmp125 + tmp128 + tmp131; -double tmp133 = -(tmp73*tmp122); -double tmp134 = 1 + tmp133; -double tmp135 = std::sqrt(tmp134); -double tmp136 = 1/std::sqrt(tmp134); -double tmp137 = tmp108*tmp24*tmp121; -double tmp138 = tmp60*tmp121*tmp52; -double tmp139 = tmp137 + tmp138; -double tmp140 = std::pow(tmp75,-1.5); -double tmp141 = -(tmp47*tmp72*tmp140)/2.; -double tmp142 = tmp65*tmp76; -double tmp143 = tmp141 + tmp142; -double tmp144 = cp(1+_OFF); -double tmp145 = gravity(1+_OFF); -double tmp146 = cp(4+_OFF); -double tmp147 = 1/tmp146; -double tmp148 = 2*tmp6*tmp1; -double tmp149 = 2*tmp7*tmp2; -double tmp150 = -2*tmp4*tmp3; -double tmp151 = tmp148 + tmp149 + tmp150; -double tmp152 = -2*tmp4*tmp2; -double tmp153 = -2*tmp6*tmp0; -double tmp154 = -2*tmp7*tmp3; -double tmp155 = tmp152 + tmp153 + tmp154; -double tmp156 = 2*tmp155*tmp24; -double tmp157 = 2*tmp70*tmp47; -double tmp158 = 2*tmp151*tmp52; -double tmp159 = tmp156 + tmp157 + tmp158; -double tmp160 = 2*tmp65*tmp37; -double tmp161 = tmp156 + tmp160; -double tmp162 = std::pow(tmp75,-2); -double tmp163 = tmp65*tmp24*tmp121; -double tmp164 = tmp155*tmp121*tmp52; -double tmp165 = tmp163 + tmp164; -double tmp166 = -(tmp47*tmp159*tmp140)/2.; -double tmp167 = tmp70*tmp76; -double tmp168 = tmp166 + tmp167; -double tmp169 = std::pow(tmp144,2); -double tmp170 = std::pow(M_PI,-2); -double tmp171 = fThrust(3+_OFF); -double tmp172 = 1/M_PI; -double tmp173 = fThrust(2+_OFF); -double tmp174 = z(1+_OFF); -double tmp175 = 2*tmp108*tmp24; -double tmp176 = 2*tmp155*tmp47; -double tmp177 = 2*tmp60*tmp52; -double tmp178 = tmp175 + tmp176 + tmp177; -double tmp179 = 2*tmp4*tmp1; -double tmp180 = -2*tmp7*tmp0; -double tmp181 = 2*tmp6*tmp3; +double tmp42 = tmp41 + tmp14; +double tmp43 = tmp6 + tmp16 + tmp8 + tmp10; +double tmp44 = -2*tmp11*tmp40; +double tmp45 = -2*tmp15*tmp42; +double tmp46 = -(tmp13*tmp43); +double tmp47 = x(35+_OFF); +double tmp48 = tmp44 + tmp45 + tmp46 + tmp47; +double tmp49 = std::pow(tmp27,2); +double tmp50 = std::pow(tmp48,2); +double tmp51 = std::pow(tmp37,2); +double tmp52 = tmp49 + tmp50 + tmp51; +double tmp53 = 1/std::sqrt(tmp52); +double tmp54 = tmp48*tmp53; +double tmp55 = std::asin(tmp54); +double tmp56 = cp(2+_OFF); +double tmp57 = x(4+_OFF); +double tmp58 = x(7+_OFF); +double tmp59 = fLat(1+_OFF); +double tmp60 = std::sqrt(tmp52); +double tmp61 = tmp49 + tmp51; +double tmp62 = 1/std::sqrt(tmp61); +double tmp63 = fDrag(1+_OFF); +double tmp64 = fDrag(2+_OFF); +double tmp65 = -(tmp38*tmp64); +double tmp66 = std::pow(tmp38,2); +double tmp67 = fDrag(3+_OFF); +double tmp68 = tmp66*tmp67; +double tmp69 = std::pow(tmp55,2); +double tmp70 = fDrag(4+_OFF); +double tmp71 = tmp69*tmp70; +double tmp72 = tmp63 + tmp65 + tmp68 + tmp71; +double tmp73 = 1/tmp52; +double tmp74 = -(tmp50*tmp73); +double tmp75 = 1 + tmp74; +double tmp76 = std::sqrt(tmp75); +double tmp77 = fLift(1+_OFF); +double tmp78 = -tmp77; +double tmp79 = fLift(2+_OFF); +double tmp80 = tmp38*tmp79; +double tmp81 = tmp78 + tmp80; +double tmp82 = cp(1+_OFF); +double tmp83 = std::pow(tmp82,2); +double tmp84 = z(1+_OFF); +double tmp85 = gravity(1+_OFF); +double tmp86 = cp(4+_OFF); +double tmp87 = 1/tmp86; +double tmp88 = 0.634*tmp56*tmp72*tmp48*tmp60; +double tmp89 = 0.634*tmp55*tmp56*tmp59*tmp52*tmp76; +double tmp90 = 0. + tmp88 + tmp89; +double tmp91 = x(5+_OFF); +double tmp92 = -0.634*tmp55*tmp56*tmp59*tmp48*tmp37*tmp62*tmp60; +double tmp93 = 0.634*tmp56*tmp81*tmp27*tmp62*tmp52; +double tmp94 = 0.634*tmp56*tmp72*tmp37*tmp62*tmp52*tmp76; +double tmp95 = tmp92 + tmp93 + tmp94; +double tmp96 = -0.634*tmp55*tmp56*tmp59*tmp27*tmp48*tmp62*tmp60; +double tmp97 = -0.634*tmp56*tmp81*tmp37*tmp62*tmp52; +double tmp98 = 0.634*tmp56*tmp72*tmp27*tmp62*tmp52*tmp76; +double tmp99 = std::pow(M_PI,-2); +double tmp100 = fThrust(3+_OFF); +double tmp101 = tmp99*tmp100*tmp52; +double tmp102 = 1/M_PI; +double tmp103 = fThrust(2+_OFF); +double tmp104 = tmp102*tmp82*tmp103*tmp60*tmp84; +double tmp105 = fThrust(1+_OFF); +double tmp106 = std::pow(tmp84,2); +double tmp107 = tmp83*tmp105*tmp106; +double tmp108 = tmp101 + tmp104 + tmp107; +double tmp109 = 1.268*tmp83*tmp108; +double tmp110 = tmp96 + tmp97 + tmp98 + tmp109; +double tmp111 = 2*tmp85*tmp91; +double tmp112 = -4*tmp58*tmp40; +double tmp113 = 4*tmp91*tmp42; +double tmp114 = 2*tmp57*tmp43; +double tmp115 = tmp112 + tmp113 + tmp114; +double tmp116 = tmp115*tmp90; +double tmp117 = -4*tmp58*tmp29; +double tmp118 = 4*tmp57*tmp32; +double tmp119 = 2*tmp91*tmp34; +double tmp120 = tmp117 + tmp118 + tmp119; +double tmp121 = tmp120*tmp95; +double tmp122 = 4*tmp57*tmp19; +double tmp123 = 4*tmp91*tmp22; +double tmp124 = -2*tmp58*tmp24; +double tmp125 = tmp122 + tmp123 + tmp124; +double tmp126 = tmp125*tmp110; +double tmp127 = tmp116 + tmp121 + tmp126; +double tmp128 = -(tmp87*tmp127); +double tmp129 = tmp111 + tmp128; +double tmp130 = 2*tmp85*tmp58; +double tmp131 = 4*tmp91*tmp40; +double tmp132 = 4*tmp58*tmp42; +double tmp133 = 2*tmp0*tmp43; +double tmp134 = tmp131 + tmp132 + tmp133; +double tmp135 = tmp134*tmp90; +double tmp136 = 4*tmp91*tmp29; +double tmp137 = 4*tmp0*tmp32; +double tmp138 = 2*tmp58*tmp34; +double tmp139 = tmp136 + tmp137 + tmp138; +double tmp140 = tmp139*tmp95; +double tmp141 = 4*tmp0*tmp19; +double tmp142 = 4*tmp58*tmp22; +double tmp143 = 2*tmp91*tmp24; +double tmp144 = tmp141 + tmp142 + tmp143; +double tmp145 = tmp144*tmp110; +double tmp146 = tmp135 + tmp140 + tmp145; +double tmp147 = -(tmp87*tmp146); +double tmp148 = tmp130 + tmp147; +double tmp149 = 2*tmp85*tmp57; +double tmp150 = 4*tmp0*tmp40; +double tmp151 = 4*tmp57*tmp42; +double tmp152 = -2*tmp91*tmp43; +double tmp153 = tmp150 + tmp151 + tmp152; +double tmp154 = tmp153*tmp90; +double tmp155 = 4*tmp0*tmp29; +double tmp156 = -4*tmp91*tmp32; +double tmp157 = 2*tmp57*tmp34; +double tmp158 = tmp155 + tmp156 + tmp157; +double tmp159 = tmp158*tmp95; +double tmp160 = -4*tmp91*tmp19; +double tmp161 = 4*tmp57*tmp22; +double tmp162 = 2*tmp0*tmp24; +double tmp163 = tmp160 + tmp161 + tmp162; +double tmp164 = tmp163*tmp110; +double tmp165 = tmp154 + tmp159 + tmp164; +double tmp166 = -(tmp87*tmp165); +double tmp167 = tmp149 + tmp166; +double tmp168 = -2*tmp85*tmp0; +double tmp169 = 4*tmp57*tmp40; +double tmp170 = -4*tmp0*tmp42; +double tmp171 = 2*tmp58*tmp43; +double tmp172 = tmp169 + tmp170 + tmp171; +double tmp173 = tmp172*tmp90; +double tmp174 = 4*tmp57*tmp29; +double tmp175 = 4*tmp58*tmp32; +double tmp176 = -2*tmp0*tmp34; +double tmp177 = tmp174 + tmp175 + tmp176; +double tmp178 = tmp177*tmp95; +double tmp179 = 4*tmp58*tmp19; +double tmp180 = -4*tmp0*tmp22; +double tmp181 = 2*tmp57*tmp24; double tmp182 = tmp179 + tmp180 + tmp181; -double tmp183 = 2*tmp182*tmp37; -double tmp184 = tmp175 + tmp183; -double tmp185 = tmp182*tmp24*tmp121; -double tmp186 = tmp108*tmp121*tmp52; -double tmp187 = tmp185 + tmp186; -double tmp188 = -(tmp47*tmp178*tmp140)/2.; -double tmp189 = tmp155*tmp76; -double tmp190 = tmp188 + tmp189; -double tmp191 = 2*tmp65*tmp24; -double tmp192 = 2*tmp182*tmp47; -double tmp193 = 2*tmp155*tmp52; -double tmp194 = tmp191 + tmp192 + tmp193; -double tmp195 = 2*tmp4*tmp2; -double tmp196 = 2*tmp6*tmp0; -double tmp197 = 2*tmp7*tmp3; -double tmp198 = tmp195 + tmp196 + tmp197; -double tmp199 = 2*tmp198*tmp37; -double tmp200 = tmp191 + tmp199; -double tmp201 = tmp198*tmp24*tmp121; -double tmp202 = tmp65*tmp121*tmp52; -double tmp203 = tmp201 + tmp202; -double tmp204 = -(tmp47*tmp194*tmp140)/2.; -double tmp205 = tmp182*tmp76; -double tmp206 = tmp204 + tmp205; -double tmp207 = 2*tmp145*tmp2; -double tmp208 = tmp73*tmp72*tmp162; -double tmp209 = -2*tmp65*tmp47*tmp122; -double tmp210 = tmp208 + tmp209; -double tmp211 = -(tmp6*tmp8); -double tmp212 = -2*tmp7*tmp1*tmp2; -double tmp213 = tmp6*tmp9; -double tmp214 = -2*tmp4*tmp2*tmp0; -double tmp215 = -(tmp6*tmp10); -double tmp216 = 2*tmp4*tmp1*tmp3; -double tmp217 = -2*tmp7*tmp0*tmp3; -double tmp218 = tmp6*tmp12; -double tmp219 = tmp211 + tmp212 + tmp213 + tmp214 + tmp215 + tmp216 + tmp217 + tmp218 + tmp46; -double tmp220 = tmp124*tmp139; -double tmp221 = 2*tmp101*tmp127*tmp139; -double tmp222 = 2*tmp78*tmp130*tmp136*tmp143; -double tmp223 = tmp220 + tmp221 + tmp222; -double tmp224 = tmp73*tmp159*tmp162; -double tmp225 = -2*tmp70*tmp47*tmp122; -double tmp226 = tmp224 + tmp225; -double tmp227 = tmp124*tmp165; -double tmp228 = 2*tmp101*tmp127*tmp165; -double tmp229 = 2*tmp78*tmp130*tmp136*tmp168; -double tmp230 = tmp227 + tmp228 + tmp229; -double tmp231 = 2*tmp145*tmp3; -double tmp232 = tmp73*tmp178*tmp162; -double tmp233 = -2*tmp155*tmp47*tmp122; -double tmp234 = tmp232 + tmp233; -double tmp235 = tmp124*tmp187; -double tmp236 = 2*tmp101*tmp127*tmp187; -double tmp237 = 2*tmp78*tmp130*tmp136*tmp190; -double tmp238 = tmp235 + tmp236 + tmp237; -double tmp239 = tmp73*tmp194*tmp162; -double tmp240 = -2*tmp182*tmp47*tmp122; -double tmp241 = tmp239 + tmp240; -double tmp242 = tmp124*tmp203; -double tmp243 = 2*tmp101*tmp127*tmp203; -double tmp244 = 2*tmp78*tmp130*tmp136*tmp206; -double tmp245 = tmp242 + tmp243 + tmp244; -double tmp246 = 2*tmp145*tmp1; -double tmp247 = -(tmp7*tmp8); -double tmp248 = 2*tmp6*tmp1*tmp2; -double tmp249 = tmp7*tmp9; -double tmp250 = -2*tmp4*tmp1*tmp0; -double tmp251 = tmp7*tmp10; -double tmp252 = -2*tmp4*tmp2*tmp3; -double tmp253 = -2*tmp6*tmp0*tmp3; -double tmp254 = -(tmp7*tmp12); -double tmp255 = tmp247 + tmp248 + tmp249 + tmp250 + tmp251 + tmp252 + tmp253 + tmp254 + tmp35; -double tmp256 = -2*tmp145*tmp0; -double tmp257 = 0.5*tmp78*tmp53*tmp79*tmp60*tmp98*tmp56*tmp99; -double tmp258 = -0.5*tmp53*tmp104*tmp108*tmp56*tmp75; -double tmp259 = 0.5*tmp53*tmp132*tmp60*tmp56*tmp75*tmp135; -double tmp260 = 0.5*tmp78*tmp53*tmp79*tmp155*tmp98*tmp56*tmp99; -double tmp261 = -0.5*tmp53*tmp104*tmp65*tmp56*tmp75; -double tmp262 = 0.5*tmp53*tmp132*tmp155*tmp56*tmp75*tmp135; -double tmp263 = ibd(2+_OFF); -double tmp264 = ibd(3+_OFF); -double tmp265 = ibod(1+_OFF); -double tmp266 = ibod(2+_OFF); -double tmp267 = ibod(3+_OFF); -double tmp268 = ibd(1+_OFF); -double tmp269 = std::pow(tmp267,2); -double tmp270 = cp(3+_OFF); -double tmp271 = mRoll(2+_OFF); -double tmp272 = mRoll(3+_OFF); -double tmp273 = x(11+_OFF); -double tmp274 = mRoll(4+_OFF); -double tmp275 = x(13+_OFF); -double tmp276 = tmp268*tmp263*tmp264; -double tmp277 = std::pow(tmp265,2); -double tmp278 = -(tmp263*tmp277); -double tmp279 = std::pow(tmp266,2); -double tmp280 = -(tmp264*tmp279); -double tmp281 = 2*tmp265*tmp266*tmp267; -double tmp282 = -(tmp268*tmp269); -double tmp283 = tmp276 + tmp278 + tmp280 + tmp281 + tmp282; -double tmp284 = 1/tmp283; -double tmp285 = cBar(1+_OFF); -double tmp286 = mPitch(3+_OFF); -double tmp287 = mPitch(4+_OFF); -double tmp288 = x(12+_OFF); -double tmp289 = mYaw(2+_OFF); -double tmp290 = mYaw(3+_OFF); -double tmp291 = tmp263*tmp264; -double tmp292 = -tmp269; -double tmp293 = tmp291 + tmp292; -double tmp294 = -(tmp78*tmp271); -double tmp295 = (tmp270*tmp272*tmp76*tmp273)/2.; -double tmp296 = -(tmp270*tmp274*tmp76*tmp275)/2.; -double tmp297 = mRoll(1+_OFF); -double tmp298 = z(2+_OFF); -double tmp299 = -(tmp297*tmp298); -double tmp300 = tmp294 + tmp295 + tmp296 + tmp299; -double tmp301 = -(tmp264*tmp266); -double tmp302 = tmp265*tmp267; -double tmp303 = tmp301 + tmp302; -double tmp304 = mPitch(1+_OFF); -double tmp305 = -tmp304; -double tmp306 = -(tmp101*tmp286); -double tmp307 = (tmp285*tmp287*tmp76*tmp288)/2.; -double tmp308 = mPitch(2+_OFF); -double tmp309 = z(3+_OFF); -double tmp310 = -(tmp308*tmp309); -double tmp311 = tmp305 + tmp306 + tmp307 + tmp310; -double tmp312 = -(tmp263*tmp265); -double tmp313 = tmp266*tmp267; -double tmp314 = tmp312 + tmp313; -double tmp315 = tmp78*tmp289; -double tmp316 = (tmp270*tmp290*tmp76*tmp275)/2.; -double tmp317 = mYaw(1+_OFF); -double tmp318 = z(4+_OFF); -double tmp319 = tmp317*tmp318; -double tmp320 = tmp315 + tmp316 + tmp319; -double tmp321 = -(tmp271*tmp136*tmp143); -double tmp322 = -(tmp270*tmp272*tmp72*tmp140*tmp273)/4.; -double tmp323 = (tmp270*tmp274*tmp72*tmp140*tmp275)/4.; -double tmp324 = tmp321 + tmp322 + tmp323; -double tmp325 = 0.5*tmp53*tmp270*tmp75*tmp324; -double tmp326 = 0.5*tmp53*tmp270*tmp72*tmp300; -double tmp327 = tmp325 + tmp326; -double tmp328 = -(tmp286*tmp139); -double tmp329 = -(tmp285*tmp287*tmp72*tmp140*tmp288)/4.; -double tmp330 = tmp328 + tmp329; -double tmp331 = 0.5*tmp285*tmp53*tmp75*tmp330; -double tmp332 = 0.5*tmp285*tmp53*tmp72*tmp311; -double tmp333 = tmp331 + tmp332; -double tmp334 = tmp289*tmp136*tmp143; -double tmp335 = -(tmp270*tmp290*tmp72*tmp140*tmp275)/4.; -double tmp336 = tmp334 + tmp335; -double tmp337 = 0.5*tmp53*tmp270*tmp75*tmp336; -double tmp338 = 0.5*tmp53*tmp270*tmp72*tmp320; -double tmp339 = tmp337 + tmp338; -double tmp340 = -(tmp271*tmp136*tmp168); -double tmp341 = -(tmp270*tmp272*tmp159*tmp140*tmp273)/4.; -double tmp342 = (tmp270*tmp274*tmp159*tmp140*tmp275)/4.; -double tmp343 = tmp340 + tmp341 + tmp342; -double tmp344 = 0.5*tmp53*tmp270*tmp75*tmp343; -double tmp345 = 0.5*tmp53*tmp270*tmp159*tmp300; -double tmp346 = tmp344 + tmp345; -double tmp347 = tmp268*tmp264; -double tmp348 = -tmp277; -double tmp349 = tmp347 + tmp348; -double tmp350 = -(tmp286*tmp165); -double tmp351 = -(tmp285*tmp287*tmp159*tmp140*tmp288)/4.; -double tmp352 = tmp350 + tmp351; -double tmp353 = 0.5*tmp285*tmp53*tmp75*tmp352; -double tmp354 = 0.5*tmp285*tmp53*tmp159*tmp311; -double tmp355 = tmp353 + tmp354; -double tmp356 = tmp265*tmp266; -double tmp357 = -(tmp268*tmp267); -double tmp358 = tmp356 + tmp357; -double tmp359 = tmp289*tmp136*tmp168; -double tmp360 = -(tmp270*tmp290*tmp159*tmp140*tmp275)/4.; -double tmp361 = tmp359 + tmp360; -double tmp362 = 0.5*tmp53*tmp270*tmp75*tmp361; -double tmp363 = 0.5*tmp53*tmp270*tmp159*tmp320; -double tmp364 = tmp362 + tmp363; -double tmp365 = -(tmp271*tmp136*tmp190); -double tmp366 = -(tmp270*tmp272*tmp178*tmp140*tmp273)/4.; -double tmp367 = (tmp270*tmp274*tmp178*tmp140*tmp275)/4.; -double tmp368 = tmp365 + tmp366 + tmp367; -double tmp369 = 0.5*tmp53*tmp270*tmp75*tmp368; -double tmp370 = 0.5*tmp53*tmp270*tmp178*tmp300; -double tmp371 = tmp369 + tmp370; -double tmp372 = -(tmp286*tmp187); -double tmp373 = -(tmp285*tmp287*tmp178*tmp140*tmp288)/4.; -double tmp374 = tmp372 + tmp373; -double tmp375 = 0.5*tmp285*tmp53*tmp75*tmp374; -double tmp376 = 0.5*tmp285*tmp53*tmp178*tmp311; -double tmp377 = tmp375 + tmp376; -double tmp378 = tmp289*tmp136*tmp190; -double tmp379 = -(tmp270*tmp290*tmp178*tmp140*tmp275)/4.; -double tmp380 = tmp378 + tmp379; -double tmp381 = 0.5*tmp53*tmp270*tmp75*tmp380; -double tmp382 = 0.5*tmp53*tmp270*tmp178*tmp320; -double tmp383 = tmp381 + tmp382; -double tmp384 = -(tmp271*tmp136*tmp206); -double tmp385 = -(tmp270*tmp272*tmp194*tmp140*tmp273)/4.; -double tmp386 = (tmp270*tmp274*tmp194*tmp140*tmp275)/4.; -double tmp387 = tmp384 + tmp385 + tmp386; -double tmp388 = 0.5*tmp53*tmp270*tmp75*tmp387; -double tmp389 = 0.5*tmp53*tmp270*tmp194*tmp300; -double tmp390 = tmp388 + tmp389; -double tmp391 = -(tmp286*tmp203); -double tmp392 = -(tmp285*tmp287*tmp194*tmp140*tmp288)/4.; -double tmp393 = tmp391 + tmp392; -double tmp394 = 0.5*tmp285*tmp53*tmp75*tmp393; -double tmp395 = 0.5*tmp285*tmp53*tmp194*tmp311; -double tmp396 = tmp394 + tmp395; -double tmp397 = tmp289*tmp136*tmp206; -double tmp398 = -(tmp270*tmp290*tmp194*tmp140*tmp275)/4.; -double tmp399 = tmp397 + tmp398; -double tmp400 = 0.5*tmp53*tmp270*tmp75*tmp399; -double tmp401 = 0.5*tmp53*tmp270*tmp194*tmp320; -double tmp402 = tmp400 + tmp401; -double tmp403 = tmp268*tmp263; -double tmp404 = -tmp279; -double tmp405 = tmp403 + tmp404; -J(1+_OFF,1+_OFF) = tmp256 - tmp147*(-0.5*tmp53*tmp104*tmp56*tmp120*tmp72 + 0.25*tmp78*tmp53*tmp79*tmp88*tmp98*tmp56*tmp72*tmp76 - 0.25*tmp78*tmp53*tmp79*tmp88*tmp98*tmp110*tmp111*tmp99 + 0.5*tmp78*tmp53*tmp79*tmp151*tmp88*tmp56*tmp99 + tmp257 + tmp258 + 0.25*tmp53*tmp104*tmp110*tmp111*tmp120*tmp75 - 0.5*tmp53*tmp102*tmp56*tmp120*tmp139*tmp75 + 0.25*tmp53*tmp132*tmp88*tmp56*tmp75*tmp210*tmp136 + 0.5*tmp53*tmp132*tmp88*tmp56*tmp72*tmp135 - 0.25*tmp53*tmp132*tmp88*tmp110*tmp111*tmp75*tmp135 + tmp259 + 0.5*tmp53*tmp79*tmp88*tmp98*tmp56*tmp99*tmp136*tmp143 + 0.5*tmp53*tmp88*tmp56*tmp75*tmp135*tmp223 + 1.*tmp169*(tmp170*tmp171*tmp72 + (tmp172*tmp144*tmp173*tmp72*tmp76*tmp174)/2.)); -J(1+_OFF,2+_OFF) = tmp231 - tmp147*(-0.5*tmp53*tmp104*tmp56*tmp120*tmp159 + 0.25*tmp78*tmp53*tmp79*tmp88*tmp98*tmp56*tmp159*tmp76 - 0.25*tmp78*tmp53*tmp79*tmp88*tmp98*tmp161*tmp111*tmp99 + 0.5*tmp78*tmp53*tmp79*tmp108*tmp88*tmp56*tmp99 + tmp260 + tmp261 + 0.25*tmp53*tmp104*tmp161*tmp111*tmp120*tmp75 - 0.5*tmp53*tmp102*tmp56*tmp120*tmp165*tmp75 + 0.25*tmp53*tmp132*tmp88*tmp56*tmp75*tmp226*tmp136 + 0.5*tmp53*tmp132*tmp88*tmp56*tmp159*tmp135 - 0.25*tmp53*tmp132*tmp88*tmp161*tmp111*tmp75*tmp135 + tmp262 + 0.5*tmp53*tmp79*tmp88*tmp98*tmp56*tmp99*tmp136*tmp168 + 0.5*tmp53*tmp88*tmp56*tmp75*tmp135*tmp230 + 1.*tmp169*(tmp170*tmp171*tmp159 + (tmp172*tmp144*tmp173*tmp159*tmp76*tmp174)/2.)); -J(1+_OFF,3+_OFF) = -2*tmp145*tmp1 - tmp147*(-0.5*tmp53*tmp104*tmp56*tmp120*tmp178 + 0.25*tmp78*tmp53*tmp79*tmp88*tmp98*tmp56*tmp178*tmp76 - 0.25*tmp78*tmp53*tmp79*tmp88*tmp98*tmp184*tmp111*tmp99 + 0.5*tmp78*tmp53*tmp79*tmp198*tmp88*tmp56*tmp99 + 0.5*tmp78*tmp53*tmp79*tmp108*tmp98*tmp56*tmp99 - 0.5*tmp53*tmp104*tmp182*tmp56*tmp75 + 0.25*tmp53*tmp104*tmp184*tmp111*tmp120*tmp75 - 0.5*tmp53*tmp102*tmp56*tmp120*tmp187*tmp75 + 0.25*tmp53*tmp132*tmp88*tmp56*tmp75*tmp234*tmp136 + 0.5*tmp53*tmp132*tmp88*tmp56*tmp178*tmp135 - 0.25*tmp53*tmp132*tmp88*tmp184*tmp111*tmp75*tmp135 + 0.5*tmp53*tmp132*tmp108*tmp56*tmp75*tmp135 + 0.5*tmp53*tmp79*tmp88*tmp98*tmp56*tmp99*tmp136*tmp190 + 0.5*tmp53*tmp88*tmp56*tmp75*tmp135*tmp238 + 1.*tmp169*(tmp170*tmp171*tmp178 + (tmp172*tmp144*tmp173*tmp178*tmp76*tmp174)/2.)); -J(1+_OFF,4+_OFF) = tmp207 - tmp147*(-0.5*tmp53*tmp104*tmp56*tmp120*tmp194 + 0.25*tmp78*tmp53*tmp79*tmp88*tmp98*tmp56*tmp194*tmp76 - 0.25*tmp78*tmp53*tmp79*tmp88*tmp98*tmp200*tmp111*tmp99 + 0.5*tmp78*tmp53*tmp79*tmp60*tmp88*tmp56*tmp99 + 0.5*tmp78*tmp53*tmp79*tmp65*tmp98*tmp56*tmp99 - 0.5*tmp53*tmp104*tmp198*tmp56*tmp75 + 0.25*tmp53*tmp104*tmp200*tmp111*tmp120*tmp75 - 0.5*tmp53*tmp102*tmp56*tmp120*tmp203*tmp75 + 0.25*tmp53*tmp132*tmp88*tmp56*tmp75*tmp241*tmp136 + 0.5*tmp53*tmp132*tmp88*tmp56*tmp194*tmp135 - 0.25*tmp53*tmp132*tmp88*tmp200*tmp111*tmp75*tmp135 + 0.5*tmp53*tmp132*tmp65*tmp56*tmp75*tmp135 + 0.5*tmp53*tmp79*tmp88*tmp98*tmp56*tmp99*tmp136*tmp206 + 0.5*tmp53*tmp88*tmp56*tmp75*tmp135*tmp245 + 1.*tmp169*(tmp170*tmp171*tmp194 + (tmp172*tmp144*tmp173*tmp194*tmp76*tmp174)/2.)); -J(2+_OFF,1+_OFF) = tmp207 - tmp147*(0.25*tmp53*tmp132*tmp219*tmp72*tmp76 + 0.5*tmp53*tmp132*tmp65*tmp99 + 0.25*tmp78*tmp53*tmp79*tmp75*tmp210*tmp136 + 0.5*tmp78*tmp53*tmp79*tmp72*tmp135 + 0.5*tmp53*tmp79*tmp75*tmp143 + 0.5*tmp53*tmp219*tmp99*tmp223); -J(2+_OFF,2+_OFF) = tmp246 - tmp147*(0.25*tmp53*tmp132*tmp219*tmp159*tmp76 + 0.5*tmp53*tmp132*tmp70*tmp99 + 0.25*tmp78*tmp53*tmp79*tmp75*tmp226*tmp136 + 0.5*tmp78*tmp53*tmp79*tmp159*tmp135 + 0.5*tmp53*tmp79*tmp75*tmp168 + 0.5*tmp53*tmp219*tmp99*tmp230); -J(2+_OFF,3+_OFF) = tmp231 - tmp147*(0.25*tmp53*tmp132*tmp219*tmp178*tmp76 + 0.5*tmp53*tmp132*tmp155*tmp99 + 0.25*tmp78*tmp53*tmp79*tmp75*tmp234*tmp136 + 0.5*tmp78*tmp53*tmp79*tmp178*tmp135 + 0.5*tmp53*tmp79*tmp75*tmp190 + 0.5*tmp53*tmp219*tmp99*tmp238); -J(2+_OFF,4+_OFF) = 2*tmp145*tmp0 - tmp147*(0.25*tmp53*tmp132*tmp219*tmp194*tmp76 + 0.5*tmp53*tmp132*tmp182*tmp99 + 0.25*tmp78*tmp53*tmp79*tmp75*tmp241*tmp136 + 0.5*tmp78*tmp53*tmp79*tmp194*tmp135 + 0.5*tmp53*tmp79*tmp75*tmp206 + 0.5*tmp53*tmp219*tmp99*tmp245); -J(3+_OFF,1+_OFF) = tmp246 - tmp147*(-0.5*tmp53*tmp104*tmp88*tmp56*tmp72 + 0.25*tmp78*tmp53*tmp79*tmp98*tmp56*tmp255*tmp72*tmp76 + 0.5*tmp78*tmp53*tmp79*tmp70*tmp98*tmp56*tmp99 - 0.25*tmp78*tmp53*tmp79*tmp98*tmp110*tmp111*tmp255*tmp99 + 0.5*tmp78*tmp53*tmp79*tmp151*tmp56*tmp255*tmp99 + 0.25*tmp53*tmp104*tmp88*tmp110*tmp111*tmp75 - 0.5*tmp53*tmp104*tmp60*tmp56*tmp75 - 0.5*tmp53*tmp102*tmp88*tmp56*tmp139*tmp75 + 0.25*tmp53*tmp132*tmp56*tmp255*tmp75*tmp210*tmp136 + 0.5*tmp53*tmp132*tmp56*tmp255*tmp72*tmp135 + 0.5*tmp53*tmp132*tmp70*tmp56*tmp75*tmp135 - 0.25*tmp53*tmp132*tmp110*tmp111*tmp255*tmp75*tmp135 + 0.5*tmp53*tmp79*tmp98*tmp56*tmp255*tmp99*tmp136*tmp143 + 0.5*tmp53*tmp56*tmp255*tmp75*tmp135*tmp223); -J(3+_OFF,2+_OFF) = -2*tmp145*tmp2 - tmp147*(-0.5*tmp53*tmp104*tmp88*tmp56*tmp159 + 0.25*tmp78*tmp53*tmp79*tmp98*tmp56*tmp255*tmp159*tmp76 + 0.5*tmp78*tmp53*tmp79*tmp151*tmp98*tmp56*tmp99 - 0.25*tmp78*tmp53*tmp79*tmp98*tmp161*tmp111*tmp255*tmp99 + 0.5*tmp78*tmp53*tmp79*tmp108*tmp56*tmp255*tmp99 + 0.25*tmp53*tmp104*tmp88*tmp161*tmp111*tmp75 - 0.5*tmp53*tmp104*tmp155*tmp56*tmp75 - 0.5*tmp53*tmp102*tmp88*tmp56*tmp165*tmp75 + 0.25*tmp53*tmp132*tmp56*tmp255*tmp75*tmp226*tmp136 + 0.5*tmp53*tmp132*tmp56*tmp255*tmp159*tmp135 + 0.5*tmp53*tmp132*tmp151*tmp56*tmp75*tmp135 - 0.25*tmp53*tmp132*tmp161*tmp111*tmp255*tmp75*tmp135 + 0.5*tmp53*tmp79*tmp98*tmp56*tmp255*tmp99*tmp136*tmp168 + 0.5*tmp53*tmp56*tmp255*tmp75*tmp135*tmp230); -J(3+_OFF,3+_OFF) = tmp256 - tmp147*(-0.5*tmp53*tmp104*tmp88*tmp56*tmp178 + 0.25*tmp78*tmp53*tmp79*tmp98*tmp56*tmp255*tmp178*tmp76 + tmp257 - 0.25*tmp78*tmp53*tmp79*tmp98*tmp184*tmp111*tmp255*tmp99 + 0.5*tmp78*tmp53*tmp79*tmp198*tmp56*tmp255*tmp99 + 0.25*tmp53*tmp104*tmp88*tmp184*tmp111*tmp75 + tmp258 - 0.5*tmp53*tmp102*tmp88*tmp56*tmp187*tmp75 + 0.25*tmp53*tmp132*tmp56*tmp255*tmp75*tmp234*tmp136 + 0.5*tmp53*tmp132*tmp56*tmp255*tmp178*tmp135 + tmp259 - 0.25*tmp53*tmp132*tmp184*tmp111*tmp255*tmp75*tmp135 + 0.5*tmp53*tmp79*tmp98*tmp56*tmp255*tmp99*tmp136*tmp190 + 0.5*tmp53*tmp56*tmp255*tmp75*tmp135*tmp238); -J(3+_OFF,4+_OFF) = tmp231 - tmp147*(-0.5*tmp53*tmp104*tmp88*tmp56*tmp194 + 0.25*tmp78*tmp53*tmp79*tmp98*tmp56*tmp255*tmp194*tmp76 + tmp260 - 0.25*tmp78*tmp53*tmp79*tmp98*tmp200*tmp111*tmp255*tmp99 + 0.5*tmp78*tmp53*tmp79*tmp60*tmp56*tmp255*tmp99 + 0.25*tmp53*tmp104*tmp88*tmp200*tmp111*tmp75 + tmp261 - 0.5*tmp53*tmp102*tmp88*tmp56*tmp203*tmp75 + 0.25*tmp53*tmp132*tmp56*tmp255*tmp75*tmp241*tmp136 + 0.5*tmp53*tmp132*tmp56*tmp255*tmp194*tmp135 + tmp262 - 0.25*tmp53*tmp132*tmp200*tmp111*tmp255*tmp75*tmp135 + 0.5*tmp53*tmp79*tmp98*tmp56*tmp255*tmp99*tmp136*tmp206 + 0.5*tmp53*tmp56*tmp255*tmp75*tmp135*tmp245); -J(4+_OFF,1+_OFF) = -(tmp293*tmp284*tmp327) - tmp303*tmp284*tmp333 - tmp314*tmp284*tmp339; -J(4+_OFF,2+_OFF) = -(tmp293*tmp284*tmp346) - tmp303*tmp284*tmp355 - tmp314*tmp284*tmp364; -J(4+_OFF,3+_OFF) = -(tmp293*tmp284*tmp371) - tmp303*tmp284*tmp377 - tmp314*tmp284*tmp383; -J(4+_OFF,4+_OFF) = -(tmp293*tmp284*tmp390) - tmp303*tmp284*tmp396 - tmp314*tmp284*tmp402; -J(5+_OFF,1+_OFF) = -(tmp303*tmp284*tmp327) - tmp349*tmp284*tmp333 - tmp358*tmp284*tmp339; -J(5+_OFF,2+_OFF) = -(tmp303*tmp284*tmp346) - tmp349*tmp284*tmp355 - tmp358*tmp284*tmp364; -J(5+_OFF,3+_OFF) = -(tmp303*tmp284*tmp371) - tmp349*tmp284*tmp377 - tmp358*tmp284*tmp383; -J(5+_OFF,4+_OFF) = -(tmp303*tmp284*tmp390) - tmp349*tmp284*tmp396 - tmp358*tmp284*tmp402; -J(6+_OFF,1+_OFF) = -(tmp314*tmp284*tmp327) - tmp358*tmp284*tmp333 - tmp405*tmp284*tmp339; -J(6+_OFF,2+_OFF) = -(tmp314*tmp284*tmp346) - tmp358*tmp284*tmp355 - tmp405*tmp284*tmp364; -J(6+_OFF,3+_OFF) = -(tmp314*tmp284*tmp371) - tmp358*tmp284*tmp377 - tmp405*tmp284*tmp383; -J(6+_OFF,4+_OFF) = -(tmp314*tmp284*tmp390) - tmp358*tmp284*tmp396 - tmp405*tmp284*tmp402; +double tmp183 = tmp182*tmp110; +double tmp184 = tmp173 + tmp178 + tmp183; +double tmp185 = -(tmp87*tmp184); +double tmp186 = tmp168 + tmp185; +double tmp187 = ibd(2+_OFF); +double tmp188 = ibod(1+_OFF); +double tmp189 = ibd(3+_OFF); +double tmp190 = ibod(2+_OFF); +double tmp191 = ibod(3+_OFF); +double tmp192 = ibd(1+_OFF); +double tmp193 = cp(3+_OFF); +double tmp194 = cBar(1+_OFF); +double tmp195 = x(39+_OFF); +double tmp196 = tmp192*tmp187*tmp189; +double tmp197 = std::pow(tmp188,2); +double tmp198 = -(tmp187*tmp197); +double tmp199 = std::pow(tmp190,2); +double tmp200 = -(tmp189*tmp199); +double tmp201 = 2*tmp188*tmp190*tmp191; +double tmp202 = std::pow(tmp191,2); +double tmp203 = -(tmp192*tmp202); +double tmp204 = tmp196 + tmp198 + tmp200 + tmp201 + tmp203; +double tmp205 = 1/tmp204; +double tmp206 = mRoll(2+_OFF); +double tmp207 = -(tmp55*tmp206); +double tmp208 = mRoll(3+_OFF); +double tmp209 = x(37+_OFF); +double tmp210 = (tmp193*tmp208*tmp53*tmp209)/2.; +double tmp211 = mRoll(4+_OFF); +double tmp212 = -0.5*(tmp193*tmp211*tmp53*tmp195); +double tmp213 = mRoll(1+_OFF); +double tmp214 = z(2+_OFF); +double tmp215 = tmp213*tmp214; +double tmp216 = tmp207 + tmp210 + tmp212 + tmp215; +double tmp217 = mPitch(1+_OFF); +double tmp218 = -tmp217; +double tmp219 = mPitch(3+_OFF); +double tmp220 = tmp38*tmp219; +double tmp221 = mPitch(4+_OFF); +double tmp222 = x(38+_OFF); +double tmp223 = (tmp194*tmp221*tmp53*tmp222)/2.; +double tmp224 = mPitch(2+_OFF); +double tmp225 = z(3+_OFF); +double tmp226 = -(tmp224*tmp225); +double tmp227 = tmp218 + tmp220 + tmp223 + tmp226; +double tmp228 = mYaw(2+_OFF); +double tmp229 = tmp55*tmp228; +double tmp230 = mYaw(3+_OFF); +double tmp231 = (tmp193*tmp230*tmp53*tmp195)/2.; +double tmp232 = mYaw(1+_OFF); +double tmp233 = z(4+_OFF); +double tmp234 = -(tmp232*tmp233); +double tmp235 = tmp229 + tmp231 + tmp234; +double tmp236 = -(tmp189*tmp190); +double tmp237 = tmp188*tmp191; +double tmp238 = tmp236 + tmp237; +double tmp239 = 0.634*tmp56*tmp193*tmp163*tmp52*tmp216; +double tmp240 = 0.634*tmp194*tmp56*tmp153*tmp52*tmp227; +double tmp241 = 0.634*tmp56*tmp193*tmp158*tmp52*tmp235; +double tmp242 = tmp239 + tmp240 + tmp241; +double tmp243 = -(tmp187*tmp188); +double tmp244 = tmp190*tmp191; +double tmp245 = tmp243 + tmp244; +double tmp246 = -4*tmp57*tmp19; +double tmp247 = -4*tmp91*tmp22; +double tmp248 = 2*tmp58*tmp24; +double tmp249 = tmp246 + tmp247 + tmp248; +double tmp250 = 4*tmp58*tmp40; +double tmp251 = -4*tmp91*tmp42; +double tmp252 = -2*tmp57*tmp43; +double tmp253 = tmp250 + tmp251 + tmp252; +double tmp254 = 4*tmp58*tmp29; +double tmp255 = -4*tmp57*tmp32; +double tmp256 = -2*tmp91*tmp34; +double tmp257 = tmp254 + tmp255 + tmp256; +double tmp258 = tmp187*tmp189; +double tmp259 = -tmp202; +double tmp260 = tmp258 + tmp259; +double tmp261 = 4*tmp91*tmp19; +double tmp262 = -4*tmp57*tmp22; +double tmp263 = -2*tmp0*tmp24; +double tmp264 = tmp261 + tmp262 + tmp263; +double tmp265 = -4*tmp0*tmp40; +double tmp266 = -4*tmp57*tmp42; +double tmp267 = 2*tmp91*tmp43; +double tmp268 = tmp265 + tmp266 + tmp267; +double tmp269 = -4*tmp0*tmp29; +double tmp270 = 4*tmp91*tmp32; +double tmp271 = -2*tmp57*tmp34; +double tmp272 = tmp269 + tmp270 + tmp271; +double tmp273 = 0.634*tmp56*tmp193*tmp182*tmp52*tmp216; +double tmp274 = 0.634*tmp194*tmp56*tmp172*tmp52*tmp227; +double tmp275 = 0.634*tmp56*tmp193*tmp177*tmp52*tmp235; +double tmp276 = tmp273 + tmp274 + tmp275; +double tmp277 = 0.634*tmp56*tmp193*tmp144*tmp52*tmp216; +double tmp278 = 0.634*tmp194*tmp56*tmp134*tmp52*tmp227; +double tmp279 = 0.634*tmp56*tmp193*tmp139*tmp52*tmp235; +double tmp280 = tmp277 + tmp278 + tmp279; +double tmp281 = 0.634*tmp56*tmp193*tmp125*tmp52*tmp216; +double tmp282 = 0.634*tmp194*tmp56*tmp115*tmp52*tmp227; +double tmp283 = 0.634*tmp56*tmp193*tmp120*tmp52*tmp235; +double tmp284 = tmp281 + tmp282 + tmp283; +double tmp285 = -4*tmp58*tmp19; +double tmp286 = 4*tmp0*tmp22; +double tmp287 = -2*tmp57*tmp24; +double tmp288 = tmp285 + tmp286 + tmp287; +double tmp289 = -4*tmp57*tmp40; +double tmp290 = 4*tmp0*tmp42; +double tmp291 = -2*tmp58*tmp43; +double tmp292 = tmp289 + tmp290 + tmp291; +double tmp293 = -4*tmp57*tmp29; +double tmp294 = -4*tmp58*tmp32; +double tmp295 = 2*tmp0*tmp34; +double tmp296 = tmp293 + tmp294 + tmp295; +double tmp297 = tmp192*tmp189; +double tmp298 = -tmp197; +double tmp299 = tmp297 + tmp298; +double tmp300 = tmp188*tmp190; +double tmp301 = -(tmp192*tmp191); +double tmp302 = tmp300 + tmp301; +double tmp303 = 0.634*tmp56*tmp193*tmp249*tmp52*tmp216; +double tmp304 = 0.634*tmp194*tmp56*tmp253*tmp52*tmp227; +double tmp305 = 0.634*tmp56*tmp193*tmp257*tmp52*tmp235; +double tmp306 = tmp303 + tmp304 + tmp305; +double tmp307 = -(tmp238*tmp205*tmp280); +double tmp308 = 0.634*tmp56*tmp193*tmp264*tmp52*tmp216; +double tmp309 = 0.634*tmp194*tmp56*tmp268*tmp52*tmp227; +double tmp310 = 0.634*tmp56*tmp193*tmp272*tmp52*tmp235; +double tmp311 = tmp308 + tmp309 + tmp310; +double tmp312 = -(tmp238*tmp205*tmp284); +double tmp313 = 0.634*tmp56*tmp193*tmp288*tmp52*tmp216; +double tmp314 = 0.634*tmp194*tmp56*tmp292*tmp52*tmp227; +double tmp315 = 0.634*tmp56*tmp193*tmp296*tmp52*tmp235; +double tmp316 = tmp313 + tmp314 + tmp315; +double tmp317 = -(tmp245*tmp205*tmp276); +double tmp318 = -(tmp302*tmp205*tmp242); +double tmp319 = tmp192*tmp187; +double tmp320 = -tmp199; +double tmp321 = tmp319 + tmp320; +double tmp322 = -(tmp245*tmp205*tmp280); +double tmp323 = -(tmp302*tmp205*tmp280); +J(1+_OFF,1+_OFF) = tmp186; +J(1+_OFF,2+_OFF) = tmp148; +J(1+_OFF,3+_OFF) = -2*tmp85*tmp57 - tmp87*(tmp268*tmp90 + tmp272*tmp95 + tmp264*tmp110); +J(1+_OFF,4+_OFF) = tmp129; +J(2+_OFF,1+_OFF) = tmp129; +J(2+_OFF,2+_OFF) = tmp167; +J(2+_OFF,3+_OFF) = tmp148; +J(2+_OFF,4+_OFF) = 2*tmp85*tmp0 - tmp87*(tmp292*tmp90 + tmp296*tmp95 + tmp288*tmp110); +J(3+_OFF,1+_OFF) = tmp167; +J(3+_OFF,2+_OFF) = -2*tmp85*tmp91 - tmp87*(tmp253*tmp90 + tmp257*tmp95 + tmp249*tmp110); +J(3+_OFF,3+_OFF) = tmp186; +J(3+_OFF,4+_OFF) = tmp148; +J(4+_OFF,1+_OFF) = -(tmp245*tmp205*tmp242) + tmp312 - tmp260*tmp205*tmp276; +J(4+_OFF,2+_OFF) = -(tmp238*tmp205*tmp242) - tmp245*tmp205*tmp306 - tmp260*tmp205*tmp280; +J(4+_OFF,3+_OFF) = -(tmp260*tmp205*tmp311) + tmp317 + tmp307; +J(4+_OFF,4+_OFF) = -(tmp260*tmp205*tmp284) - tmp238*tmp205*tmp316 + tmp322; +J(5+_OFF,1+_OFF) = tmp318 - tmp299*tmp205*tmp284 - tmp238*tmp205*tmp276; +J(5+_OFF,2+_OFF) = -(tmp299*tmp205*tmp242) - tmp302*tmp205*tmp306 + tmp307; +J(5+_OFF,3+_OFF) = -(tmp238*tmp205*tmp311) - tmp302*tmp205*tmp276 - tmp299*tmp205*tmp280; +J(5+_OFF,4+_OFF) = tmp312 - tmp299*tmp205*tmp316 + tmp323; +J(6+_OFF,1+_OFF) = -(tmp321*tmp205*tmp242) - tmp302*tmp205*tmp284 + tmp317; +J(6+_OFF,2+_OFF) = tmp318 - tmp321*tmp205*tmp306 + tmp322; +J(6+_OFF,3+_OFF) = -(tmp245*tmp205*tmp311) - tmp321*tmp205*tmp276 + tmp323; +J(6+_OFF,4+_OFF) = -(tmp245*tmp205*tmp284) - tmp302*tmp205*tmp316 - tmp321*tmp205*tmp280; diff --git a/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrQprev.cppready b/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrQprev.cppready new file mode 100644 index 0000000..4a47d7d --- /dev/null +++ b/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrQprev.cppready @@ -0,0 +1,776 @@ +double tmp0 = x(4+_OFF); +double tmp1 = x(6+_OFF); +double tmp2 = x(5+_OFF); +double tmp3 = x(7+_OFF); +double tmp4 = x(31+_OFF); +double tmp5 = x(30+_OFF); +double tmp6 = x(33+_OFF); +double tmp7 = x(32+_OFF); +double tmp8 = wind(1+_OFF); +double tmp9 = tmp4*tmp6; +double tmp10 = wind(2+_OFF); +double tmp11 = wind(3+_OFF); +double tmp12 = std::pow(tmp5,2); +double tmp13 = std::pow(tmp4,2); +double tmp14 = std::pow(tmp7,2); +double tmp15 = -tmp14; +double tmp16 = std::pow(tmp6,2); +double tmp17 = tmp4*tmp7; +double tmp18 = tmp5*tmp6; +double tmp19 = tmp17 + tmp18; +double tmp20 = -2*tmp10*tmp19; +double tmp21 = -(tmp5*tmp7); +double tmp22 = tmp21 + tmp9; +double tmp23 = -2*tmp11*tmp22; +double tmp24 = -tmp16; +double tmp25 = tmp12 + tmp13 + tmp15 + tmp24; +double tmp26 = -(tmp8*tmp25); +double tmp27 = x(34+_OFF); +double tmp28 = tmp20 + tmp23 + tmp26 + tmp27; +double tmp29 = tmp5*tmp7; +double tmp30 = tmp29 + tmp9; +double tmp31 = -2*tmp8*tmp30; +double tmp32 = -(tmp5*tmp4); +double tmp33 = tmp7*tmp6; +double tmp34 = tmp32 + tmp33; +double tmp35 = -2*tmp10*tmp34; +double tmp36 = -tmp13; +double tmp37 = tmp12 + tmp36 + tmp15 + tmp16; +double tmp38 = -(tmp11*tmp37); +double tmp39 = x(36+_OFF); +double tmp40 = tmp31 + tmp35 + tmp38 + tmp39; +double tmp41 = std::atan2(tmp40,tmp28); +double tmp42 = -(tmp5*tmp6); +double tmp43 = tmp17 + tmp42; +double tmp44 = -2*tmp8*tmp43; +double tmp45 = tmp5*tmp4; +double tmp46 = tmp45 + tmp33; +double tmp47 = -2*tmp11*tmp46; +double tmp48 = tmp12 + tmp36 + tmp14 + tmp24; +double tmp49 = -(tmp10*tmp48); +double tmp50 = x(35+_OFF); +double tmp51 = tmp44 + tmp47 + tmp49 + tmp50; +double tmp52 = std::pow(tmp28,2); +double tmp53 = std::pow(tmp51,2); +double tmp54 = std::pow(tmp40,2); +double tmp55 = tmp52 + tmp53 + tmp54; +double tmp56 = 1/std::sqrt(tmp55); +double tmp57 = tmp51*tmp56; +double tmp58 = std::asin(tmp57); +double tmp59 = cp(2+_OFF); +double tmp60 = -(tmp0*tmp1); +double tmp61 = tmp2*tmp3; +double tmp62 = tmp60 + tmp61; +double tmp63 = tmp2*tmp1; +double tmp64 = tmp0*tmp3; +double tmp65 = tmp63 + tmp64; +double tmp66 = std::pow(tmp0,2); +double tmp67 = std::pow(tmp2,2); +double tmp68 = std::pow(tmp1,2); +double tmp69 = -tmp68; +double tmp70 = std::pow(tmp3,2); +double tmp71 = -tmp70; +double tmp72 = tmp66 + tmp67 + tmp69 + tmp71; +double tmp73 = fLat(1+_OFF); +double tmp74 = std::sqrt(tmp55); +double tmp75 = tmp52 + tmp54; +double tmp76 = 1/std::sqrt(tmp75); +double tmp77 = fDrag(1+_OFF); +double tmp78 = fDrag(2+_OFF); +double tmp79 = -(tmp41*tmp78); +double tmp80 = std::pow(tmp41,2); +double tmp81 = fDrag(3+_OFF); +double tmp82 = tmp80*tmp81; +double tmp83 = std::pow(tmp58,2); +double tmp84 = fDrag(4+_OFF); +double tmp85 = tmp83*tmp84; +double tmp86 = tmp77 + tmp79 + tmp82 + tmp85; +double tmp87 = 1/tmp55; +double tmp88 = -(tmp53*tmp87); +double tmp89 = 1 + tmp88; +double tmp90 = std::sqrt(tmp89); +double tmp91 = -2*tmp10*tmp5; +double tmp92 = -2*tmp11*tmp4; +double tmp93 = 2*tmp8*tmp6; +double tmp94 = tmp91 + tmp92 + tmp93; +double tmp95 = -2*tmp8*tmp5; +double tmp96 = 2*tmp11*tmp7; +double tmp97 = -2*tmp10*tmp6; +double tmp98 = tmp95 + tmp96 + tmp97; +double tmp99 = 2*tmp98*tmp28; +double tmp100 = 2*tmp94*tmp51; +double tmp101 = -2*tmp11*tmp5; +double tmp102 = 2*tmp10*tmp4; +double tmp103 = -2*tmp8*tmp7; +double tmp104 = tmp101 + tmp102 + tmp103; +double tmp105 = 2*tmp104*tmp40; +double tmp106 = tmp99 + tmp100 + tmp105; +double tmp107 = 1/tmp75; +double tmp108 = tmp104*tmp28*tmp107; +double tmp109 = 2*tmp8*tmp30; +double tmp110 = 2*tmp10*tmp34; +double tmp111 = tmp11*tmp37; +double tmp112 = -tmp39; +double tmp113 = tmp109 + tmp110 + tmp111 + tmp112; +double tmp114 = tmp98*tmp113*tmp107; +double tmp115 = tmp108 + tmp114; +double tmp116 = 1/std::sqrt(tmp89); +double tmp117 = std::pow(tmp55,-1.5); +double tmp118 = -0.5*(tmp51*tmp106*tmp117); +double tmp119 = tmp94*tmp56; +double tmp120 = tmp118 + tmp119; +double tmp121 = fLift(1+_OFF); +double tmp122 = -tmp121; +double tmp123 = fLift(2+_OFF); +double tmp124 = tmp41*tmp123; +double tmp125 = tmp122 + tmp124; +double tmp126 = tmp99 + tmp105; +double tmp127 = std::pow(tmp75,-1.5); +double tmp128 = std::pow(tmp55,-2); +double tmp129 = tmp53*tmp106*tmp128; +double tmp130 = -2*tmp94*tmp51*tmp87; +double tmp131 = tmp129 + tmp130; +double tmp132 = -(tmp78*tmp115); +double tmp133 = 2*tmp41*tmp81*tmp115; +double tmp134 = 2*tmp58*tmp84*tmp116*tmp120; +double tmp135 = tmp132 + tmp133 + tmp134; +double tmp136 = cp(1+_OFF); +double tmp137 = std::pow(tmp136,2); +double tmp138 = std::pow(M_PI,-2); +double tmp139 = fThrust(3+_OFF); +double tmp140 = 1/M_PI; +double tmp141 = fThrust(2+_OFF); +double tmp142 = z(1+_OFF); +double tmp143 = cp(4+_OFF); +double tmp144 = 1/tmp143; +double tmp145 = 4*tmp62*tmp5; +double tmp146 = -4*tmp65*tmp4; +double tmp147 = 2*tmp72*tmp7; +double tmp148 = tmp145 + tmp146 + tmp147; +double tmp149 = 0.634*tmp59*tmp86*tmp51*tmp74; +double tmp150 = 0.634*tmp58*tmp59*tmp73*tmp55*tmp90; +double tmp151 = 0. + tmp149 + tmp150; +double tmp152 = -0.634*tmp58*tmp59*tmp73*tmp51*tmp40*tmp76*tmp74; +double tmp153 = 0.634*tmp59*tmp125*tmp28*tmp76*tmp55; +double tmp154 = 0.634*tmp59*tmp86*tmp40*tmp76*tmp55*tmp90; +double tmp155 = tmp152 + tmp153 + tmp154; +double tmp156 = 2*tmp72*tmp43; +double tmp157 = 4*tmp62*tmp46; +double tmp158 = 2*tmp65*tmp48; +double tmp159 = tmp156 + tmp157 + tmp158; +double tmp160 = -2*tmp8*tmp4; +double tmp161 = -2*tmp10*tmp7; +double tmp162 = -2*tmp11*tmp6; +double tmp163 = tmp160 + tmp161 + tmp162; +double tmp164 = 2*tmp163*tmp28; +double tmp165 = 2*tmp104*tmp51; +double tmp166 = 2*tmp10*tmp5; +double tmp167 = 2*tmp11*tmp4; +double tmp168 = -2*tmp8*tmp6; +double tmp169 = tmp166 + tmp167 + tmp168; +double tmp170 = 2*tmp169*tmp40; +double tmp171 = tmp164 + tmp165 + tmp170; +double tmp172 = tmp169*tmp28*tmp107; +double tmp173 = tmp163*tmp113*tmp107; +double tmp174 = tmp172 + tmp173; +double tmp175 = -0.5*(tmp51*tmp171*tmp117); +double tmp176 = tmp104*tmp56; +double tmp177 = tmp175 + tmp176; +double tmp178 = 2*tmp72*tmp30; +double tmp179 = 4*tmp65*tmp34; +double tmp180 = 2*tmp62*tmp37; +double tmp181 = tmp178 + tmp179 + tmp180; +double tmp182 = tmp164 + tmp170; +double tmp183 = tmp53*tmp171*tmp128; +double tmp184 = -2*tmp104*tmp51*tmp87; +double tmp185 = tmp183 + tmp184; +double tmp186 = -(tmp78*tmp174); +double tmp187 = 2*tmp41*tmp81*tmp174; +double tmp188 = 2*tmp58*tmp84*tmp116*tmp177; +double tmp189 = tmp186 + tmp187 + tmp188; +double tmp190 = 4*tmp65*tmp19; +double tmp191 = 4*tmp62*tmp22; +double tmp192 = tmp72*tmp25; +double tmp193 = tmp190 + tmp191 + tmp192; +double tmp194 = -0.634*tmp58*tmp59*tmp73*tmp28*tmp51*tmp76*tmp74; +double tmp195 = -0.634*tmp59*tmp125*tmp40*tmp76*tmp55; +double tmp196 = 0.634*tmp59*tmp86*tmp28*tmp76*tmp55*tmp90; +double tmp197 = tmp138*tmp139*tmp55; +double tmp198 = tmp140*tmp136*tmp141*tmp74*tmp142; +double tmp199 = fThrust(1+_OFF); +double tmp200 = std::pow(tmp142,2); +double tmp201 = tmp137*tmp199*tmp200; +double tmp202 = tmp197 + tmp198 + tmp201; +double tmp203 = 1.268*tmp137*tmp202; +double tmp204 = tmp194 + tmp195 + tmp196 + tmp203; +double tmp205 = 2*tmp72*tmp4; +double tmp206 = 4*tmp65*tmp7; +double tmp207 = 4*tmp62*tmp6; +double tmp208 = tmp205 + tmp206 + tmp207; +double tmp209 = 2*tmp72*tmp5; +double tmp210 = -4*tmp62*tmp7; +double tmp211 = 4*tmp65*tmp6; +double tmp212 = tmp209 + tmp210 + tmp211; +double tmp213 = 2*tmp11*tmp5; +double tmp214 = -2*tmp10*tmp4; +double tmp215 = 2*tmp8*tmp7; +double tmp216 = tmp213 + tmp214 + tmp215; +double tmp217 = 2*tmp216*tmp28; +double tmp218 = 2*tmp163*tmp51; +double tmp219 = 2*tmp98*tmp40; +double tmp220 = tmp217 + tmp218 + tmp219; +double tmp221 = tmp98*tmp28*tmp107; +double tmp222 = tmp216*tmp113*tmp107; +double tmp223 = tmp221 + tmp222; +double tmp224 = -0.5*(tmp51*tmp220*tmp117); +double tmp225 = tmp163*tmp56; +double tmp226 = tmp224 + tmp225; +double tmp227 = -0.634*tmp58*tmp59*tmp73*tmp98*tmp51*tmp76*tmp74; +double tmp228 = tmp217 + tmp219; +double tmp229 = tmp53*tmp220*tmp128; +double tmp230 = -2*tmp163*tmp51*tmp87; +double tmp231 = tmp229 + tmp230; +double tmp232 = 0.634*tmp59*tmp86*tmp98*tmp76*tmp55*tmp90; +double tmp233 = -(tmp78*tmp223); +double tmp234 = 2*tmp41*tmp81*tmp223; +double tmp235 = 2*tmp58*tmp84*tmp116*tmp226; +double tmp236 = tmp233 + tmp234 + tmp235; +double tmp237 = 2*tmp8*tmp5; +double tmp238 = -2*tmp11*tmp7; +double tmp239 = 2*tmp10*tmp6; +double tmp240 = tmp237 + tmp238 + tmp239; +double tmp241 = 2*tmp94*tmp28; +double tmp242 = 2*tmp240*tmp51; +double tmp243 = 2*tmp163*tmp40; +double tmp244 = tmp241 + tmp242 + tmp243; +double tmp245 = tmp163*tmp28*tmp107; +double tmp246 = tmp94*tmp113*tmp107; +double tmp247 = tmp245 + tmp246; +double tmp248 = -0.5*(tmp51*tmp244*tmp117); +double tmp249 = tmp240*tmp56; +double tmp250 = tmp248 + tmp249; +double tmp251 = -0.634*tmp58*tmp59*tmp73*tmp163*tmp51*tmp76*tmp74; +double tmp252 = tmp241 + tmp243; +double tmp253 = tmp53*tmp244*tmp128; +double tmp254 = -2*tmp240*tmp51*tmp87; +double tmp255 = tmp253 + tmp254; +double tmp256 = 0.634*tmp59*tmp86*tmp163*tmp76*tmp55*tmp90; +double tmp257 = -(tmp78*tmp247); +double tmp258 = 2*tmp41*tmp81*tmp247; +double tmp259 = 2*tmp58*tmp84*tmp116*tmp250; +double tmp260 = tmp257 + tmp258 + tmp259; +double tmp261 = 4*tmp65*tmp5; +double tmp262 = 4*tmp62*tmp4; +double tmp263 = -2*tmp72*tmp6; +double tmp264 = tmp261 + tmp262 + tmp263; +double tmp265 = tmp0*tmp2; +double tmp266 = tmp1*tmp3; +double tmp267 = tmp265 + tmp266; +double tmp268 = -tmp67; +double tmp269 = tmp66 + tmp268 + tmp68 + tmp71; +double tmp270 = -(tmp0*tmp3); +double tmp271 = tmp63 + tmp270; +double tmp272 = 0.317*tmp59*tmp86*tmp51*tmp106*tmp56; +double tmp273 = 0.634*tmp59*tmp86*tmp94*tmp74; +double tmp274 = 0.317*tmp58*tmp59*tmp73*tmp55*tmp131*tmp116; +double tmp275 = 0.634*tmp58*tmp59*tmp73*tmp106*tmp90; +double tmp276 = 0.634*tmp59*tmp73*tmp55*tmp120; +double tmp277 = 0.634*tmp59*tmp51*tmp74*tmp135; +double tmp278 = tmp272 + tmp273 + tmp274 + tmp275 + tmp276 + tmp277; +double tmp279 = 0.634*tmp59*tmp125*tmp28*tmp106*tmp76; +double tmp280 = -0.317*tmp58*tmp59*tmp73*tmp51*tmp40*tmp106*tmp76*tmp56; +double tmp281 = 0.317*tmp58*tmp59*tmp73*tmp51*tmp40*tmp126*tmp127*tmp74; +double tmp282 = -0.634*tmp58*tmp59*tmp73*tmp104*tmp51*tmp76*tmp74; +double tmp283 = -0.634*tmp58*tmp59*tmp73*tmp94*tmp40*tmp76*tmp74; +double tmp284 = -0.317*tmp59*tmp125*tmp28*tmp126*tmp127*tmp55; +double tmp285 = 0.634*tmp59*tmp125*tmp98*tmp76*tmp55; +double tmp286 = 0.634*tmp59*tmp123*tmp28*tmp76*tmp55*tmp115; +double tmp287 = 0.317*tmp59*tmp86*tmp40*tmp76*tmp55*tmp131*tmp116; +double tmp288 = 0.634*tmp59*tmp86*tmp40*tmp106*tmp76*tmp90; +double tmp289 = -0.317*tmp59*tmp86*tmp40*tmp126*tmp127*tmp55*tmp90; +double tmp290 = 0.634*tmp59*tmp86*tmp104*tmp76*tmp55*tmp90; +double tmp291 = -0.634*tmp59*tmp73*tmp51*tmp40*tmp76*tmp74*tmp116*tmp120; +double tmp292 = 0.634*tmp59*tmp40*tmp76*tmp55*tmp90*tmp135; +double tmp293 = tmp279 + tmp280 + tmp281 + tmp282 + tmp283 + tmp284 + tmp285 + tmp286 + tmp287 + tmp288 + tmp289 + tmp290 + tmp291 + tmp292; +double tmp294 = -0.634*tmp59*tmp125*tmp40*tmp106*tmp76; +double tmp295 = -0.317*tmp58*tmp59*tmp73*tmp28*tmp51*tmp106*tmp76*tmp56; +double tmp296 = 0.317*tmp58*tmp59*tmp73*tmp28*tmp51*tmp126*tmp127*tmp74; +double tmp297 = -0.634*tmp58*tmp59*tmp73*tmp94*tmp28*tmp76*tmp74; +double tmp298 = 0.317*tmp59*tmp125*tmp40*tmp126*tmp127*tmp55; +double tmp299 = -0.634*tmp59*tmp125*tmp104*tmp76*tmp55; +double tmp300 = -0.634*tmp59*tmp123*tmp40*tmp76*tmp55*tmp115; +double tmp301 = 0.317*tmp59*tmp86*tmp28*tmp76*tmp55*tmp131*tmp116; +double tmp302 = 0.634*tmp59*tmp86*tmp28*tmp106*tmp76*tmp90; +double tmp303 = -0.317*tmp59*tmp86*tmp28*tmp126*tmp127*tmp55*tmp90; +double tmp304 = -0.634*tmp59*tmp73*tmp28*tmp51*tmp76*tmp74*tmp116*tmp120; +double tmp305 = 0.634*tmp59*tmp28*tmp76*tmp55*tmp90*tmp135; +double tmp306 = tmp138*tmp139*tmp106; +double tmp307 = (tmp140*tmp136*tmp141*tmp106*tmp56*tmp142)/2.; +double tmp308 = tmp306 + tmp307; +double tmp309 = 1.268*tmp137*tmp308; +double tmp310 = tmp294 + tmp295 + tmp296 + tmp297 + tmp227 + tmp298 + tmp299 + tmp300 + tmp301 + tmp302 + tmp303 + tmp232 + tmp304 + tmp305 + tmp309; +double tmp311 = 4*tmp267*tmp5; +double tmp312 = -2*tmp269*tmp4; +double tmp313 = 4*tmp271*tmp7; +double tmp314 = tmp311 + tmp312 + tmp313; +double tmp315 = 4*tmp271*tmp43; +double tmp316 = 4*tmp267*tmp46; +double tmp317 = tmp269*tmp48; +double tmp318 = tmp315 + tmp316 + tmp317; +double tmp319 = 0.317*tmp59*tmp86*tmp51*tmp171*tmp56; +double tmp320 = 0.634*tmp59*tmp86*tmp104*tmp74; +double tmp321 = 0.317*tmp58*tmp59*tmp73*tmp55*tmp185*tmp116; +double tmp322 = 0.634*tmp58*tmp59*tmp73*tmp171*tmp90; +double tmp323 = 0.634*tmp59*tmp73*tmp55*tmp177; +double tmp324 = 0.634*tmp59*tmp51*tmp74*tmp189; +double tmp325 = tmp319 + tmp320 + tmp321 + tmp322 + tmp323 + tmp324; +double tmp326 = 4*tmp271*tmp30; +double tmp327 = 2*tmp269*tmp34; +double tmp328 = 2*tmp267*tmp37; +double tmp329 = tmp326 + tmp327 + tmp328; +double tmp330 = 0.634*tmp59*tmp125*tmp28*tmp171*tmp76; +double tmp331 = -0.317*tmp58*tmp59*tmp73*tmp51*tmp40*tmp171*tmp76*tmp56; +double tmp332 = 0.317*tmp58*tmp59*tmp73*tmp51*tmp40*tmp182*tmp127*tmp74; +double tmp333 = -0.634*tmp58*tmp59*tmp73*tmp169*tmp51*tmp76*tmp74; +double tmp334 = -0.634*tmp58*tmp59*tmp73*tmp104*tmp40*tmp76*tmp74; +double tmp335 = -0.317*tmp59*tmp125*tmp28*tmp182*tmp127*tmp55; +double tmp336 = 0.634*tmp59*tmp125*tmp163*tmp76*tmp55; +double tmp337 = 0.634*tmp59*tmp123*tmp28*tmp76*tmp55*tmp174; +double tmp338 = 0.317*tmp59*tmp86*tmp40*tmp76*tmp55*tmp185*tmp116; +double tmp339 = 0.634*tmp59*tmp86*tmp40*tmp171*tmp76*tmp90; +double tmp340 = -0.317*tmp59*tmp86*tmp40*tmp182*tmp127*tmp55*tmp90; +double tmp341 = 0.634*tmp59*tmp86*tmp169*tmp76*tmp55*tmp90; +double tmp342 = -0.634*tmp59*tmp73*tmp51*tmp40*tmp76*tmp74*tmp116*tmp177; +double tmp343 = 0.634*tmp59*tmp40*tmp76*tmp55*tmp90*tmp189; +double tmp344 = tmp330 + tmp331 + tmp332 + tmp333 + tmp334 + tmp335 + tmp336 + tmp337 + tmp338 + tmp339 + tmp340 + tmp341 + tmp342 + tmp343; +double tmp345 = 2*tmp269*tmp19; +double tmp346 = 4*tmp267*tmp22; +double tmp347 = 2*tmp271*tmp25; +double tmp348 = tmp345 + tmp346 + tmp347; +double tmp349 = -0.634*tmp59*tmp125*tmp40*tmp171*tmp76; +double tmp350 = -0.317*tmp58*tmp59*tmp73*tmp28*tmp51*tmp171*tmp76*tmp56; +double tmp351 = 0.317*tmp58*tmp59*tmp73*tmp28*tmp51*tmp182*tmp127*tmp74; +double tmp352 = -0.634*tmp58*tmp59*tmp73*tmp104*tmp28*tmp76*tmp74; +double tmp353 = 0.317*tmp59*tmp125*tmp40*tmp182*tmp127*tmp55; +double tmp354 = -0.634*tmp59*tmp125*tmp169*tmp76*tmp55; +double tmp355 = -0.634*tmp59*tmp123*tmp40*tmp76*tmp55*tmp174; +double tmp356 = 0.317*tmp59*tmp86*tmp28*tmp76*tmp55*tmp185*tmp116; +double tmp357 = 0.634*tmp59*tmp86*tmp28*tmp171*tmp76*tmp90; +double tmp358 = -0.317*tmp59*tmp86*tmp28*tmp182*tmp127*tmp55*tmp90; +double tmp359 = -0.634*tmp59*tmp73*tmp28*tmp51*tmp76*tmp74*tmp116*tmp177; +double tmp360 = 0.634*tmp59*tmp28*tmp76*tmp55*tmp90*tmp189; +double tmp361 = tmp138*tmp139*tmp171; +double tmp362 = (tmp140*tmp136*tmp141*tmp171*tmp56*tmp142)/2.; +double tmp363 = tmp361 + tmp362; +double tmp364 = 1.268*tmp137*tmp363; +double tmp365 = tmp349 + tmp350 + tmp351 + tmp352 + tmp251 + tmp353 + tmp354 + tmp355 + tmp356 + tmp357 + tmp358 + tmp256 + tmp359 + tmp360 + tmp364; +double tmp366 = 4*tmp271*tmp4; +double tmp367 = 2*tmp269*tmp7; +double tmp368 = 4*tmp267*tmp6; +double tmp369 = tmp366 + tmp367 + tmp368; +double tmp370 = 4*tmp271*tmp5; +double tmp371 = -4*tmp267*tmp7; +double tmp372 = 2*tmp269*tmp6; +double tmp373 = tmp370 + tmp371 + tmp372; +double tmp374 = 0.317*tmp59*tmp86*tmp51*tmp220*tmp56; +double tmp375 = 0.634*tmp59*tmp86*tmp163*tmp74; +double tmp376 = 0.317*tmp58*tmp59*tmp73*tmp55*tmp231*tmp116; +double tmp377 = 0.634*tmp58*tmp59*tmp73*tmp220*tmp90; +double tmp378 = 0.634*tmp59*tmp73*tmp55*tmp226; +double tmp379 = 0.634*tmp59*tmp51*tmp74*tmp236; +double tmp380 = tmp374 + tmp375 + tmp376 + tmp377 + tmp378 + tmp379; +double tmp381 = 0.634*tmp59*tmp125*tmp28*tmp220*tmp76; +double tmp382 = -0.317*tmp58*tmp59*tmp73*tmp51*tmp40*tmp220*tmp76*tmp56; +double tmp383 = 0.317*tmp58*tmp59*tmp73*tmp51*tmp40*tmp228*tmp127*tmp74; +double tmp384 = -0.634*tmp58*tmp59*tmp73*tmp163*tmp40*tmp76*tmp74; +double tmp385 = -0.317*tmp59*tmp125*tmp28*tmp228*tmp127*tmp55; +double tmp386 = 0.634*tmp59*tmp125*tmp216*tmp76*tmp55; +double tmp387 = 0.634*tmp59*tmp123*tmp28*tmp76*tmp55*tmp223; +double tmp388 = 0.317*tmp59*tmp86*tmp40*tmp76*tmp55*tmp231*tmp116; +double tmp389 = 0.634*tmp59*tmp86*tmp40*tmp220*tmp76*tmp90; +double tmp390 = -0.317*tmp59*tmp86*tmp40*tmp228*tmp127*tmp55*tmp90; +double tmp391 = -0.634*tmp59*tmp73*tmp51*tmp40*tmp76*tmp74*tmp116*tmp226; +double tmp392 = 0.634*tmp59*tmp40*tmp76*tmp55*tmp90*tmp236; +double tmp393 = tmp381 + tmp382 + tmp383 + tmp227 + tmp384 + tmp385 + tmp386 + tmp387 + tmp388 + tmp389 + tmp390 + tmp232 + tmp391 + tmp392; +double tmp394 = -0.634*tmp59*tmp125*tmp40*tmp220*tmp76; +double tmp395 = -0.317*tmp58*tmp59*tmp73*tmp28*tmp51*tmp220*tmp76*tmp56; +double tmp396 = 0.317*tmp58*tmp59*tmp73*tmp28*tmp51*tmp228*tmp127*tmp74; +double tmp397 = -0.634*tmp58*tmp59*tmp73*tmp163*tmp28*tmp76*tmp74; +double tmp398 = -0.634*tmp58*tmp59*tmp73*tmp216*tmp51*tmp76*tmp74; +double tmp399 = 0.317*tmp59*tmp125*tmp40*tmp228*tmp127*tmp55; +double tmp400 = -0.634*tmp59*tmp125*tmp98*tmp76*tmp55; +double tmp401 = -0.634*tmp59*tmp123*tmp40*tmp76*tmp55*tmp223; +double tmp402 = 0.317*tmp59*tmp86*tmp28*tmp76*tmp55*tmp231*tmp116; +double tmp403 = 0.634*tmp59*tmp86*tmp28*tmp220*tmp76*tmp90; +double tmp404 = -0.317*tmp59*tmp86*tmp28*tmp228*tmp127*tmp55*tmp90; +double tmp405 = 0.634*tmp59*tmp86*tmp216*tmp76*tmp55*tmp90; +double tmp406 = -0.634*tmp59*tmp73*tmp28*tmp51*tmp76*tmp74*tmp116*tmp226; +double tmp407 = 0.634*tmp59*tmp28*tmp76*tmp55*tmp90*tmp236; +double tmp408 = tmp138*tmp139*tmp220; +double tmp409 = (tmp140*tmp136*tmp141*tmp220*tmp56*tmp142)/2.; +double tmp410 = tmp408 + tmp409; +double tmp411 = 1.268*tmp137*tmp410; +double tmp412 = tmp394 + tmp395 + tmp396 + tmp397 + tmp398 + tmp399 + tmp400 + tmp401 + tmp402 + tmp403 + tmp404 + tmp405 + tmp406 + tmp407 + tmp411; +double tmp413 = 0.317*tmp59*tmp86*tmp51*tmp244*tmp56; +double tmp414 = 0.634*tmp59*tmp86*tmp240*tmp74; +double tmp415 = 0.317*tmp58*tmp59*tmp73*tmp55*tmp255*tmp116; +double tmp416 = 0.634*tmp58*tmp59*tmp73*tmp244*tmp90; +double tmp417 = 0.634*tmp59*tmp73*tmp55*tmp250; +double tmp418 = 0.634*tmp59*tmp51*tmp74*tmp260; +double tmp419 = tmp413 + tmp414 + tmp415 + tmp416 + tmp417 + tmp418; +double tmp420 = 0.634*tmp59*tmp125*tmp28*tmp244*tmp76; +double tmp421 = -0.317*tmp58*tmp59*tmp73*tmp51*tmp40*tmp244*tmp76*tmp56; +double tmp422 = 0.317*tmp58*tmp59*tmp73*tmp51*tmp40*tmp252*tmp127*tmp74; +double tmp423 = -0.634*tmp58*tmp59*tmp73*tmp240*tmp40*tmp76*tmp74; +double tmp424 = -0.317*tmp59*tmp125*tmp28*tmp252*tmp127*tmp55; +double tmp425 = 0.634*tmp59*tmp125*tmp94*tmp76*tmp55; +double tmp426 = 0.634*tmp59*tmp123*tmp28*tmp76*tmp55*tmp247; +double tmp427 = 0.317*tmp59*tmp86*tmp40*tmp76*tmp55*tmp255*tmp116; +double tmp428 = 0.634*tmp59*tmp86*tmp40*tmp244*tmp76*tmp90; +double tmp429 = -0.317*tmp59*tmp86*tmp40*tmp252*tmp127*tmp55*tmp90; +double tmp430 = -0.634*tmp59*tmp73*tmp51*tmp40*tmp76*tmp74*tmp116*tmp250; +double tmp431 = 0.634*tmp59*tmp40*tmp76*tmp55*tmp90*tmp260; +double tmp432 = tmp420 + tmp421 + tmp422 + tmp251 + tmp423 + tmp424 + tmp425 + tmp426 + tmp427 + tmp428 + tmp429 + tmp256 + tmp430 + tmp431; +double tmp433 = -0.634*tmp59*tmp125*tmp40*tmp244*tmp76; +double tmp434 = -0.317*tmp58*tmp59*tmp73*tmp28*tmp51*tmp244*tmp76*tmp56; +double tmp435 = 0.317*tmp58*tmp59*tmp73*tmp28*tmp51*tmp252*tmp127*tmp74; +double tmp436 = -0.634*tmp58*tmp59*tmp73*tmp240*tmp28*tmp76*tmp74; +double tmp437 = -0.634*tmp58*tmp59*tmp73*tmp94*tmp51*tmp76*tmp74; +double tmp438 = 0.317*tmp59*tmp125*tmp40*tmp252*tmp127*tmp55; +double tmp439 = -0.634*tmp59*tmp125*tmp163*tmp76*tmp55; +double tmp440 = -0.634*tmp59*tmp123*tmp40*tmp76*tmp55*tmp247; +double tmp441 = 0.317*tmp59*tmp86*tmp28*tmp76*tmp55*tmp255*tmp116; +double tmp442 = 0.634*tmp59*tmp86*tmp28*tmp244*tmp76*tmp90; +double tmp443 = -0.317*tmp59*tmp86*tmp28*tmp252*tmp127*tmp55*tmp90; +double tmp444 = 0.634*tmp59*tmp86*tmp94*tmp76*tmp55*tmp90; +double tmp445 = -0.634*tmp59*tmp73*tmp28*tmp51*tmp76*tmp74*tmp116*tmp250; +double tmp446 = 0.634*tmp59*tmp28*tmp76*tmp55*tmp90*tmp260; +double tmp447 = tmp138*tmp139*tmp244; +double tmp448 = (tmp140*tmp136*tmp141*tmp244*tmp56*tmp142)/2.; +double tmp449 = tmp447 + tmp448; +double tmp450 = 1.268*tmp137*tmp449; +double tmp451 = tmp433 + tmp434 + tmp435 + tmp436 + tmp437 + tmp438 + tmp439 + tmp440 + tmp441 + tmp442 + tmp443 + tmp444 + tmp445 + tmp446 + tmp450; +double tmp452 = 2*tmp269*tmp5; +double tmp453 = 4*tmp267*tmp4; +double tmp454 = -4*tmp271*tmp6; +double tmp455 = tmp452 + tmp453 + tmp454; +double tmp456 = tmp66 + tmp268 + tmp69 + tmp70; +double tmp457 = -(tmp0*tmp2); +double tmp458 = tmp457 + tmp266; +double tmp459 = tmp0*tmp1; +double tmp460 = tmp459 + tmp61; +double tmp461 = 2*tmp456*tmp5; +double tmp462 = -4*tmp458*tmp4; +double tmp463 = 4*tmp460*tmp7; +double tmp464 = tmp461 + tmp462 + tmp463; +double tmp465 = 4*tmp460*tmp43; +double tmp466 = 2*tmp456*tmp46; +double tmp467 = 2*tmp458*tmp48; +double tmp468 = tmp465 + tmp466 + tmp467; +double tmp469 = 4*tmp460*tmp30; +double tmp470 = 4*tmp458*tmp34; +double tmp471 = tmp456*tmp37; +double tmp472 = tmp469 + tmp470 + tmp471; +double tmp473 = 4*tmp458*tmp19; +double tmp474 = 2*tmp456*tmp22; +double tmp475 = 2*tmp460*tmp25; +double tmp476 = tmp473 + tmp474 + tmp475; +double tmp477 = 4*tmp460*tmp4; +double tmp478 = 4*tmp458*tmp7; +double tmp479 = 2*tmp456*tmp6; +double tmp480 = tmp477 + tmp478 + tmp479; +double tmp481 = 4*tmp460*tmp5; +double tmp482 = -2*tmp456*tmp7; +double tmp483 = 4*tmp458*tmp6; +double tmp484 = tmp481 + tmp482 + tmp483; +double tmp485 = 4*tmp458*tmp5; +double tmp486 = 2*tmp456*tmp4; +double tmp487 = -4*tmp460*tmp6; +double tmp488 = tmp485 + tmp486 + tmp487; +double tmp489 = ibd(3+_OFF); +double tmp490 = ibd(2+_OFF); +double tmp491 = ibod(1+_OFF); +double tmp492 = ibod(2+_OFF); +double tmp493 = ibod(3+_OFF); +double tmp494 = ibd(1+_OFF); +double tmp495 = cBar(1+_OFF); +double tmp496 = cp(3+_OFF); +double tmp497 = x(39+_OFF); +double tmp498 = mRoll(2+_OFF); +double tmp499 = mRoll(3+_OFF); +double tmp500 = x(37+_OFF); +double tmp501 = mRoll(4+_OFF); +double tmp502 = -(tmp58*tmp498); +double tmp503 = (tmp496*tmp499*tmp56*tmp500)/2.; +double tmp504 = -0.5*(tmp496*tmp501*tmp56*tmp497); +double tmp505 = mRoll(1+_OFF); +double tmp506 = z(2+_OFF); +double tmp507 = tmp505*tmp506; +double tmp508 = tmp502 + tmp503 + tmp504 + tmp507; +double tmp509 = mPitch(3+_OFF); +double tmp510 = mPitch(4+_OFF); +double tmp511 = x(38+_OFF); +double tmp512 = mPitch(1+_OFF); +double tmp513 = -tmp512; +double tmp514 = tmp41*tmp509; +double tmp515 = (tmp495*tmp510*tmp56*tmp511)/2.; +double tmp516 = mPitch(2+_OFF); +double tmp517 = z(3+_OFF); +double tmp518 = -(tmp516*tmp517); +double tmp519 = tmp513 + tmp514 + tmp515 + tmp518; +double tmp520 = mYaw(2+_OFF); +double tmp521 = mYaw(3+_OFF); +double tmp522 = tmp58*tmp520; +double tmp523 = (tmp496*tmp521*tmp56*tmp497)/2.; +double tmp524 = mYaw(1+_OFF); +double tmp525 = z(4+_OFF); +double tmp526 = -(tmp524*tmp525); +double tmp527 = tmp522 + tmp523 + tmp526; +double tmp528 = tmp494*tmp490*tmp489; +double tmp529 = std::pow(tmp491,2); +double tmp530 = -(tmp490*tmp529); +double tmp531 = std::pow(tmp492,2); +double tmp532 = -(tmp489*tmp531); +double tmp533 = 2*tmp491*tmp492*tmp493; +double tmp534 = std::pow(tmp493,2); +double tmp535 = -(tmp494*tmp534); +double tmp536 = tmp528 + tmp530 + tmp532 + tmp533 + tmp535; +double tmp537 = 1/tmp536; +double tmp538 = tmp509*tmp115; +double tmp539 = -0.25*(tmp495*tmp510*tmp106*tmp117*tmp511); +double tmp540 = tmp538 + tmp539; +double tmp541 = -(tmp498*tmp116*tmp120); +double tmp542 = -0.25*(tmp496*tmp499*tmp106*tmp117*tmp500); +double tmp543 = (tmp496*tmp501*tmp106*tmp117*tmp497)/4.; +double tmp544 = tmp541 + tmp542 + tmp543; +double tmp545 = tmp520*tmp116*tmp120; +double tmp546 = -0.25*(tmp496*tmp521*tmp106*tmp117*tmp497); +double tmp547 = tmp545 + tmp546; +double tmp548 = -(tmp489*tmp492); +double tmp549 = tmp491*tmp493; +double tmp550 = tmp548 + tmp549; +double tmp551 = -2*tmp269*tmp5; +double tmp552 = -4*tmp267*tmp4; +double tmp553 = 4*tmp271*tmp6; +double tmp554 = tmp551 + tmp552 + tmp553; +double tmp555 = -(tmp490*tmp491); +double tmp556 = tmp492*tmp493; +double tmp557 = tmp555 + tmp556; +double tmp558 = tmp509*tmp174; +double tmp559 = -0.25*(tmp495*tmp510*tmp171*tmp117*tmp511); +double tmp560 = tmp558 + tmp559; +double tmp561 = -(tmp498*tmp116*tmp177); +double tmp562 = -0.25*(tmp496*tmp499*tmp171*tmp117*tmp500); +double tmp563 = (tmp496*tmp501*tmp171*tmp117*tmp497)/4.; +double tmp564 = tmp561 + tmp562 + tmp563; +double tmp565 = tmp520*tmp116*tmp177; +double tmp566 = -0.25*(tmp496*tmp521*tmp171*tmp117*tmp497); +double tmp567 = tmp565 + tmp566; +double tmp568 = -4*tmp458*tmp5; +double tmp569 = -2*tmp456*tmp4; +double tmp570 = 4*tmp460*tmp6; +double tmp571 = tmp568 + tmp569 + tmp570; +double tmp572 = tmp490*tmp489; +double tmp573 = -tmp534; +double tmp574 = tmp572 + tmp573; +double tmp575 = -4*tmp65*tmp5; +double tmp576 = -4*tmp62*tmp4; +double tmp577 = 2*tmp72*tmp6; +double tmp578 = tmp575 + tmp576 + tmp577; +double tmp579 = -4*tmp62*tmp5; +double tmp580 = 4*tmp65*tmp4; +double tmp581 = -2*tmp72*tmp7; +double tmp582 = tmp579 + tmp580 + tmp581; +double tmp583 = tmp509*tmp223; +double tmp584 = -0.25*(tmp495*tmp510*tmp220*tmp117*tmp511); +double tmp585 = tmp583 + tmp584; +double tmp586 = -(tmp498*tmp116*tmp226); +double tmp587 = -0.25*(tmp496*tmp499*tmp220*tmp117*tmp500); +double tmp588 = (tmp496*tmp501*tmp220*tmp117*tmp497)/4.; +double tmp589 = tmp586 + tmp587 + tmp588; +double tmp590 = tmp520*tmp116*tmp226; +double tmp591 = -0.25*(tmp496*tmp521*tmp220*tmp117*tmp497); +double tmp592 = tmp590 + tmp591; +double tmp593 = -2*tmp456*tmp5; +double tmp594 = 4*tmp458*tmp4; +double tmp595 = -4*tmp460*tmp7; +double tmp596 = tmp593 + tmp594 + tmp595; +double tmp597 = -4*tmp267*tmp5; +double tmp598 = 2*tmp269*tmp4; +double tmp599 = -4*tmp271*tmp7; +double tmp600 = tmp597 + tmp598 + tmp599; +double tmp601 = -2*tmp72*tmp5; +double tmp602 = 4*tmp62*tmp7; +double tmp603 = -4*tmp65*tmp6; +double tmp604 = tmp601 + tmp602 + tmp603; +double tmp605 = tmp509*tmp247; +double tmp606 = -0.25*(tmp495*tmp510*tmp244*tmp117*tmp511); +double tmp607 = tmp605 + tmp606; +double tmp608 = -(tmp498*tmp116*tmp250); +double tmp609 = -0.25*(tmp496*tmp499*tmp244*tmp117*tmp500); +double tmp610 = (tmp496*tmp501*tmp244*tmp117*tmp497)/4.; +double tmp611 = tmp608 + tmp609 + tmp610; +double tmp612 = tmp520*tmp116*tmp250; +double tmp613 = -0.25*(tmp496*tmp521*tmp244*tmp117*tmp497); +double tmp614 = tmp612 + tmp613; +double tmp615 = -4*tmp271*tmp5; +double tmp616 = 4*tmp267*tmp7; +double tmp617 = -2*tmp269*tmp6; +double tmp618 = tmp615 + tmp616 + tmp617; +double tmp619 = -4*tmp460*tmp5; +double tmp620 = 2*tmp456*tmp7; +double tmp621 = -4*tmp458*tmp6; +double tmp622 = tmp619 + tmp620 + tmp621; +double tmp623 = 0.634*tmp495*tmp59*tmp318*tmp55*tmp540; +double tmp624 = 0.634*tmp59*tmp496*tmp348*tmp55*tmp544; +double tmp625 = 0.634*tmp59*tmp496*tmp329*tmp55*tmp547; +double tmp626 = 0.634*tmp59*tmp496*tmp348*tmp106*tmp508; +double tmp627 = 0.634*tmp59*tmp496*tmp373*tmp55*tmp508; +double tmp628 = 0.634*tmp495*tmp59*tmp318*tmp106*tmp519; +double tmp629 = 0.634*tmp495*tmp59*tmp455*tmp55*tmp519; +double tmp630 = 0.634*tmp59*tmp496*tmp329*tmp106*tmp527; +double tmp631 = 0.634*tmp59*tmp496*tmp314*tmp55*tmp527; +double tmp632 = tmp623 + tmp624 + tmp625 + tmp626 + tmp627 + tmp628 + tmp629 + tmp630 + tmp631; +double tmp633 = 0.634*tmp495*tmp59*tmp468*tmp55*tmp540; +double tmp634 = 0.634*tmp59*tmp496*tmp476*tmp55*tmp544; +double tmp635 = 0.634*tmp59*tmp496*tmp472*tmp55*tmp547; +double tmp636 = 0.634*tmp59*tmp496*tmp476*tmp106*tmp508; +double tmp637 = 0.634*tmp59*tmp496*tmp484*tmp55*tmp508; +double tmp638 = 0.634*tmp495*tmp59*tmp468*tmp106*tmp519; +double tmp639 = 0.634*tmp495*tmp59*tmp488*tmp55*tmp519; +double tmp640 = 0.634*tmp59*tmp496*tmp472*tmp106*tmp527; +double tmp641 = 0.634*tmp59*tmp496*tmp464*tmp55*tmp527; +double tmp642 = tmp633 + tmp634 + tmp635 + tmp636 + tmp637 + tmp638 + tmp639 + tmp640 + tmp641; +double tmp643 = 0.634*tmp495*tmp59*tmp159*tmp55*tmp540; +double tmp644 = 0.634*tmp59*tmp496*tmp193*tmp55*tmp544; +double tmp645 = 0.634*tmp59*tmp496*tmp181*tmp55*tmp547; +double tmp646 = 0.634*tmp59*tmp496*tmp193*tmp106*tmp508; +double tmp647 = 0.634*tmp59*tmp496*tmp212*tmp55*tmp508; +double tmp648 = 0.634*tmp495*tmp59*tmp159*tmp106*tmp519; +double tmp649 = 0.634*tmp495*tmp59*tmp264*tmp55*tmp519; +double tmp650 = 0.634*tmp59*tmp496*tmp181*tmp106*tmp527; +double tmp651 = 0.634*tmp59*tmp496*tmp148*tmp55*tmp527; +double tmp652 = tmp643 + tmp644 + tmp645 + tmp646 + tmp647 + tmp648 + tmp649 + tmp650 + tmp651; +double tmp653 = tmp494*tmp489; +double tmp654 = -tmp529; +double tmp655 = tmp653 + tmp654; +double tmp656 = 0.634*tmp495*tmp59*tmp318*tmp55*tmp560; +double tmp657 = 0.634*tmp59*tmp496*tmp348*tmp55*tmp564; +double tmp658 = 0.634*tmp59*tmp496*tmp329*tmp55*tmp567; +double tmp659 = 0.634*tmp59*tmp496*tmp348*tmp171*tmp508; +double tmp660 = 0.634*tmp59*tmp496*tmp369*tmp55*tmp508; +double tmp661 = 0.634*tmp495*tmp59*tmp318*tmp171*tmp519; +double tmp662 = 0.634*tmp495*tmp59*tmp314*tmp55*tmp519; +double tmp663 = 0.634*tmp59*tmp496*tmp329*tmp171*tmp527; +double tmp664 = 0.634*tmp59*tmp496*tmp554*tmp55*tmp527; +double tmp665 = tmp656 + tmp657 + tmp658 + tmp659 + tmp660 + tmp661 + tmp662 + tmp663 + tmp664; +double tmp666 = tmp491*tmp492; +double tmp667 = -(tmp494*tmp493); +double tmp668 = tmp666 + tmp667; +double tmp669 = 0.634*tmp495*tmp59*tmp468*tmp55*tmp560; +double tmp670 = 0.634*tmp59*tmp496*tmp476*tmp55*tmp564; +double tmp671 = 0.634*tmp59*tmp496*tmp472*tmp55*tmp567; +double tmp672 = 0.634*tmp59*tmp496*tmp476*tmp171*tmp508; +double tmp673 = 0.634*tmp59*tmp496*tmp480*tmp55*tmp508; +double tmp674 = 0.634*tmp495*tmp59*tmp468*tmp171*tmp519; +double tmp675 = 0.634*tmp495*tmp59*tmp464*tmp55*tmp519; +double tmp676 = 0.634*tmp59*tmp496*tmp472*tmp171*tmp527; +double tmp677 = 0.634*tmp59*tmp496*tmp571*tmp55*tmp527; +double tmp678 = tmp669 + tmp670 + tmp671 + tmp672 + tmp673 + tmp674 + tmp675 + tmp676 + tmp677; +double tmp679 = 0.634*tmp495*tmp59*tmp159*tmp55*tmp560; +double tmp680 = 0.634*tmp59*tmp496*tmp193*tmp55*tmp564; +double tmp681 = 0.634*tmp59*tmp496*tmp181*tmp55*tmp567; +double tmp682 = 0.634*tmp59*tmp496*tmp193*tmp171*tmp508; +double tmp683 = 0.634*tmp59*tmp496*tmp208*tmp55*tmp508; +double tmp684 = 0.634*tmp495*tmp59*tmp159*tmp171*tmp519; +double tmp685 = 0.634*tmp495*tmp59*tmp148*tmp55*tmp519; +double tmp686 = 0.634*tmp59*tmp496*tmp181*tmp171*tmp527; +double tmp687 = 0.634*tmp59*tmp496*tmp578*tmp55*tmp527; +double tmp688 = tmp679 + tmp680 + tmp681 + tmp682 + tmp683 + tmp684 + tmp685 + tmp686 + tmp687; +double tmp689 = 0.634*tmp495*tmp59*tmp159*tmp55*tmp585; +double tmp690 = 0.634*tmp59*tmp496*tmp193*tmp55*tmp589; +double tmp691 = 0.634*tmp59*tmp496*tmp181*tmp55*tmp592; +double tmp692 = 0.634*tmp59*tmp496*tmp193*tmp220*tmp508; +double tmp693 = 0.634*tmp59*tmp496*tmp582*tmp55*tmp508; +double tmp694 = 0.634*tmp495*tmp59*tmp159*tmp220*tmp519; +double tmp695 = 0.634*tmp495*tmp59*tmp208*tmp55*tmp519; +double tmp696 = 0.634*tmp59*tmp496*tmp181*tmp220*tmp527; +double tmp697 = 0.634*tmp59*tmp496*tmp212*tmp55*tmp527; +double tmp698 = tmp689 + tmp690 + tmp691 + tmp692 + tmp693 + tmp694 + tmp695 + tmp696 + tmp697; +double tmp699 = 0.634*tmp495*tmp59*tmp468*tmp55*tmp585; +double tmp700 = 0.634*tmp59*tmp496*tmp476*tmp55*tmp589; +double tmp701 = 0.634*tmp59*tmp496*tmp472*tmp55*tmp592; +double tmp702 = 0.634*tmp59*tmp496*tmp476*tmp220*tmp508; +double tmp703 = 0.634*tmp59*tmp496*tmp596*tmp55*tmp508; +double tmp704 = 0.634*tmp495*tmp59*tmp468*tmp220*tmp519; +double tmp705 = 0.634*tmp495*tmp59*tmp480*tmp55*tmp519; +double tmp706 = 0.634*tmp59*tmp496*tmp472*tmp220*tmp527; +double tmp707 = 0.634*tmp59*tmp496*tmp484*tmp55*tmp527; +double tmp708 = tmp699 + tmp700 + tmp701 + tmp702 + tmp703 + tmp704 + tmp705 + tmp706 + tmp707; +double tmp709 = 0.634*tmp495*tmp59*tmp318*tmp55*tmp585; +double tmp710 = 0.634*tmp59*tmp496*tmp348*tmp55*tmp589; +double tmp711 = 0.634*tmp59*tmp496*tmp329*tmp55*tmp592; +double tmp712 = 0.634*tmp59*tmp496*tmp348*tmp220*tmp508; +double tmp713 = 0.634*tmp59*tmp496*tmp600*tmp55*tmp508; +double tmp714 = 0.634*tmp495*tmp59*tmp318*tmp220*tmp519; +double tmp715 = 0.634*tmp495*tmp59*tmp369*tmp55*tmp519; +double tmp716 = 0.634*tmp59*tmp496*tmp329*tmp220*tmp527; +double tmp717 = 0.634*tmp59*tmp496*tmp373*tmp55*tmp527; +double tmp718 = tmp709 + tmp710 + tmp711 + tmp712 + tmp713 + tmp714 + tmp715 + tmp716 + tmp717; +double tmp719 = 0.634*tmp495*tmp59*tmp159*tmp55*tmp607; +double tmp720 = 0.634*tmp59*tmp496*tmp193*tmp55*tmp611; +double tmp721 = 0.634*tmp59*tmp496*tmp181*tmp55*tmp614; +double tmp722 = 0.634*tmp59*tmp496*tmp193*tmp244*tmp508; +double tmp723 = 0.634*tmp59*tmp496*tmp264*tmp55*tmp508; +double tmp724 = 0.634*tmp495*tmp59*tmp159*tmp244*tmp519; +double tmp725 = 0.634*tmp495*tmp59*tmp604*tmp55*tmp519; +double tmp726 = 0.634*tmp59*tmp496*tmp181*tmp244*tmp527; +double tmp727 = 0.634*tmp59*tmp496*tmp208*tmp55*tmp527; +double tmp728 = tmp719 + tmp720 + tmp721 + tmp722 + tmp723 + tmp724 + tmp725 + tmp726 + tmp727; +double tmp729 = 0.634*tmp495*tmp59*tmp318*tmp55*tmp607; +double tmp730 = 0.634*tmp59*tmp496*tmp348*tmp55*tmp611; +double tmp731 = 0.634*tmp59*tmp496*tmp329*tmp55*tmp614; +double tmp732 = 0.634*tmp59*tmp496*tmp348*tmp244*tmp508; +double tmp733 = 0.634*tmp59*tmp496*tmp455*tmp55*tmp508; +double tmp734 = 0.634*tmp495*tmp59*tmp318*tmp244*tmp519; +double tmp735 = 0.634*tmp495*tmp59*tmp618*tmp55*tmp519; +double tmp736 = 0.634*tmp59*tmp496*tmp329*tmp244*tmp527; +double tmp737 = 0.634*tmp59*tmp496*tmp369*tmp55*tmp527; +double tmp738 = tmp729 + tmp730 + tmp731 + tmp732 + tmp733 + tmp734 + tmp735 + tmp736 + tmp737; +double tmp739 = 0.634*tmp495*tmp59*tmp468*tmp55*tmp607; +double tmp740 = 0.634*tmp59*tmp496*tmp476*tmp55*tmp611; +double tmp741 = 0.634*tmp59*tmp496*tmp472*tmp55*tmp614; +double tmp742 = 0.634*tmp59*tmp496*tmp476*tmp244*tmp508; +double tmp743 = 0.634*tmp59*tmp496*tmp488*tmp55*tmp508; +double tmp744 = 0.634*tmp495*tmp59*tmp468*tmp244*tmp519; +double tmp745 = 0.634*tmp495*tmp59*tmp622*tmp55*tmp519; +double tmp746 = 0.634*tmp59*tmp496*tmp472*tmp244*tmp527; +double tmp747 = 0.634*tmp59*tmp496*tmp480*tmp55*tmp527; +double tmp748 = tmp739 + tmp740 + tmp741 + tmp742 + tmp743 + tmp744 + tmp745 + tmp746 + tmp747; +double tmp749 = tmp494*tmp490; +double tmp750 = -tmp531; +double tmp751 = tmp749 + tmp750; +J(1+_OFF,1+_OFF) = -(tmp144*(tmp264*tmp151 + tmp148*tmp155 + tmp159*tmp278 + tmp181*tmp293 + tmp193*tmp310 + tmp212*tmp204)); +J(1+_OFF,2+_OFF) = -(tmp144*(tmp148*tmp151 + tmp578*tmp155 + tmp159*tmp325 + tmp181*tmp344 + tmp193*tmp365 + tmp208*tmp204)); +J(1+_OFF,3+_OFF) = -(tmp144*(tmp208*tmp151 + tmp212*tmp155 + tmp159*tmp380 + tmp181*tmp393 + tmp193*tmp412 + tmp582*tmp204)); +J(1+_OFF,4+_OFF) = -(tmp144*(tmp604*tmp151 + tmp208*tmp155 + tmp159*tmp419 + tmp181*tmp432 + tmp193*tmp451 + tmp264*tmp204)); +J(2+_OFF,1+_OFF) = -(tmp144*(tmp455*tmp151 + tmp314*tmp155 + tmp318*tmp278 + tmp329*tmp293 + tmp348*tmp310 + tmp373*tmp204)); +J(2+_OFF,2+_OFF) = -(tmp144*(tmp314*tmp151 + tmp554*tmp155 + tmp318*tmp325 + tmp329*tmp344 + tmp348*tmp365 + tmp369*tmp204)); +J(2+_OFF,3+_OFF) = -(tmp144*(tmp369*tmp151 + tmp373*tmp155 + tmp318*tmp380 + tmp329*tmp393 + tmp348*tmp412 + tmp600*tmp204)); +J(2+_OFF,4+_OFF) = -(tmp144*(tmp618*tmp151 + tmp369*tmp155 + tmp318*tmp419 + tmp329*tmp432 + tmp348*tmp451 + tmp455*tmp204)); +J(3+_OFF,1+_OFF) = -(tmp144*(tmp488*tmp151 + tmp464*tmp155 + tmp468*tmp278 + tmp472*tmp293 + tmp476*tmp310 + tmp484*tmp204)); +J(3+_OFF,2+_OFF) = -(tmp144*(tmp464*tmp151 + tmp571*tmp155 + tmp468*tmp325 + tmp472*tmp344 + tmp476*tmp365 + tmp480*tmp204)); +J(3+_OFF,3+_OFF) = -(tmp144*(tmp480*tmp151 + tmp484*tmp155 + tmp468*tmp380 + tmp472*tmp393 + tmp476*tmp412 + tmp596*tmp204)); +J(3+_OFF,4+_OFF) = -(tmp144*(tmp622*tmp151 + tmp480*tmp155 + tmp468*tmp419 + tmp472*tmp432 + tmp476*tmp451 + tmp488*tmp204)); +J(4+_OFF,1+_OFF) = -(tmp550*tmp537*tmp632) - tmp557*tmp537*tmp642 - tmp574*tmp537*tmp652; +J(4+_OFF,2+_OFF) = -(tmp550*tmp537*tmp665) - tmp557*tmp537*tmp678 - tmp574*tmp537*tmp688; +J(4+_OFF,3+_OFF) = -(tmp574*tmp537*tmp698) - tmp557*tmp537*tmp708 - tmp550*tmp537*tmp718; +J(4+_OFF,4+_OFF) = -(tmp574*tmp537*tmp728) - tmp550*tmp537*tmp738 - tmp557*tmp537*tmp748; +J(5+_OFF,1+_OFF) = -(tmp655*tmp537*tmp632) - tmp668*tmp537*tmp642 - tmp550*tmp537*tmp652; +J(5+_OFF,2+_OFF) = -(tmp655*tmp537*tmp665) - tmp668*tmp537*tmp678 - tmp550*tmp537*tmp688; +J(5+_OFF,3+_OFF) = -(tmp550*tmp537*tmp698) - tmp668*tmp537*tmp708 - tmp655*tmp537*tmp718; +J(5+_OFF,4+_OFF) = -(tmp550*tmp537*tmp728) - tmp655*tmp537*tmp738 - tmp668*tmp537*tmp748; +J(6+_OFF,1+_OFF) = -(tmp668*tmp537*tmp632) - tmp751*tmp537*tmp642 - tmp557*tmp537*tmp652; +J(6+_OFF,2+_OFF) = -(tmp668*tmp537*tmp665) - tmp751*tmp537*tmp678 - tmp557*tmp537*tmp688; +J(6+_OFF,3+_OFF) = -(tmp557*tmp537*tmp698) - tmp751*tmp537*tmp708 - tmp668*tmp537*tmp718; +J(6+_OFF,4+_OFF) = -(tmp557*tmp537*tmp728) - tmp668*tmp537*tmp738 - tmp751*tmp537*tmp748; diff --git a/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrV.cppready b/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrV.cppready index 9258b1b..5b3af21 100644 --- a/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrV.cppready +++ b/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrV.cppready @@ -1,300 +1,18 @@ -double tmp0 = x(4+_OFF); -double tmp1 = x(6+_OFF); -double tmp2 = x(5+_OFF); -double tmp3 = x(7+_OFF); -double tmp4 = wind(1+_OFF); -double tmp5 = tmp2*tmp3; -double tmp6 = wind(2+_OFF); -double tmp7 = wind(3+_OFF); -double tmp8 = std::pow(tmp0,2); -double tmp9 = std::pow(tmp2,2); -double tmp10 = std::pow(tmp1,2); -double tmp11 = -tmp10; -double tmp12 = std::pow(tmp3,2); -double tmp13 = tmp2*tmp1; -double tmp14 = tmp0*tmp3; -double tmp15 = tmp13 + tmp14; -double tmp16 = -2*tmp6*tmp15; -double tmp17 = -(tmp0*tmp1); -double tmp18 = tmp17 + tmp5; -double tmp19 = -2*tmp7*tmp18; -double tmp20 = -tmp12; -double tmp21 = tmp8 + tmp9 + tmp11 + tmp20; -double tmp22 = -(tmp4*tmp21); -double tmp23 = x(8+_OFF); -double tmp24 = tmp16 + tmp19 + tmp22 + tmp23; -double tmp25 = tmp0*tmp1; -double tmp26 = tmp25 + tmp5; -double tmp27 = 2*tmp4*tmp26; -double tmp28 = -(tmp0*tmp2); -double tmp29 = tmp1*tmp3; -double tmp30 = tmp28 + tmp29; -double tmp31 = 2*tmp6*tmp30; -double tmp32 = -tmp9; -double tmp33 = tmp8 + tmp32 + tmp11 + tmp12; -double tmp34 = tmp7*tmp33; -double tmp35 = x(10+_OFF); -double tmp36 = -tmp35; -double tmp37 = tmp27 + tmp31 + tmp34 + tmp36; -double tmp38 = std::pow(tmp24,2); -double tmp39 = -(tmp0*tmp3); -double tmp40 = tmp13 + tmp39; -double tmp41 = -2*tmp4*tmp40; -double tmp42 = tmp0*tmp2; -double tmp43 = tmp42 + tmp29; -double tmp44 = -2*tmp7*tmp43; -double tmp45 = tmp8 + tmp32 + tmp10 + tmp20; -double tmp46 = -(tmp6*tmp45); -double tmp47 = x(9+_OFF); -double tmp48 = tmp41 + tmp44 + tmp46 + tmp47; -double tmp49 = cp(2+_OFF); -double tmp50 = std::pow(tmp37,2); -double tmp51 = tmp38 + tmp50; -double tmp52 = 1/std::sqrt(tmp51); -double tmp53 = std::pow(tmp48,2); -double tmp54 = -2*tmp4*tmp26; -double tmp55 = -2*tmp6*tmp30; -double tmp56 = -(tmp7*tmp33); -double tmp57 = tmp54 + tmp55 + tmp56 + tmp35; -double tmp58 = std::pow(tmp57,2); -double tmp59 = tmp38 + tmp53 + tmp58; -double tmp60 = 1/std::sqrt(tmp59); -double tmp61 = tmp48*tmp60; -double tmp62 = std::asin(tmp61); -double tmp63 = fLat(1+_OFF); -double tmp64 = -(tmp4*tmp8); -double tmp65 = -(tmp4*tmp9); -double tmp66 = 2*tmp7*tmp0*tmp1; -double tmp67 = -2*tmp6*tmp2*tmp1; -double tmp68 = tmp4*tmp10; -double tmp69 = -2*tmp6*tmp0*tmp3; -double tmp70 = -2*tmp7*tmp2*tmp3; -double tmp71 = tmp4*tmp12; -double tmp72 = tmp64 + tmp65 + tmp66 + tmp67 + tmp68 + tmp69 + tmp70 + tmp71 + tmp23; -double tmp73 = tmp6*tmp8; -double tmp74 = 2*tmp7*tmp0*tmp2; -double tmp75 = -(tmp6*tmp9); -double tmp76 = 2*tmp4*tmp2*tmp1; -double tmp77 = tmp6*tmp10; -double tmp78 = -2*tmp4*tmp0*tmp3; -double tmp79 = 2*tmp7*tmp1*tmp3; -double tmp80 = -(tmp6*tmp12); -double tmp81 = -tmp47; -double tmp82 = tmp73 + tmp74 + tmp75 + tmp76 + tmp77 + tmp78 + tmp79 + tmp80 + tmp81; -double tmp83 = std::sqrt(tmp59); -double tmp84 = fLift(1+_OFF); -double tmp85 = std::atan2(tmp37,tmp24); -double tmp86 = fLift(2+_OFF); -double tmp87 = tmp85*tmp86; -double tmp88 = tmp84 + tmp87; -double tmp89 = std::pow(tmp51,-1.5); -double tmp90 = tmp7*tmp8; -double tmp91 = -2*tmp6*tmp0*tmp2; -double tmp92 = -(tmp7*tmp9); -double tmp93 = 2*tmp4*tmp0*tmp1; -double tmp94 = -(tmp7*tmp10); -double tmp95 = 2*tmp4*tmp2*tmp3; -double tmp96 = 2*tmp6*tmp1*tmp3; -double tmp97 = tmp7*tmp12; -double tmp98 = tmp90 + tmp91 + tmp92 + tmp93 + tmp94 + tmp95 + tmp96 + tmp97 + tmp36; -double tmp99 = 1/tmp59; -double tmp100 = -(tmp53*tmp99); -double tmp101 = 1 + tmp100; -double tmp102 = 1/std::sqrt(tmp101); -double tmp103 = fDrag(1+_OFF); -double tmp104 = fDrag(2+_OFF); -double tmp105 = tmp85*tmp104; -double tmp106 = std::pow(tmp85,2); -double tmp107 = fDrag(3+_OFF); -double tmp108 = tmp106*tmp107; -double tmp109 = std::pow(tmp62,2); -double tmp110 = fDrag(4+_OFF); -double tmp111 = tmp109*tmp110; -double tmp112 = tmp103 + tmp105 + tmp108 + tmp111; -double tmp113 = std::sqrt(tmp101); -double tmp114 = 1/tmp51; -double tmp115 = cp(1+_OFF); -double tmp116 = cp(4+_OFF); -double tmp117 = 1/tmp116; -double tmp118 = std::pow(tmp59,-1.5); -double tmp119 = -(tmp53*tmp118); -double tmp120 = tmp119 + tmp60; -double tmp121 = std::pow(tmp115,2); -double tmp122 = std::pow(M_PI,-2); -double tmp123 = fThrust(3+_OFF); -double tmp124 = 1/M_PI; -double tmp125 = fThrust(2+_OFF); -double tmp126 = z(1+_OFF); -double tmp127 = -(tmp6*tmp8); -double tmp128 = -2*tmp7*tmp0*tmp2; -double tmp129 = tmp6*tmp9; -double tmp130 = -2*tmp4*tmp2*tmp1; -double tmp131 = -(tmp6*tmp10); -double tmp132 = 2*tmp4*tmp0*tmp3; -double tmp133 = -2*tmp7*tmp1*tmp3; -double tmp134 = tmp6*tmp12; -double tmp135 = tmp127 + tmp128 + tmp129 + tmp130 + tmp131 + tmp132 + tmp133 + tmp134 + tmp47; -double tmp136 = tmp104*tmp114*tmp57; -double tmp137 = 2*tmp85*tmp107*tmp114*tmp57; -double tmp138 = -2*tmp62*tmp110*tmp24*tmp48*tmp118*tmp102; -double tmp139 = tmp136 + tmp137 + tmp138; -double tmp140 = std::pow(tmp48,3); -double tmp141 = std::pow(tmp59,-2); -double tmp142 = 2*tmp140*tmp141; -double tmp143 = -2*tmp48*tmp99; -double tmp144 = tmp142 + tmp143; -double tmp145 = -(tmp104*tmp24*tmp114); -double tmp146 = -2*tmp85*tmp107*tmp24*tmp114; -double tmp147 = -2*tmp62*tmp110*tmp48*tmp57*tmp118*tmp102; -double tmp148 = tmp145 + tmp146 + tmp147; -double tmp149 = -(tmp7*tmp8); -double tmp150 = 2*tmp6*tmp0*tmp2; -double tmp151 = tmp7*tmp9; -double tmp152 = -2*tmp4*tmp0*tmp1; -double tmp153 = tmp7*tmp10; -double tmp154 = -2*tmp4*tmp2*tmp3; -double tmp155 = -2*tmp6*tmp1*tmp3; -double tmp156 = -(tmp7*tmp12); -double tmp157 = tmp149 + tmp150 + tmp151 + tmp152 + tmp153 + tmp154 + tmp155 + tmp156 + tmp35; -double tmp158 = 0.5*tmp62*tmp49*tmp63*tmp82*tmp52*tmp83; -double tmp159 = 0.5*tmp49*tmp112*tmp52*tmp59*tmp113; -double tmp160 = ibd(2+_OFF); -double tmp161 = ibd(3+_OFF); -double tmp162 = ibod(1+_OFF); -double tmp163 = ibod(2+_OFF); -double tmp164 = ibod(3+_OFF); -double tmp165 = ibd(1+_OFF); -double tmp166 = std::pow(tmp164,2); -double tmp167 = cp(3+_OFF); -double tmp168 = mRoll(2+_OFF); -double tmp169 = mRoll(3+_OFF); -double tmp170 = x(11+_OFF); -double tmp171 = mRoll(4+_OFF); -double tmp172 = x(13+_OFF); -double tmp173 = tmp165*tmp160*tmp161; -double tmp174 = std::pow(tmp162,2); -double tmp175 = -(tmp160*tmp174); -double tmp176 = std::pow(tmp163,2); -double tmp177 = -(tmp161*tmp176); -double tmp178 = 2*tmp162*tmp163*tmp164; -double tmp179 = -(tmp165*tmp166); -double tmp180 = tmp173 + tmp175 + tmp177 + tmp178 + tmp179; -double tmp181 = 1/tmp180; -double tmp182 = cBar(1+_OFF); -double tmp183 = mPitch(3+_OFF); -double tmp184 = mPitch(4+_OFF); -double tmp185 = x(12+_OFF); -double tmp186 = mYaw(2+_OFF); -double tmp187 = mYaw(3+_OFF); -double tmp188 = tmp160*tmp161; -double tmp189 = -tmp166; -double tmp190 = tmp188 + tmp189; -double tmp191 = -(tmp62*tmp168); -double tmp192 = (tmp167*tmp169*tmp60*tmp170)/2.; -double tmp193 = -(tmp167*tmp171*tmp60*tmp172)/2.; -double tmp194 = mRoll(1+_OFF); -double tmp195 = z(2+_OFF); -double tmp196 = -(tmp194*tmp195); -double tmp197 = tmp191 + tmp192 + tmp193 + tmp196; -double tmp198 = -(tmp161*tmp163); -double tmp199 = tmp162*tmp164; -double tmp200 = tmp198 + tmp199; -double tmp201 = mPitch(1+_OFF); -double tmp202 = -tmp201; -double tmp203 = -(tmp85*tmp183); -double tmp204 = (tmp182*tmp184*tmp60*tmp185)/2.; -double tmp205 = mPitch(2+_OFF); -double tmp206 = z(3+_OFF); -double tmp207 = -(tmp205*tmp206); -double tmp208 = tmp202 + tmp203 + tmp204 + tmp207; -double tmp209 = -(tmp160*tmp162); -double tmp210 = tmp163*tmp164; -double tmp211 = tmp209 + tmp210; -double tmp212 = tmp62*tmp186; -double tmp213 = (tmp167*tmp187*tmp60*tmp172)/2.; -double tmp214 = mYaw(1+_OFF); -double tmp215 = z(4+_OFF); -double tmp216 = tmp214*tmp215; -double tmp217 = tmp212 + tmp213 + tmp216; -double tmp218 = tmp168*tmp24*tmp48*tmp118*tmp102; -double tmp219 = -(tmp167*tmp169*tmp24*tmp118*tmp170)/2.; -double tmp220 = (tmp167*tmp171*tmp24*tmp118*tmp172)/2.; -double tmp221 = tmp218 + tmp219 + tmp220; -double tmp222 = 0.5*tmp49*tmp167*tmp59*tmp221; -double tmp223 = 1.*tmp49*tmp167*tmp24*tmp197; -double tmp224 = tmp222 + tmp223; -double tmp225 = -(tmp183*tmp114*tmp57); -double tmp226 = -(tmp182*tmp184*tmp24*tmp118*tmp185)/2.; -double tmp227 = tmp225 + tmp226; -double tmp228 = 0.5*tmp182*tmp49*tmp59*tmp227; -double tmp229 = 1.*tmp182*tmp49*tmp24*tmp208; -double tmp230 = tmp228 + tmp229; -double tmp231 = -(tmp186*tmp24*tmp48*tmp118*tmp102); -double tmp232 = -(tmp167*tmp187*tmp24*tmp118*tmp172)/2.; -double tmp233 = tmp231 + tmp232; -double tmp234 = 0.5*tmp49*tmp167*tmp59*tmp233; -double tmp235 = 1.*tmp49*tmp167*tmp24*tmp217; -double tmp236 = tmp234 + tmp235; -double tmp237 = -(tmp168*tmp102*tmp120); -double tmp238 = -(tmp167*tmp169*tmp48*tmp118*tmp170)/2.; -double tmp239 = (tmp167*tmp171*tmp48*tmp118*tmp172)/2.; -double tmp240 = tmp237 + tmp238 + tmp239; -double tmp241 = 0.5*tmp49*tmp167*tmp59*tmp240; -double tmp242 = 1.*tmp49*tmp167*tmp48*tmp197; -double tmp243 = tmp241 + tmp242; -double tmp244 = tmp165*tmp161; -double tmp245 = -tmp174; -double tmp246 = tmp244 + tmp245; -double tmp247 = std::pow(tmp182,2); -double tmp248 = -0.25*tmp247*tmp49*tmp184*tmp48*tmp60*tmp185; -double tmp249 = 1.*tmp182*tmp49*tmp48*tmp208; -double tmp250 = tmp248 + tmp249; -double tmp251 = tmp162*tmp163; -double tmp252 = -(tmp165*tmp164); -double tmp253 = tmp251 + tmp252; -double tmp254 = tmp186*tmp102*tmp120; -double tmp255 = -(tmp167*tmp187*tmp48*tmp118*tmp172)/2.; -double tmp256 = tmp254 + tmp255; -double tmp257 = 0.5*tmp49*tmp167*tmp59*tmp256; -double tmp258 = 1.*tmp49*tmp167*tmp48*tmp217; -double tmp259 = tmp257 + tmp258; -double tmp260 = tmp168*tmp48*tmp57*tmp118*tmp102; -double tmp261 = -(tmp167*tmp169*tmp57*tmp118*tmp170)/2.; -double tmp262 = (tmp167*tmp171*tmp57*tmp118*tmp172)/2.; -double tmp263 = tmp260 + tmp261 + tmp262; -double tmp264 = 0.5*tmp49*tmp167*tmp59*tmp263; -double tmp265 = 1.*tmp49*tmp167*tmp57*tmp197; -double tmp266 = tmp264 + tmp265; -double tmp267 = tmp183*tmp24*tmp114; -double tmp268 = -(tmp182*tmp184*tmp57*tmp118*tmp185)/2.; -double tmp269 = tmp267 + tmp268; -double tmp270 = 0.5*tmp182*tmp49*tmp59*tmp269; -double tmp271 = 1.*tmp182*tmp49*tmp57*tmp208; -double tmp272 = tmp270 + tmp271; -double tmp273 = -(tmp186*tmp48*tmp57*tmp118*tmp102); -double tmp274 = -(tmp167*tmp187*tmp57*tmp118*tmp172)/2.; -double tmp275 = tmp273 + tmp274; -double tmp276 = 0.5*tmp49*tmp167*tmp59*tmp275; -double tmp277 = 1.*tmp49*tmp167*tmp57*tmp217; -double tmp278 = tmp276 + tmp277; -double tmp279 = tmp165*tmp160; -double tmp280 = -tmp176; -double tmp281 = tmp279 + tmp280; -J(1+_OFF,1+_OFF) = -(tmp117*(-1.*tmp49*tmp88*tmp24*tmp52*tmp98 + 0.5*tmp62*tmp49*tmp63*tmp72*tmp24*tmp82*tmp52*tmp60 - 0.5*tmp62*tmp49*tmp63*tmp72*tmp24*tmp82*tmp89*tmp83 + tmp158 + 0.5*tmp49*tmp88*tmp24*tmp89*tmp98*tmp59 - 0.5*tmp49*tmp86*tmp89*tmp98*tmp57*tmp59 - 0.5*tmp49*tmp63*tmp72*tmp24*tmp82*tmp48*tmp52*tmp99*tmp102 + 0.5*tmp49*tmp112*tmp72*tmp24*tmp53*tmp52*tmp99*tmp102 + 1.*tmp49*tmp112*tmp72*tmp24*tmp52*tmp113 - 0.5*tmp49*tmp112*tmp72*tmp24*tmp89*tmp59*tmp113 + tmp159 + 0.5*tmp49*tmp72*tmp52*tmp59*tmp113*tmp139 + 1.*tmp121*(2*tmp122*tmp123*tmp24 + tmp124*tmp115*tmp125*tmp24*tmp60*tmp126))); -J(1+_OFF,2+_OFF) = -(tmp117*(-1.*tmp49*tmp88*tmp48*tmp52*tmp98 + 0.5*tmp62*tmp49*tmp63*tmp72*tmp82*tmp48*tmp52*tmp60 - 0.5*tmp62*tmp49*tmp63*tmp72*tmp52*tmp83 + 0.25*tmp49*tmp112*tmp72*tmp52*tmp59*tmp144*tmp102 + 1.*tmp49*tmp112*tmp72*tmp48*tmp52*tmp113 + 1.*tmp62*tmp49*tmp110*tmp72*tmp52*tmp59*tmp120 + 0.5*tmp49*tmp63*tmp72*tmp82*tmp52*tmp83*tmp102*tmp120 + 1.*tmp121*(2*tmp122*tmp123*tmp48 + tmp124*tmp115*tmp125*tmp48*tmp60*tmp126))); -J(1+_OFF,3+_OFF) = -(tmp117*(-1.*tmp49*tmp88*tmp52*tmp98*tmp57 + 0.5*tmp62*tmp49*tmp63*tmp72*tmp82*tmp52*tmp57*tmp60 + 0.5*tmp62*tmp49*tmp63*tmp72*tmp82*tmp89*tmp37*tmp83 + 0.5*tmp49*tmp88*tmp52*tmp59 + 0.5*tmp49*tmp86*tmp24*tmp89*tmp98*tmp59 - 0.5*tmp49*tmp88*tmp89*tmp98*tmp37*tmp59 - 0.5*tmp49*tmp63*tmp72*tmp82*tmp48*tmp52*tmp57*tmp99*tmp102 + 0.5*tmp49*tmp112*tmp72*tmp53*tmp52*tmp57*tmp99*tmp102 + 1.*tmp49*tmp112*tmp72*tmp52*tmp57*tmp113 + 0.5*tmp49*tmp112*tmp72*tmp89*tmp37*tmp59*tmp113 + 0.5*tmp49*tmp72*tmp52*tmp59*tmp113*tmp148 + 1.*tmp121*(2*tmp122*tmp123*tmp57 + tmp124*tmp115*tmp125*tmp57*tmp60*tmp126))); -J(2+_OFF,1+_OFF) = -(tmp117*(0.5*tmp49*tmp112*tmp24*tmp135*tmp60 - 0.5*tmp49*tmp63*tmp24*tmp48*tmp60 + 0.5*tmp62*tmp49*tmp63*tmp24*tmp53*tmp99*tmp102 + 1.*tmp62*tmp49*tmp63*tmp24*tmp113 + 0.5*tmp49*tmp135*tmp83*tmp139)); -J(2+_OFF,2+_OFF) = -(tmp117*(0.5*tmp49*tmp112*tmp135*tmp48*tmp60 + 0.5*tmp49*tmp112*tmp83 + 0.25*tmp62*tmp49*tmp63*tmp59*tmp144*tmp102 + 1.*tmp62*tmp49*tmp63*tmp48*tmp113 + 0.5*tmp49*tmp63*tmp59*tmp120 + 1.*tmp62*tmp49*tmp110*tmp135*tmp83*tmp102*tmp120)); -J(2+_OFF,3+_OFF) = -(tmp117*(0.5*tmp49*tmp112*tmp135*tmp57*tmp60 - 0.5*tmp49*tmp63*tmp48*tmp57*tmp60 + 0.5*tmp62*tmp49*tmp63*tmp53*tmp57*tmp99*tmp102 + 1.*tmp62*tmp49*tmp63*tmp57*tmp113 + 0.5*tmp49*tmp135*tmp83*tmp148)); -J(3+_OFF,1+_OFF) = -(tmp117*(-1.*tmp49*tmp88*tmp72*tmp24*tmp52 + 0.5*tmp62*tmp49*tmp63*tmp24*tmp82*tmp52*tmp157*tmp60 - 0.5*tmp62*tmp49*tmp63*tmp24*tmp82*tmp89*tmp157*tmp83 + 0.5*tmp49*tmp88*tmp72*tmp24*tmp89*tmp59 - 0.5*tmp49*tmp88*tmp52*tmp59 - 0.5*tmp49*tmp86*tmp72*tmp89*tmp57*tmp59 - 0.5*tmp49*tmp63*tmp24*tmp82*tmp48*tmp52*tmp157*tmp99*tmp102 + 0.5*tmp49*tmp112*tmp24*tmp53*tmp52*tmp157*tmp99*tmp102 + 1.*tmp49*tmp112*tmp24*tmp52*tmp157*tmp113 - 0.5*tmp49*tmp112*tmp24*tmp89*tmp157*tmp59*tmp113 + 0.5*tmp49*tmp52*tmp157*tmp59*tmp113*tmp139)); -J(3+_OFF,2+_OFF) = -(tmp117*(-1.*tmp49*tmp88*tmp72*tmp48*tmp52 + 0.5*tmp62*tmp49*tmp63*tmp82*tmp48*tmp52*tmp157*tmp60 - 0.5*tmp62*tmp49*tmp63*tmp52*tmp157*tmp83 + 0.25*tmp49*tmp112*tmp52*tmp157*tmp59*tmp144*tmp102 + 1.*tmp49*tmp112*tmp48*tmp52*tmp157*tmp113 + 1.*tmp62*tmp49*tmp110*tmp52*tmp157*tmp59*tmp120 + 0.5*tmp49*tmp63*tmp82*tmp52*tmp157*tmp83*tmp102*tmp120)); -J(3+_OFF,3+_OFF) = -(tmp117*(-1.*tmp49*tmp88*tmp72*tmp52*tmp57 + 0.5*tmp62*tmp49*tmp63*tmp82*tmp52*tmp157*tmp57*tmp60 + tmp158 + 0.5*tmp62*tmp49*tmp63*tmp82*tmp89*tmp37*tmp157*tmp83 + 0.5*tmp49*tmp86*tmp72*tmp24*tmp89*tmp59 - 0.5*tmp49*tmp88*tmp72*tmp89*tmp37*tmp59 - 0.5*tmp49*tmp63*tmp82*tmp48*tmp52*tmp157*tmp57*tmp99*tmp102 + 0.5*tmp49*tmp112*tmp53*tmp52*tmp157*tmp57*tmp99*tmp102 + 1.*tmp49*tmp112*tmp52*tmp157*tmp57*tmp113 + tmp159 + 0.5*tmp49*tmp112*tmp89*tmp37*tmp157*tmp59*tmp113 + 0.5*tmp49*tmp52*tmp157*tmp59*tmp113*tmp148)); -J(4+_OFF,1+_OFF) = -(tmp190*tmp181*tmp224) - tmp200*tmp181*tmp230 - tmp211*tmp181*tmp236; -J(4+_OFF,2+_OFF) = -(tmp190*tmp181*tmp243) - tmp200*tmp181*tmp250 - tmp211*tmp181*tmp259; -J(4+_OFF,3+_OFF) = -(tmp190*tmp181*tmp266) - tmp200*tmp181*tmp272 - tmp211*tmp181*tmp278; -J(5+_OFF,1+_OFF) = -(tmp200*tmp181*tmp224) - tmp246*tmp181*tmp230 - tmp253*tmp181*tmp236; -J(5+_OFF,2+_OFF) = -(tmp200*tmp181*tmp243) - tmp246*tmp181*tmp250 - tmp253*tmp181*tmp259; -J(5+_OFF,3+_OFF) = -(tmp200*tmp181*tmp266) - tmp246*tmp181*tmp272 - tmp253*tmp181*tmp278; -J(6+_OFF,1+_OFF) = -(tmp211*tmp181*tmp224) - tmp253*tmp181*tmp230 - tmp281*tmp181*tmp236; -J(6+_OFF,2+_OFF) = -(tmp211*tmp181*tmp243) - tmp253*tmp181*tmp250 - tmp281*tmp181*tmp259; -J(6+_OFF,3+_OFF) = -(tmp211*tmp181*tmp266) - tmp253*tmp181*tmp272 - tmp281*tmp181*tmp278; +J(1+_OFF,1+_OFF) = 0; +J(1+_OFF,2+_OFF) = 0; +J(1+_OFF,3+_OFF) = 0; +J(2+_OFF,1+_OFF) = 0; +J(2+_OFF,2+_OFF) = 0; +J(2+_OFF,3+_OFF) = 0; +J(3+_OFF,1+_OFF) = 0; +J(3+_OFF,2+_OFF) = 0; +J(3+_OFF,3+_OFF) = 0; +J(4+_OFF,1+_OFF) = 0; +J(4+_OFF,2+_OFF) = 0; +J(4+_OFF,3+_OFF) = 0; +J(5+_OFF,1+_OFF) = 0; +J(5+_OFF,2+_OFF) = 0; +J(5+_OFF,3+_OFF) = 0; +J(6+_OFF,1+_OFF) = 0; +J(6+_OFF,2+_OFF) = 0; +J(6+_OFF,3+_OFF) = 0; diff --git a/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrVprev.cppready b/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrVprev.cppready new file mode 100644 index 0000000..41a6cda --- /dev/null +++ b/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrVprev.cppready @@ -0,0 +1,442 @@ +double tmp0 = x(4+_OFF); +double tmp1 = x(6+_OFF); +double tmp2 = x(5+_OFF); +double tmp3 = x(7+_OFF); +double tmp4 = x(30+_OFF); +double tmp5 = x(31+_OFF); +double tmp6 = x(32+_OFF); +double tmp7 = x(33+_OFF); +double tmp8 = tmp5*tmp6; +double tmp9 = std::pow(tmp4,2); +double tmp10 = std::pow(tmp5,2); +double tmp11 = std::pow(tmp6,2); +double tmp12 = std::pow(tmp7,2); +double tmp13 = -tmp12; +double tmp14 = wind(1+_OFF); +double tmp15 = tmp5*tmp7; +double tmp16 = wind(2+_OFF); +double tmp17 = tmp6*tmp7; +double tmp18 = wind(3+_OFF); +double tmp19 = -tmp10; +double tmp20 = -tmp11; +double tmp21 = tmp4*tmp7; +double tmp22 = tmp8 + tmp21; +double tmp23 = -2*tmp16*tmp22; +double tmp24 = -(tmp4*tmp6); +double tmp25 = tmp24 + tmp15; +double tmp26 = -2*tmp18*tmp25; +double tmp27 = tmp9 + tmp10 + tmp20 + tmp13; +double tmp28 = -(tmp14*tmp27); +double tmp29 = x(34+_OFF); +double tmp30 = tmp23 + tmp26 + tmp28 + tmp29; +double tmp31 = tmp4*tmp6; +double tmp32 = tmp31 + tmp15; +double tmp33 = -2*tmp14*tmp32; +double tmp34 = -(tmp4*tmp5); +double tmp35 = tmp34 + tmp17; +double tmp36 = -2*tmp16*tmp35; +double tmp37 = tmp9 + tmp19 + tmp20 + tmp12; +double tmp38 = -(tmp18*tmp37); +double tmp39 = x(36+_OFF); +double tmp40 = tmp33 + tmp36 + tmp38 + tmp39; +double tmp41 = std::atan2(tmp40,tmp30); +double tmp42 = -(tmp4*tmp7); +double tmp43 = tmp8 + tmp42; +double tmp44 = tmp4*tmp5; +double tmp45 = tmp44 + tmp17; +double tmp46 = tmp9 + tmp19 + tmp11 + tmp13; +double tmp47 = -2*tmp14*tmp43; +double tmp48 = -2*tmp18*tmp45; +double tmp49 = -(tmp16*tmp46); +double tmp50 = x(35+_OFF); +double tmp51 = tmp47 + tmp48 + tmp49 + tmp50; +double tmp52 = std::pow(tmp30,2); +double tmp53 = std::pow(tmp51,2); +double tmp54 = std::pow(tmp40,2); +double tmp55 = tmp52 + tmp53 + tmp54; +double tmp56 = 1/std::sqrt(tmp55); +double tmp57 = cp(2+_OFF); +double tmp58 = tmp51*tmp56; +double tmp59 = std::asin(tmp58); +double tmp60 = fLat(1+_OFF); +double tmp61 = 1/tmp55; +double tmp62 = -(tmp53*tmp61); +double tmp63 = 1 + tmp62; +double tmp64 = fDrag(2+_OFF); +double tmp65 = fDrag(3+_OFF); +double tmp66 = 2*tmp14*tmp32; +double tmp67 = 2*tmp16*tmp35; +double tmp68 = tmp18*tmp37; +double tmp69 = -tmp39; +double tmp70 = tmp66 + tmp67 + tmp68 + tmp69; +double tmp71 = tmp52 + tmp54; +double tmp72 = 1/tmp71; +double tmp73 = fDrag(4+_OFF); +double tmp74 = 1/std::sqrt(tmp63); +double tmp75 = std::pow(tmp0,2); +double tmp76 = std::pow(tmp2,2); +double tmp77 = std::pow(tmp1,2); +double tmp78 = -tmp77; +double tmp79 = std::pow(tmp3,2); +double tmp80 = -tmp79; +double tmp81 = tmp75 + tmp76 + tmp78 + tmp80; +double tmp82 = tmp2*tmp1; +double tmp83 = tmp0*tmp3; +double tmp84 = tmp82 + tmp83; +double tmp85 = -(tmp0*tmp1); +double tmp86 = tmp2*tmp3; +double tmp87 = tmp85 + tmp86; +double tmp88 = 1/std::sqrt(tmp71); +double tmp89 = std::sqrt(tmp55); +double tmp90 = fLift(1+_OFF); +double tmp91 = -tmp90; +double tmp92 = fLift(2+_OFF); +double tmp93 = tmp41*tmp92; +double tmp94 = tmp91 + tmp93; +double tmp95 = std::pow(tmp71,-1.5); +double tmp96 = fDrag(1+_OFF); +double tmp97 = -(tmp41*tmp64); +double tmp98 = std::pow(tmp41,2); +double tmp99 = tmp98*tmp65; +double tmp100 = std::pow(tmp59,2); +double tmp101 = tmp100*tmp73; +double tmp102 = tmp96 + tmp97 + tmp99 + tmp101; +double tmp103 = std::sqrt(tmp63); +double tmp104 = -(tmp64*tmp70*tmp72); +double tmp105 = 2*tmp41*tmp65*tmp70*tmp72; +double tmp106 = std::pow(tmp55,-1.5); +double tmp107 = -2*tmp59*tmp73*tmp30*tmp51*tmp106*tmp74; +double tmp108 = tmp104 + tmp105 + tmp107; +double tmp109 = cp(1+_OFF); +double tmp110 = cp(4+_OFF); +double tmp111 = 1/tmp110; +double tmp112 = 2*tmp81*tmp43; +double tmp113 = 4*tmp87*tmp45; +double tmp114 = 2*tmp84*tmp46; +double tmp115 = tmp112 + tmp113 + tmp114; +double tmp116 = -(tmp53*tmp106); +double tmp117 = tmp116 + tmp56; +double tmp118 = 2*tmp81*tmp32; +double tmp119 = 4*tmp84*tmp35; +double tmp120 = 2*tmp87*tmp37; +double tmp121 = tmp118 + tmp119 + tmp120; +double tmp122 = std::pow(tmp51,3); +double tmp123 = std::pow(tmp55,-2); +double tmp124 = 2*tmp122*tmp123; +double tmp125 = -2*tmp51*tmp61; +double tmp126 = tmp124 + tmp125; +double tmp127 = 4*tmp84*tmp22; +double tmp128 = 4*tmp87*tmp25; +double tmp129 = tmp81*tmp27; +double tmp130 = tmp127 + tmp128 + tmp129; +double tmp131 = std::pow(tmp109,2); +double tmp132 = std::pow(M_PI,-2); +double tmp133 = fThrust(3+_OFF); +double tmp134 = 1/M_PI; +double tmp135 = fThrust(2+_OFF); +double tmp136 = z(1+_OFF); +double tmp137 = -0.634*tmp59*tmp57*tmp60*tmp51*tmp88*tmp89; +double tmp138 = 0.634*tmp57*tmp102*tmp88*tmp55*tmp103; +double tmp139 = -(tmp64*tmp30*tmp72); +double tmp140 = 2*tmp41*tmp65*tmp30*tmp72; +double tmp141 = -2*tmp59*tmp73*tmp51*tmp40*tmp106*tmp74; +double tmp142 = tmp139 + tmp140 + tmp141; +double tmp143 = -0.634*tmp59*tmp57*tmp60*tmp30*tmp51*tmp40*tmp88*tmp56; +double tmp144 = 0.634*tmp59*tmp57*tmp60*tmp30*tmp51*tmp40*tmp95*tmp89; +double tmp145 = 0.634*tmp57*tmp102*tmp30*tmp53*tmp40*tmp88*tmp61*tmp74; +double tmp146 = 0.634*tmp57*tmp60*tmp30*tmp53*tmp40*tmp88*tmp61*tmp74; +double tmp147 = 1.268*tmp57*tmp102*tmp30*tmp40*tmp88*tmp103; +double tmp148 = -0.634*tmp57*tmp102*tmp30*tmp40*tmp95*tmp55*tmp103; +double tmp149 = 0.634*tmp57*tmp102*tmp30*tmp51*tmp56; +double tmp150 = -0.634*tmp57*tmp60*tmp30*tmp51*tmp56; +double tmp151 = 0.634*tmp59*tmp57*tmp60*tmp30*tmp53*tmp61*tmp74; +double tmp152 = 1.268*tmp59*tmp57*tmp60*tmp30*tmp103; +double tmp153 = 0.634*tmp57*tmp51*tmp89*tmp108; +double tmp154 = tmp149 + tmp150 + tmp151 + tmp152 + tmp153; +double tmp155 = -(tmp0*tmp3); +double tmp156 = tmp82 + tmp155; +double tmp157 = -tmp76; +double tmp158 = tmp75 + tmp157 + tmp77 + tmp80; +double tmp159 = tmp0*tmp2; +double tmp160 = tmp1*tmp3; +double tmp161 = tmp159 + tmp160; +double tmp162 = 1.268*tmp57*tmp94*tmp52*tmp88; +double tmp163 = -0.634*tmp57*tmp94*tmp52*tmp95*tmp55; +double tmp164 = 0.634*tmp57*tmp92*tmp30*tmp70*tmp95*tmp55; +double tmp165 = 0.634*tmp57*tmp94*tmp88*tmp55; +double tmp166 = 0.634*tmp57*tmp40*tmp88*tmp55*tmp103*tmp108; +double tmp167 = tmp162 + tmp143 + tmp144 + tmp163 + tmp164 + tmp165 + tmp145 + tmp146 + tmp147 + tmp148 + tmp166; +double tmp168 = -1.268*tmp57*tmp94*tmp30*tmp40*tmp88; +double tmp169 = -0.634*tmp59*tmp57*tmp60*tmp52*tmp51*tmp88*tmp56; +double tmp170 = 0.634*tmp59*tmp57*tmp60*tmp52*tmp51*tmp95*tmp89; +double tmp171 = 0.634*tmp57*tmp94*tmp30*tmp40*tmp95*tmp55; +double tmp172 = -0.634*tmp57*tmp92*tmp70*tmp40*tmp95*tmp55; +double tmp173 = 0.634*tmp57*tmp102*tmp52*tmp53*tmp88*tmp61*tmp74; +double tmp174 = 0.634*tmp57*tmp60*tmp52*tmp53*tmp88*tmp61*tmp74; +double tmp175 = 1.268*tmp57*tmp102*tmp52*tmp88*tmp103; +double tmp176 = -0.634*tmp57*tmp102*tmp52*tmp95*tmp55*tmp103; +double tmp177 = 0.634*tmp57*tmp30*tmp88*tmp55*tmp103*tmp108; +double tmp178 = 2*tmp132*tmp133*tmp30; +double tmp179 = tmp134*tmp109*tmp135*tmp30*tmp56*tmp136; +double tmp180 = tmp178 + tmp179; +double tmp181 = 1.268*tmp131*tmp180; +double tmp182 = tmp168 + tmp169 + tmp170 + tmp137 + tmp171 + tmp172 + tmp173 + tmp174 + tmp175 + tmp176 + tmp138 + tmp177 + tmp181; +double tmp183 = 4*tmp156*tmp43; +double tmp184 = 4*tmp161*tmp45; +double tmp185 = tmp158*tmp46; +double tmp186 = tmp183 + tmp184 + tmp185; +double tmp187 = 0.634*tmp57*tmp102*tmp53*tmp56; +double tmp188 = 0.634*tmp57*tmp102*tmp89; +double tmp189 = 0.317*tmp59*tmp57*tmp60*tmp55*tmp126*tmp74; +double tmp190 = 1.268*tmp59*tmp57*tmp60*tmp51*tmp103; +double tmp191 = 0.634*tmp57*tmp60*tmp55*tmp117; +double tmp192 = 1.268*tmp59*tmp57*tmp73*tmp51*tmp89*tmp74*tmp117; +double tmp193 = tmp187 + tmp188 + tmp189 + tmp190 + tmp191 + tmp192; +double tmp194 = 4*tmp156*tmp32; +double tmp195 = 2*tmp158*tmp35; +double tmp196 = 2*tmp161*tmp37; +double tmp197 = tmp194 + tmp195 + tmp196; +double tmp198 = 1.268*tmp57*tmp94*tmp30*tmp51*tmp88; +double tmp199 = -0.634*tmp59*tmp57*tmp60*tmp53*tmp40*tmp88*tmp56; +double tmp200 = -0.634*tmp59*tmp57*tmp60*tmp40*tmp88*tmp89; +double tmp201 = 0.317*tmp57*tmp102*tmp40*tmp88*tmp55*tmp126*tmp74; +double tmp202 = 1.268*tmp57*tmp102*tmp51*tmp40*tmp88*tmp103; +double tmp203 = 1.268*tmp59*tmp57*tmp73*tmp40*tmp88*tmp55*tmp117; +double tmp204 = -0.634*tmp57*tmp60*tmp51*tmp40*tmp88*tmp89*tmp74*tmp117; +double tmp205 = tmp198 + tmp199 + tmp200 + tmp201 + tmp202 + tmp203 + tmp204; +double tmp206 = 2*tmp158*tmp22; +double tmp207 = 4*tmp161*tmp25; +double tmp208 = 2*tmp156*tmp27; +double tmp209 = tmp206 + tmp207 + tmp208; +double tmp210 = -1.268*tmp57*tmp94*tmp51*tmp40*tmp88; +double tmp211 = -0.634*tmp59*tmp57*tmp60*tmp30*tmp53*tmp88*tmp56; +double tmp212 = -0.634*tmp59*tmp57*tmp60*tmp30*tmp88*tmp89; +double tmp213 = 0.317*tmp57*tmp102*tmp30*tmp88*tmp55*tmp126*tmp74; +double tmp214 = 1.268*tmp57*tmp102*tmp30*tmp51*tmp88*tmp103; +double tmp215 = 1.268*tmp59*tmp57*tmp73*tmp30*tmp88*tmp55*tmp117; +double tmp216 = -0.634*tmp57*tmp60*tmp30*tmp51*tmp88*tmp89*tmp74*tmp117; +double tmp217 = 2*tmp132*tmp133*tmp51; +double tmp218 = tmp134*tmp109*tmp135*tmp51*tmp56*tmp136; +double tmp219 = tmp217 + tmp218; +double tmp220 = 1.268*tmp131*tmp219; +double tmp221 = tmp210 + tmp211 + tmp212 + tmp213 + tmp214 + tmp215 + tmp216 + tmp220; +double tmp222 = 0.634*tmp57*tmp102*tmp51*tmp40*tmp56; +double tmp223 = -0.634*tmp57*tmp60*tmp51*tmp40*tmp56; +double tmp224 = 0.634*tmp59*tmp57*tmp60*tmp53*tmp40*tmp61*tmp74; +double tmp225 = 1.268*tmp59*tmp57*tmp60*tmp40*tmp103; +double tmp226 = 0.634*tmp57*tmp51*tmp89*tmp142; +double tmp227 = tmp222 + tmp223 + tmp224 + tmp225 + tmp226; +double tmp228 = 1.268*tmp57*tmp94*tmp30*tmp40*tmp88; +double tmp229 = -0.634*tmp59*tmp57*tmp60*tmp51*tmp54*tmp88*tmp56; +double tmp230 = 0.634*tmp59*tmp57*tmp60*tmp51*tmp54*tmp95*tmp89; +double tmp231 = 0.634*tmp57*tmp92*tmp52*tmp95*tmp55; +double tmp232 = -0.634*tmp57*tmp94*tmp30*tmp40*tmp95*tmp55; +double tmp233 = 0.634*tmp57*tmp102*tmp53*tmp54*tmp88*tmp61*tmp74; +double tmp234 = 0.634*tmp57*tmp60*tmp53*tmp54*tmp88*tmp61*tmp74; +double tmp235 = 1.268*tmp57*tmp102*tmp54*tmp88*tmp103; +double tmp236 = -0.634*tmp57*tmp102*tmp54*tmp95*tmp55*tmp103; +double tmp237 = 0.634*tmp57*tmp40*tmp88*tmp55*tmp103*tmp142; +double tmp238 = tmp228 + tmp229 + tmp230 + tmp137 + tmp231 + tmp232 + tmp233 + tmp234 + tmp235 + tmp236 + tmp138 + tmp237; +double tmp239 = -1.268*tmp57*tmp94*tmp54*tmp88; +double tmp240 = -0.634*tmp57*tmp92*tmp30*tmp40*tmp95*tmp55; +double tmp241 = 0.634*tmp57*tmp94*tmp54*tmp95*tmp55; +double tmp242 = -0.634*tmp57*tmp94*tmp88*tmp55; +double tmp243 = 0.634*tmp57*tmp30*tmp88*tmp55*tmp103*tmp142; +double tmp244 = 2*tmp132*tmp133*tmp40; +double tmp245 = tmp134*tmp109*tmp135*tmp40*tmp56*tmp136; +double tmp246 = tmp244 + tmp245; +double tmp247 = 1.268*tmp131*tmp246; +double tmp248 = tmp239 + tmp143 + tmp144 + tmp240 + tmp241 + tmp242 + tmp145 + tmp146 + tmp147 + tmp148 + tmp243 + tmp247; +double tmp249 = tmp0*tmp1; +double tmp250 = tmp249 + tmp86; +double tmp251 = -(tmp0*tmp2); +double tmp252 = tmp251 + tmp160; +double tmp253 = tmp75 + tmp157 + tmp78 + tmp79; +double tmp254 = 4*tmp250*tmp43; +double tmp255 = 2*tmp253*tmp45; +double tmp256 = 2*tmp252*tmp46; +double tmp257 = tmp254 + tmp255 + tmp256; +double tmp258 = 4*tmp250*tmp32; +double tmp259 = 4*tmp252*tmp35; +double tmp260 = tmp253*tmp37; +double tmp261 = tmp258 + tmp259 + tmp260; +double tmp262 = 4*tmp252*tmp22; +double tmp263 = 2*tmp253*tmp25; +double tmp264 = 2*tmp250*tmp27; +double tmp265 = tmp262 + tmp263 + tmp264; +double tmp266 = ibd(2+_OFF); +double tmp267 = ibd(3+_OFF); +double tmp268 = ibod(1+_OFF); +double tmp269 = ibod(2+_OFF); +double tmp270 = ibod(3+_OFF); +double tmp271 = ibd(1+_OFF); +double tmp272 = std::pow(tmp270,2); +double tmp273 = cBar(1+_OFF); +double tmp274 = cp(3+_OFF); +double tmp275 = x(39+_OFF); +double tmp276 = mRoll(2+_OFF); +double tmp277 = mRoll(3+_OFF); +double tmp278 = x(37+_OFF); +double tmp279 = mRoll(4+_OFF); +double tmp280 = mPitch(3+_OFF); +double tmp281 = mPitch(4+_OFF); +double tmp282 = x(38+_OFF); +double tmp283 = mYaw(2+_OFF); +double tmp284 = mYaw(3+_OFF); +double tmp285 = tmp271*tmp266*tmp267; +double tmp286 = std::pow(tmp268,2); +double tmp287 = -(tmp266*tmp286); +double tmp288 = std::pow(tmp269,2); +double tmp289 = -(tmp267*tmp288); +double tmp290 = 2*tmp268*tmp269*tmp270; +double tmp291 = -(tmp271*tmp272); +double tmp292 = tmp285 + tmp287 + tmp289 + tmp290 + tmp291; +double tmp293 = 1/tmp292; +double tmp294 = tmp280*tmp70*tmp72; +double tmp295 = -0.5*(tmp273*tmp281*tmp30*tmp106*tmp282); +double tmp296 = tmp294 + tmp295; +double tmp297 = tmp276*tmp30*tmp51*tmp106*tmp74; +double tmp298 = -0.5*(tmp274*tmp277*tmp30*tmp106*tmp278); +double tmp299 = (tmp274*tmp279*tmp30*tmp106*tmp275)/2.; +double tmp300 = tmp297 + tmp298 + tmp299; +double tmp301 = -(tmp283*tmp30*tmp51*tmp106*tmp74); +double tmp302 = -0.5*(tmp274*tmp284*tmp30*tmp106*tmp275); +double tmp303 = tmp301 + tmp302; +double tmp304 = -(tmp59*tmp276); +double tmp305 = (tmp274*tmp277*tmp56*tmp278)/2.; +double tmp306 = -0.5*(tmp274*tmp279*tmp56*tmp275); +double tmp307 = mRoll(1+_OFF); +double tmp308 = z(2+_OFF); +double tmp309 = tmp307*tmp308; +double tmp310 = tmp304 + tmp305 + tmp306 + tmp309; +double tmp311 = mPitch(1+_OFF); +double tmp312 = -tmp311; +double tmp313 = tmp41*tmp280; +double tmp314 = (tmp273*tmp281*tmp56*tmp282)/2.; +double tmp315 = mPitch(2+_OFF); +double tmp316 = z(3+_OFF); +double tmp317 = -(tmp315*tmp316); +double tmp318 = tmp312 + tmp313 + tmp314 + tmp317; +double tmp319 = tmp59*tmp283; +double tmp320 = (tmp274*tmp284*tmp56*tmp275)/2.; +double tmp321 = mYaw(1+_OFF); +double tmp322 = z(4+_OFF); +double tmp323 = -(tmp321*tmp322); +double tmp324 = tmp319 + tmp320 + tmp323; +double tmp325 = tmp266*tmp267; +double tmp326 = -tmp272; +double tmp327 = tmp325 + tmp326; +double tmp328 = -(tmp267*tmp269); +double tmp329 = tmp268*tmp270; +double tmp330 = tmp328 + tmp329; +double tmp331 = std::pow(tmp273,2); +double tmp332 = -(tmp276*tmp74*tmp117); +double tmp333 = -0.5*(tmp274*tmp277*tmp51*tmp106*tmp278); +double tmp334 = (tmp274*tmp279*tmp51*tmp106*tmp275)/2.; +double tmp335 = tmp332 + tmp333 + tmp334; +double tmp336 = tmp283*tmp74*tmp117; +double tmp337 = -0.5*(tmp274*tmp284*tmp51*tmp106*tmp275); +double tmp338 = tmp336 + tmp337; +double tmp339 = -(tmp266*tmp268); +double tmp340 = tmp269*tmp270; +double tmp341 = tmp339 + tmp340; +double tmp342 = tmp280*tmp30*tmp72; +double tmp343 = -0.5*(tmp273*tmp281*tmp40*tmp106*tmp282); +double tmp344 = tmp342 + tmp343; +double tmp345 = tmp276*tmp51*tmp40*tmp106*tmp74; +double tmp346 = -0.5*(tmp274*tmp277*tmp40*tmp106*tmp278); +double tmp347 = (tmp274*tmp279*tmp40*tmp106*tmp275)/2.; +double tmp348 = tmp345 + tmp346 + tmp347; +double tmp349 = -(tmp283*tmp51*tmp40*tmp106*tmp74); +double tmp350 = -0.5*(tmp274*tmp284*tmp40*tmp106*tmp275); +double tmp351 = tmp349 + tmp350; +double tmp352 = 0.634*tmp273*tmp57*tmp115*tmp55*tmp296; +double tmp353 = 0.634*tmp57*tmp274*tmp130*tmp55*tmp300; +double tmp354 = 0.634*tmp57*tmp274*tmp121*tmp55*tmp303; +double tmp355 = 1.268*tmp57*tmp274*tmp130*tmp30*tmp310; +double tmp356 = 1.268*tmp273*tmp57*tmp115*tmp30*tmp318; +double tmp357 = 1.268*tmp57*tmp274*tmp121*tmp30*tmp324; +double tmp358 = tmp352 + tmp353 + tmp354 + tmp355 + tmp356 + tmp357; +double tmp359 = 0.634*tmp273*tmp57*tmp186*tmp55*tmp296; +double tmp360 = 0.634*tmp57*tmp274*tmp209*tmp55*tmp300; +double tmp361 = 0.634*tmp57*tmp274*tmp197*tmp55*tmp303; +double tmp362 = 1.268*tmp57*tmp274*tmp209*tmp30*tmp310; +double tmp363 = 1.268*tmp273*tmp57*tmp186*tmp30*tmp318; +double tmp364 = 1.268*tmp57*tmp274*tmp197*tmp30*tmp324; +double tmp365 = tmp359 + tmp360 + tmp361 + tmp362 + tmp363 + tmp364; +double tmp366 = 0.634*tmp273*tmp57*tmp257*tmp55*tmp296; +double tmp367 = 0.634*tmp57*tmp274*tmp265*tmp55*tmp300; +double tmp368 = 0.634*tmp57*tmp274*tmp261*tmp55*tmp303; +double tmp369 = 1.268*tmp57*tmp274*tmp265*tmp30*tmp310; +double tmp370 = 1.268*tmp273*tmp57*tmp257*tmp30*tmp318; +double tmp371 = 1.268*tmp57*tmp274*tmp261*tmp30*tmp324; +double tmp372 = tmp366 + tmp367 + tmp368 + tmp369 + tmp370 + tmp371; +double tmp373 = -0.317*tmp331*tmp57*tmp281*tmp115*tmp51*tmp56*tmp282; +double tmp374 = 0.634*tmp57*tmp274*tmp130*tmp55*tmp335; +double tmp375 = 0.634*tmp57*tmp274*tmp121*tmp55*tmp338; +double tmp376 = 1.268*tmp57*tmp274*tmp130*tmp51*tmp310; +double tmp377 = 1.268*tmp273*tmp57*tmp115*tmp51*tmp318; +double tmp378 = 1.268*tmp57*tmp274*tmp121*tmp51*tmp324; +double tmp379 = tmp373 + tmp374 + tmp375 + tmp376 + tmp377 + tmp378; +double tmp380 = tmp271*tmp267; +double tmp381 = -tmp286; +double tmp382 = tmp380 + tmp381; +double tmp383 = -0.317*tmp331*tmp57*tmp281*tmp186*tmp51*tmp56*tmp282; +double tmp384 = 0.634*tmp57*tmp274*tmp209*tmp55*tmp335; +double tmp385 = 0.634*tmp57*tmp274*tmp197*tmp55*tmp338; +double tmp386 = 1.268*tmp57*tmp274*tmp209*tmp51*tmp310; +double tmp387 = 1.268*tmp273*tmp57*tmp186*tmp51*tmp318; +double tmp388 = 1.268*tmp57*tmp274*tmp197*tmp51*tmp324; +double tmp389 = tmp383 + tmp384 + tmp385 + tmp386 + tmp387 + tmp388; +double tmp390 = tmp268*tmp269; +double tmp391 = -(tmp271*tmp270); +double tmp392 = tmp390 + tmp391; +double tmp393 = -0.317*tmp331*tmp57*tmp281*tmp257*tmp51*tmp56*tmp282; +double tmp394 = 0.634*tmp57*tmp274*tmp265*tmp55*tmp335; +double tmp395 = 0.634*tmp57*tmp274*tmp261*tmp55*tmp338; +double tmp396 = 1.268*tmp57*tmp274*tmp265*tmp51*tmp310; +double tmp397 = 1.268*tmp273*tmp57*tmp257*tmp51*tmp318; +double tmp398 = 1.268*tmp57*tmp274*tmp261*tmp51*tmp324; +double tmp399 = tmp393 + tmp394 + tmp395 + tmp396 + tmp397 + tmp398; +double tmp400 = 0.634*tmp273*tmp57*tmp115*tmp55*tmp344; +double tmp401 = 0.634*tmp57*tmp274*tmp130*tmp55*tmp348; +double tmp402 = 0.634*tmp57*tmp274*tmp121*tmp55*tmp351; +double tmp403 = 1.268*tmp57*tmp274*tmp130*tmp40*tmp310; +double tmp404 = 1.268*tmp273*tmp57*tmp115*tmp40*tmp318; +double tmp405 = 1.268*tmp57*tmp274*tmp121*tmp40*tmp324; +double tmp406 = tmp400 + tmp401 + tmp402 + tmp403 + tmp404 + tmp405; +double tmp407 = 0.634*tmp273*tmp57*tmp186*tmp55*tmp344; +double tmp408 = 0.634*tmp57*tmp274*tmp209*tmp55*tmp348; +double tmp409 = 0.634*tmp57*tmp274*tmp197*tmp55*tmp351; +double tmp410 = 1.268*tmp57*tmp274*tmp209*tmp40*tmp310; +double tmp411 = 1.268*tmp273*tmp57*tmp186*tmp40*tmp318; +double tmp412 = 1.268*tmp57*tmp274*tmp197*tmp40*tmp324; +double tmp413 = tmp407 + tmp408 + tmp409 + tmp410 + tmp411 + tmp412; +double tmp414 = 0.634*tmp273*tmp57*tmp257*tmp55*tmp344; +double tmp415 = 0.634*tmp57*tmp274*tmp265*tmp55*tmp348; +double tmp416 = 0.634*tmp57*tmp274*tmp261*tmp55*tmp351; +double tmp417 = 1.268*tmp57*tmp274*tmp265*tmp40*tmp310; +double tmp418 = 1.268*tmp273*tmp57*tmp257*tmp40*tmp318; +double tmp419 = 1.268*tmp57*tmp274*tmp261*tmp40*tmp324; +double tmp420 = tmp414 + tmp415 + tmp416 + tmp417 + tmp418 + tmp419; +double tmp421 = tmp271*tmp266; +double tmp422 = -tmp288; +double tmp423 = tmp421 + tmp422; +J(1+_OFF,1+_OFF) = -(tmp111*(tmp115*tmp154 + tmp121*tmp167 + tmp130*tmp182)); +J(1+_OFF,2+_OFF) = -(tmp111*(tmp115*tmp193 + tmp121*tmp205 + tmp130*tmp221)); +J(1+_OFF,3+_OFF) = -(tmp111*(tmp115*tmp227 + tmp121*tmp238 + tmp130*tmp248)); +J(2+_OFF,1+_OFF) = -(tmp111*(tmp186*tmp154 + tmp197*tmp167 + tmp209*tmp182)); +J(2+_OFF,2+_OFF) = -(tmp111*(tmp186*tmp193 + tmp197*tmp205 + tmp209*tmp221)); +J(2+_OFF,3+_OFF) = -(tmp111*(tmp186*tmp227 + tmp197*tmp238 + tmp209*tmp248)); +J(3+_OFF,1+_OFF) = -(tmp111*(tmp257*tmp154 + tmp261*tmp167 + tmp265*tmp182)); +J(3+_OFF,2+_OFF) = -(tmp111*(tmp257*tmp193 + tmp261*tmp205 + tmp265*tmp221)); +J(3+_OFF,3+_OFF) = -(tmp111*(tmp257*tmp227 + tmp261*tmp238 + tmp265*tmp248)); +J(4+_OFF,1+_OFF) = -(tmp327*tmp293*tmp358) - tmp330*tmp293*tmp365 - tmp341*tmp293*tmp372; +J(4+_OFF,2+_OFF) = -(tmp327*tmp293*tmp379) - tmp330*tmp293*tmp389 - tmp341*tmp293*tmp399; +J(4+_OFF,3+_OFF) = -(tmp327*tmp293*tmp406) - tmp330*tmp293*tmp413 - tmp341*tmp293*tmp420; +J(5+_OFF,1+_OFF) = -(tmp330*tmp293*tmp358) - tmp382*tmp293*tmp365 - tmp392*tmp293*tmp372; +J(5+_OFF,2+_OFF) = -(tmp330*tmp293*tmp379) - tmp382*tmp293*tmp389 - tmp392*tmp293*tmp399; +J(5+_OFF,3+_OFF) = -(tmp330*tmp293*tmp406) - tmp382*tmp293*tmp413 - tmp392*tmp293*tmp420; +J(6+_OFF,1+_OFF) = -(tmp341*tmp293*tmp358) - tmp392*tmp293*tmp365 - tmp423*tmp293*tmp372; +J(6+_OFF,2+_OFF) = -(tmp341*tmp293*tmp379) - tmp392*tmp293*tmp389 - tmp423*tmp293*tmp399; +J(6+_OFF,3+_OFF) = -(tmp341*tmp293*tmp406) - tmp392*tmp293*tmp413 - tmp423*tmp293*tmp420; diff --git a/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrW.cppready b/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrW.cppready index 0d49085..db64d2e 100644 --- a/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrW.cppready +++ b/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrW.cppready @@ -15,130 +15,65 @@ double tmp13 = 2*tmp2*tmp3*tmp4; double tmp14 = -(tmp5*tmp7); double tmp15 = tmp8 + tmp10 + tmp12 + tmp13 + tmp14; double tmp16 = 1/tmp15; -double tmp17 = x(4+_OFF); -double tmp18 = x(6+_OFF); -double tmp19 = x(5+_OFF); -double tmp20 = x(7+_OFF); -double tmp21 = wind(1+_OFF); -double tmp22 = tmp19*tmp18; -double tmp23 = wind(3+_OFF); -double tmp24 = wind(2+_OFF); -double tmp25 = std::pow(tmp17,2); -double tmp26 = std::pow(tmp19,2); -double tmp27 = std::pow(tmp18,2); -double tmp28 = std::pow(tmp20,2); -double tmp29 = -tmp28; -double tmp30 = tmp19*tmp20; -double tmp31 = tmp18*tmp20; -double tmp32 = -tmp26; -double tmp33 = -tmp27; -double tmp34 = x(12+_OFF); -double tmp35 = x(11+_OFF); -double tmp36 = tmp1*tmp0; -double tmp37 = -tmp7; -double tmp38 = tmp36 + tmp37; -double tmp39 = -(tmp1*tmp2); -double tmp40 = tmp3*tmp4; -double tmp41 = tmp39 + tmp40; -double tmp42 = -(tmp0*tmp3); -double tmp43 = tmp2*tmp4; -double tmp44 = tmp42 + tmp43; -double tmp45 = cp(2+_OFF); -double tmp46 = tmp17*tmp20; -double tmp47 = tmp22 + tmp46; -double tmp48 = -2*tmp24*tmp47; -double tmp49 = -(tmp17*tmp18); -double tmp50 = tmp49 + tmp30; -double tmp51 = -2*tmp23*tmp50; -double tmp52 = tmp25 + tmp26 + tmp33 + tmp29; -double tmp53 = -(tmp21*tmp52); -double tmp54 = x(8+_OFF); -double tmp55 = tmp48 + tmp51 + tmp53 + tmp54; -double tmp56 = std::pow(tmp55,2); -double tmp57 = -(tmp17*tmp20); -double tmp58 = tmp22 + tmp57; -double tmp59 = -2*tmp21*tmp58; -double tmp60 = tmp17*tmp19; -double tmp61 = tmp60 + tmp31; -double tmp62 = -2*tmp23*tmp61; -double tmp63 = tmp25 + tmp32 + tmp27 + tmp29; -double tmp64 = -(tmp24*tmp63); -double tmp65 = x(9+_OFF); -double tmp66 = tmp59 + tmp62 + tmp64 + tmp65; -double tmp67 = std::pow(tmp66,2); -double tmp68 = tmp17*tmp18; -double tmp69 = tmp68 + tmp30; -double tmp70 = -2*tmp21*tmp69; -double tmp71 = -(tmp17*tmp19); -double tmp72 = tmp71 + tmp31; -double tmp73 = -2*tmp24*tmp72; -double tmp74 = tmp25 + tmp32 + tmp33 + tmp28; -double tmp75 = -(tmp23*tmp74); -double tmp76 = x(10+_OFF); -double tmp77 = tmp70 + tmp73 + tmp75 + tmp76; -double tmp78 = std::pow(tmp77,2); -double tmp79 = tmp56 + tmp67 + tmp78; -double tmp80 = std::sqrt(tmp79); -double tmp81 = cp(3+_OFF); -double tmp82 = std::pow(tmp81,2); -double tmp83 = 2*tmp2*tmp35; -double tmp84 = tmp4*tmp34; -double tmp85 = -(tmp5*tmp6); -double tmp86 = tmp0*tmp6; -double tmp87 = tmp83 + tmp84 + tmp85 + tmp86; -double tmp88 = mRoll(3+_OFF); -double tmp89 = 0.25*tmp45*tmp82*tmp88*tmp80; -double tmp90 = -(tmp2*tmp34); -double tmp91 = tmp3*tmp6; -double tmp92 = tmp89 + tmp90 + tmp91; -double tmp93 = -2*tmp3*tmp35; -double tmp94 = tmp5*tmp34; -double tmp95 = -(tmp1*tmp34); -double tmp96 = -(tmp4*tmp6); -double tmp97 = tmp93 + tmp94 + tmp95 + tmp96; -double tmp98 = -(tmp2*tmp35); -double tmp99 = -2*tmp4*tmp34; -double tmp100 = tmp1*tmp6; -double tmp101 = -(tmp0*tmp6); -double tmp102 = tmp98 + tmp99 + tmp100 + tmp101; -double tmp103 = tmp2*tmp3; -double tmp104 = -(tmp5*tmp4); -double tmp105 = tmp103 + tmp104; -double tmp106 = tmp5*tmp35; -double tmp107 = -(tmp1*tmp35); -double tmp108 = 2*tmp3*tmp34; -double tmp109 = tmp2*tmp6; -double tmp110 = tmp106 + tmp107 + tmp108 + tmp109; -double tmp111 = tmp5*tmp0; -double tmp112 = -tmp9; -double tmp113 = tmp111 + tmp112; -double tmp114 = cBar(1+_OFF); -double tmp115 = std::pow(tmp114,2); -double tmp116 = mPitch(4+_OFF); -double tmp117 = 0.25*tmp115*tmp45*tmp116*tmp80; -double tmp118 = tmp4*tmp35; -double tmp119 = -(tmp3*tmp6); -double tmp120 = tmp117 + tmp118 + tmp119; -double tmp121 = mYaw(3+_OFF); -double tmp122 = 0.25*tmp45*tmp82*tmp121*tmp80; -double tmp123 = -(tmp4*tmp35); -double tmp124 = tmp2*tmp34; -double tmp125 = tmp122 + tmp123 + tmp124; -double tmp126 = -(tmp5*tmp35); -double tmp127 = tmp0*tmp35; -double tmp128 = -(tmp3*tmp34); -double tmp129 = -2*tmp2*tmp6; -double tmp130 = tmp126 + tmp127 + tmp128 + tmp129; -double tmp131 = mRoll(4+_OFF); -double tmp132 = -0.25*tmp45*tmp82*tmp131*tmp80; -double tmp133 = tmp3*tmp35; -double tmp134 = tmp1*tmp34; -double tmp135 = -(tmp0*tmp34); -double tmp136 = 2*tmp4*tmp6; -double tmp137 = tmp132 + tmp133 + tmp134 + tmp135 + tmp136; -double tmp138 = tmp5*tmp1; -double tmp139 = -tmp11; -double tmp140 = tmp138 + tmp139; +double tmp17 = x(12+_OFF); +double tmp18 = x(11+_OFF); +double tmp19 = tmp1*tmp0; +double tmp20 = -tmp7; +double tmp21 = tmp19 + tmp20; +double tmp22 = -(tmp1*tmp2); +double tmp23 = tmp3*tmp4; +double tmp24 = tmp22 + tmp23; +double tmp25 = -(tmp0*tmp3); +double tmp26 = tmp2*tmp4; +double tmp27 = tmp25 + tmp26; +double tmp28 = 2*tmp2*tmp18; +double tmp29 = tmp4*tmp17; +double tmp30 = -(tmp5*tmp6); +double tmp31 = tmp0*tmp6; +double tmp32 = tmp28 + tmp29 + tmp30 + tmp31; +double tmp33 = -(tmp2*tmp17); +double tmp34 = tmp3*tmp6; +double tmp35 = tmp33 + tmp34; +double tmp36 = -2*tmp3*tmp18; +double tmp37 = tmp5*tmp17; +double tmp38 = -(tmp1*tmp17); +double tmp39 = -(tmp4*tmp6); +double tmp40 = tmp36 + tmp37 + tmp38 + tmp39; +double tmp41 = -(tmp2*tmp18); +double tmp42 = -2*tmp4*tmp17; +double tmp43 = tmp1*tmp6; +double tmp44 = -(tmp0*tmp6); +double tmp45 = tmp41 + tmp42 + tmp43 + tmp44; +double tmp46 = tmp2*tmp3; +double tmp47 = -(tmp5*tmp4); +double tmp48 = tmp46 + tmp47; +double tmp49 = tmp5*tmp18; +double tmp50 = -(tmp1*tmp18); +double tmp51 = 2*tmp3*tmp17; +double tmp52 = tmp2*tmp6; +double tmp53 = tmp49 + tmp50 + tmp51 + tmp52; +double tmp54 = tmp5*tmp0; +double tmp55 = -tmp9; +double tmp56 = tmp54 + tmp55; +double tmp57 = tmp4*tmp18; +double tmp58 = -(tmp3*tmp6); +double tmp59 = tmp57 + tmp58; +double tmp60 = -(tmp4*tmp18); +double tmp61 = tmp2*tmp17; +double tmp62 = tmp60 + tmp61; +double tmp63 = -(tmp5*tmp18); +double tmp64 = tmp0*tmp18; +double tmp65 = -(tmp3*tmp17); +double tmp66 = -2*tmp2*tmp6; +double tmp67 = tmp63 + tmp64 + tmp65 + tmp66; +double tmp68 = tmp3*tmp18; +double tmp69 = tmp1*tmp17; +double tmp70 = -(tmp0*tmp17); +double tmp71 = 2*tmp4*tmp6; +double tmp72 = tmp68 + tmp69 + tmp70 + tmp71; +double tmp73 = tmp5*tmp1; +double tmp74 = -tmp11; +double tmp75 = tmp73 + tmp74; J(1+_OFF,1+_OFF) = 0; J(1+_OFF,2+_OFF) = 0; J(1+_OFF,3+_OFF) = 0; @@ -148,12 +83,12 @@ J(2+_OFF,3+_OFF) = 0; J(3+_OFF,1+_OFF) = 0; J(3+_OFF,2+_OFF) = 0; J(3+_OFF,3+_OFF) = 0; -J(4+_OFF,1+_OFF) = -(tmp44*tmp16*tmp87) - tmp38*tmp16*tmp92 - tmp41*tmp16*tmp97; -J(4+_OFF,2+_OFF) = -(tmp38*tmp16*tmp102) - tmp41*tmp16*tmp110 - tmp44*tmp16*tmp120; -J(4+_OFF,3+_OFF) = -(tmp41*tmp16*tmp125) - tmp44*tmp16*tmp130 - tmp38*tmp16*tmp137; -J(5+_OFF,1+_OFF) = -(tmp113*tmp16*tmp87) - tmp44*tmp16*tmp92 - tmp105*tmp16*tmp97; -J(5+_OFF,2+_OFF) = -(tmp44*tmp16*tmp102) - tmp105*tmp16*tmp110 - tmp113*tmp16*tmp120; -J(5+_OFF,3+_OFF) = -(tmp105*tmp16*tmp125) - tmp113*tmp16*tmp130 - tmp44*tmp16*tmp137; -J(6+_OFF,1+_OFF) = -(tmp105*tmp16*tmp87) - tmp41*tmp16*tmp92 - tmp140*tmp16*tmp97; -J(6+_OFF,2+_OFF) = -(tmp41*tmp16*tmp102) - tmp140*tmp16*tmp110 - tmp105*tmp16*tmp120; -J(6+_OFF,3+_OFF) = -(tmp140*tmp16*tmp125) - tmp105*tmp16*tmp130 - tmp41*tmp16*tmp137; +J(4+_OFF,1+_OFF) = -(tmp27*tmp16*tmp32) - tmp21*tmp16*tmp35 - tmp24*tmp16*tmp40; +J(4+_OFF,2+_OFF) = -(tmp21*tmp16*tmp45) - tmp24*tmp16*tmp53 - tmp27*tmp16*tmp59; +J(4+_OFF,3+_OFF) = -(tmp24*tmp16*tmp62) - tmp27*tmp16*tmp67 - tmp21*tmp16*tmp72; +J(5+_OFF,1+_OFF) = -(tmp56*tmp16*tmp32) - tmp27*tmp16*tmp35 - tmp48*tmp16*tmp40; +J(5+_OFF,2+_OFF) = -(tmp27*tmp16*tmp45) - tmp48*tmp16*tmp53 - tmp56*tmp16*tmp59; +J(5+_OFF,3+_OFF) = -(tmp48*tmp16*tmp62) - tmp56*tmp16*tmp67 - tmp27*tmp16*tmp72; +J(6+_OFF,1+_OFF) = -(tmp48*tmp16*tmp32) - tmp24*tmp16*tmp35 - tmp75*tmp16*tmp40; +J(6+_OFF,2+_OFF) = -(tmp24*tmp16*tmp45) - tmp75*tmp16*tmp53 - tmp48*tmp16*tmp59; +J(6+_OFF,3+_OFF) = -(tmp75*tmp16*tmp62) - tmp48*tmp16*tmp67 - tmp24*tmp16*tmp72; diff --git a/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrWind.cppready b/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrWind.cppready index b8f53c2..0fc8a52 100644 --- a/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrWind.cppready +++ b/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrWind.cppready @@ -2,362 +2,521 @@ double tmp0 = x(4+_OFF); double tmp1 = x(6+_OFF); double tmp2 = x(5+_OFF); double tmp3 = x(7+_OFF); -double tmp4 = wind(1+_OFF); -double tmp5 = tmp2*tmp3; -double tmp6 = wind(2+_OFF); -double tmp7 = wind(3+_OFF); -double tmp8 = std::pow(tmp0,2); -double tmp9 = std::pow(tmp2,2); -double tmp10 = std::pow(tmp1,2); -double tmp11 = -tmp10; -double tmp12 = std::pow(tmp3,2); -double tmp13 = tmp2*tmp1; -double tmp14 = tmp0*tmp3; -double tmp15 = tmp13 + tmp14; -double tmp16 = -2*tmp6*tmp15; -double tmp17 = -(tmp0*tmp1); -double tmp18 = tmp17 + tmp5; -double tmp19 = -2*tmp7*tmp18; -double tmp20 = -tmp12; -double tmp21 = tmp8 + tmp9 + tmp11 + tmp20; -double tmp22 = -(tmp4*tmp21); -double tmp23 = x(8+_OFF); -double tmp24 = tmp16 + tmp19 + tmp22 + tmp23; -double tmp25 = tmp0*tmp1; -double tmp26 = tmp25 + tmp5; -double tmp27 = 2*tmp4*tmp26; -double tmp28 = -(tmp0*tmp2); -double tmp29 = tmp1*tmp3; -double tmp30 = tmp28 + tmp29; -double tmp31 = 2*tmp6*tmp30; -double tmp32 = -tmp9; -double tmp33 = tmp8 + tmp32 + tmp11 + tmp12; -double tmp34 = tmp7*tmp33; -double tmp35 = x(10+_OFF); -double tmp36 = -tmp35; -double tmp37 = tmp27 + tmp31 + tmp34 + tmp36; -double tmp38 = -(tmp0*tmp3); -double tmp39 = tmp13 + tmp38; -double tmp40 = -2*tmp4*tmp39; -double tmp41 = tmp0*tmp2; -double tmp42 = tmp41 + tmp29; -double tmp43 = -2*tmp7*tmp42; -double tmp44 = tmp8 + tmp32 + tmp10 + tmp20; -double tmp45 = -(tmp6*tmp44); -double tmp46 = x(9+_OFF); -double tmp47 = tmp40 + tmp43 + tmp45 + tmp46; -double tmp48 = std::pow(tmp24,2); -double tmp49 = -2*tmp4*tmp26; -double tmp50 = -2*tmp6*tmp30; -double tmp51 = -(tmp7*tmp33); -double tmp52 = tmp49 + tmp50 + tmp51 + tmp35; -double tmp53 = cp(2+_OFF); -double tmp54 = std::pow(tmp37,2); -double tmp55 = tmp48 + tmp54; +double tmp4 = x(30+_OFF); +double tmp5 = x(31+_OFF); +double tmp6 = x(32+_OFF); +double tmp7 = x(33+_OFF); +double tmp8 = tmp5*tmp6; +double tmp9 = std::pow(tmp4,2); +double tmp10 = std::pow(tmp5,2); +double tmp11 = std::pow(tmp6,2); +double tmp12 = std::pow(tmp7,2); +double tmp13 = -tmp12; +double tmp14 = wind(1+_OFF); +double tmp15 = tmp5*tmp7; +double tmp16 = wind(2+_OFF); +double tmp17 = tmp6*tmp7; +double tmp18 = wind(3+_OFF); +double tmp19 = -tmp10; +double tmp20 = -tmp11; +double tmp21 = tmp4*tmp7; +double tmp22 = tmp8 + tmp21; +double tmp23 = -2*tmp16*tmp22; +double tmp24 = -(tmp4*tmp6); +double tmp25 = tmp24 + tmp15; +double tmp26 = -2*tmp18*tmp25; +double tmp27 = tmp9 + tmp10 + tmp20 + tmp13; +double tmp28 = -(tmp14*tmp27); +double tmp29 = x(34+_OFF); +double tmp30 = tmp23 + tmp26 + tmp28 + tmp29; +double tmp31 = tmp4*tmp6; +double tmp32 = tmp31 + tmp15; +double tmp33 = -2*tmp14*tmp32; +double tmp34 = -(tmp4*tmp5); +double tmp35 = tmp34 + tmp17; +double tmp36 = -2*tmp16*tmp35; +double tmp37 = tmp9 + tmp19 + tmp20 + tmp12; +double tmp38 = -(tmp18*tmp37); +double tmp39 = x(36+_OFF); +double tmp40 = tmp33 + tmp36 + tmp38 + tmp39; +double tmp41 = std::atan2(tmp40,tmp30); +double tmp42 = -(tmp4*tmp7); +double tmp43 = tmp8 + tmp42; +double tmp44 = tmp4*tmp5; +double tmp45 = tmp44 + tmp17; +double tmp46 = tmp9 + tmp19 + tmp11 + tmp13; +double tmp47 = -2*tmp14*tmp43; +double tmp48 = -2*tmp18*tmp45; +double tmp49 = -(tmp16*tmp46); +double tmp50 = x(35+_OFF); +double tmp51 = tmp47 + tmp48 + tmp49 + tmp50; +double tmp52 = std::pow(tmp30,2); +double tmp53 = std::pow(tmp51,2); +double tmp54 = std::pow(tmp40,2); +double tmp55 = tmp52 + tmp53 + tmp54; double tmp56 = 1/std::sqrt(tmp55); -double tmp57 = -tmp8; -double tmp58 = tmp57 + tmp32 + tmp10 + tmp12; -double tmp59 = 2*tmp58*tmp24; -double tmp60 = -4*tmp39*tmp47; -double tmp61 = -4*tmp26*tmp52; -double tmp62 = tmp59 + tmp60 + tmp61; -double tmp63 = std::pow(tmp47,2); -double tmp64 = std::pow(tmp52,2); -double tmp65 = tmp48 + tmp63 + tmp64; -double tmp66 = 1/std::sqrt(tmp65); -double tmp67 = tmp47*tmp66; -double tmp68 = std::asin(tmp67); -double tmp69 = fLat(1+_OFF); -double tmp70 = -(tmp4*tmp8); -double tmp71 = -(tmp4*tmp9); -double tmp72 = 2*tmp7*tmp0*tmp1; -double tmp73 = -2*tmp6*tmp2*tmp1; -double tmp74 = tmp4*tmp10; -double tmp75 = -2*tmp6*tmp0*tmp3; -double tmp76 = -2*tmp7*tmp2*tmp3; -double tmp77 = tmp4*tmp12; -double tmp78 = tmp70 + tmp71 + tmp72 + tmp73 + tmp74 + tmp75 + tmp76 + tmp77 + tmp23; -double tmp79 = tmp6*tmp8; -double tmp80 = 2*tmp7*tmp0*tmp2; -double tmp81 = -(tmp6*tmp9); -double tmp82 = 2*tmp4*tmp2*tmp1; -double tmp83 = tmp6*tmp10; -double tmp84 = -2*tmp4*tmp0*tmp3; -double tmp85 = 2*tmp7*tmp1*tmp3; -double tmp86 = -(tmp6*tmp12); -double tmp87 = -tmp46; -double tmp88 = tmp79 + tmp80 + tmp81 + tmp82 + tmp83 + tmp84 + tmp85 + tmp86 + tmp87; -double tmp89 = std::sqrt(tmp65); -double tmp90 = fLift(1+_OFF); -double tmp91 = std::atan2(tmp37,tmp24); -double tmp92 = fLift(2+_OFF); -double tmp93 = tmp91*tmp92; -double tmp94 = tmp90 + tmp93; -double tmp95 = 4*tmp26*tmp37; -double tmp96 = tmp59 + tmp95; -double tmp97 = std::pow(tmp55,-1.5); -double tmp98 = tmp7*tmp8; -double tmp99 = -2*tmp6*tmp0*tmp2; -double tmp100 = -(tmp7*tmp9); -double tmp101 = 2*tmp4*tmp0*tmp1; -double tmp102 = -(tmp7*tmp10); -double tmp103 = 2*tmp4*tmp2*tmp3; -double tmp104 = 2*tmp6*tmp1*tmp3; -double tmp105 = tmp7*tmp12; -double tmp106 = tmp98 + tmp99 + tmp100 + tmp101 + tmp102 + tmp103 + tmp104 + tmp105 + tmp36; -double tmp107 = 1/tmp55; -double tmp108 = 1/tmp65; -double tmp109 = fDrag(1+_OFF); -double tmp110 = fDrag(2+_OFF); -double tmp111 = tmp91*tmp110; -double tmp112 = std::pow(tmp91,2); -double tmp113 = fDrag(3+_OFF); -double tmp114 = tmp112*tmp113; -double tmp115 = std::pow(tmp68,2); -double tmp116 = fDrag(4+_OFF); -double tmp117 = tmp115*tmp116; -double tmp118 = tmp109 + tmp111 + tmp114 + tmp117; -double tmp119 = -(tmp63*tmp108); -double tmp120 = 1 + tmp119; -double tmp121 = std::sqrt(tmp120); -double tmp122 = 1/std::sqrt(tmp120); -double tmp123 = 2*tmp26*tmp24*tmp107; -double tmp124 = tmp58*tmp107*tmp52; -double tmp125 = tmp123 + tmp124; -double tmp126 = std::pow(tmp65,-1.5); -double tmp127 = -(tmp47*tmp62*tmp126)/2.; -double tmp128 = -2*tmp39*tmp66; -double tmp129 = tmp127 + tmp128; -double tmp130 = cp(1+_OFF); -double tmp131 = cp(4+_OFF); -double tmp132 = 1/tmp131; -double tmp133 = -4*tmp15*tmp24; -double tmp134 = tmp57 + tmp9 + tmp11 + tmp12; -double tmp135 = 2*tmp134*tmp47; -double tmp136 = -4*tmp30*tmp52; -double tmp137 = tmp133 + tmp135 + tmp136; -double tmp138 = -2*tmp0*tmp3; -double tmp139 = 4*tmp30*tmp37; -double tmp140 = tmp133 + tmp139; -double tmp141 = std::pow(tmp65,-2); -double tmp142 = -2*tmp2*tmp1; -double tmp143 = tmp142 + tmp138; -double tmp144 = 2*tmp30*tmp24*tmp107; -double tmp145 = -2*tmp15*tmp107*tmp52; -double tmp146 = tmp144 + tmp145; -double tmp147 = -(tmp47*tmp137*tmp126)/2.; -double tmp148 = tmp134*tmp66; -double tmp149 = tmp147 + tmp148; -double tmp150 = std::pow(tmp130,2); -double tmp151 = std::pow(M_PI,-2); -double tmp152 = fThrust(3+_OFF); -double tmp153 = 1/M_PI; -double tmp154 = fThrust(2+_OFF); -double tmp155 = z(1+_OFF); -double tmp156 = -4*tmp18*tmp24; -double tmp157 = -4*tmp42*tmp47; -double tmp158 = tmp57 + tmp9 + tmp10 + tmp20; -double tmp159 = 2*tmp158*tmp52; -double tmp160 = tmp156 + tmp157 + tmp159; -double tmp161 = 2*tmp1*tmp3; -double tmp162 = 2*tmp0*tmp1; -double tmp163 = 2*tmp33*tmp37; -double tmp164 = tmp156 + tmp163; -double tmp165 = -2*tmp2*tmp3; -double tmp166 = tmp162 + tmp165; -double tmp167 = tmp33*tmp24*tmp107; -double tmp168 = -2*tmp18*tmp107*tmp52; -double tmp169 = tmp167 + tmp168; -double tmp170 = -(tmp47*tmp160*tmp126)/2.; -double tmp171 = -2*tmp42*tmp66; -double tmp172 = tmp170 + tmp171; -double tmp173 = tmp63*tmp62*tmp141; -double tmp174 = 4*tmp39*tmp47*tmp108; -double tmp175 = tmp173 + tmp174; -double tmp176 = -(tmp6*tmp8); -double tmp177 = -2*tmp7*tmp0*tmp2; -double tmp178 = tmp6*tmp9; -double tmp179 = -2*tmp4*tmp2*tmp1; -double tmp180 = -(tmp6*tmp10); -double tmp181 = 2*tmp4*tmp0*tmp3; -double tmp182 = -2*tmp7*tmp1*tmp3; -double tmp183 = tmp6*tmp12; -double tmp184 = tmp176 + tmp177 + tmp178 + tmp179 + tmp180 + tmp181 + tmp182 + tmp183 + tmp46; -double tmp185 = tmp110*tmp125; -double tmp186 = 2*tmp91*tmp113*tmp125; -double tmp187 = 2*tmp68*tmp116*tmp122*tmp129; -double tmp188 = tmp185 + tmp186 + tmp187; -double tmp189 = tmp63*tmp137*tmp141; -double tmp190 = -2*tmp134*tmp47*tmp108; -double tmp191 = tmp189 + tmp190; -double tmp192 = tmp110*tmp146; -double tmp193 = 2*tmp91*tmp113*tmp146; -double tmp194 = 2*tmp68*tmp116*tmp122*tmp149; -double tmp195 = tmp192 + tmp193 + tmp194; -double tmp196 = -2*tmp0*tmp2; -double tmp197 = tmp63*tmp160*tmp141; -double tmp198 = 4*tmp42*tmp47*tmp108; -double tmp199 = tmp197 + tmp198; -double tmp200 = tmp110*tmp169; -double tmp201 = 2*tmp91*tmp113*tmp169; -double tmp202 = 2*tmp68*tmp116*tmp122*tmp172; -double tmp203 = tmp200 + tmp201 + tmp202; -double tmp204 = -(tmp7*tmp8); -double tmp205 = 2*tmp6*tmp0*tmp2; -double tmp206 = tmp7*tmp9; -double tmp207 = -2*tmp4*tmp0*tmp1; -double tmp208 = tmp7*tmp10; -double tmp209 = -2*tmp4*tmp2*tmp3; -double tmp210 = -2*tmp6*tmp1*tmp3; -double tmp211 = -(tmp7*tmp12); -double tmp212 = tmp204 + tmp205 + tmp206 + tmp207 + tmp208 + tmp209 + tmp210 + tmp211 + tmp35; -double tmp213 = 2*tmp2*tmp1; -double tmp214 = tmp213 + tmp138; -double tmp215 = -2*tmp0*tmp1; -double tmp216 = tmp215 + tmp165; -double tmp217 = 2*tmp0*tmp2; -double tmp218 = -2*tmp1*tmp3; -double tmp219 = tmp217 + tmp218; -double tmp220 = tmp217 + tmp161; -double tmp221 = ibd(2+_OFF); -double tmp222 = ibd(3+_OFF); -double tmp223 = ibod(1+_OFF); -double tmp224 = ibod(2+_OFF); -double tmp225 = ibod(3+_OFF); -double tmp226 = ibd(1+_OFF); -double tmp227 = std::pow(tmp225,2); -double tmp228 = cp(3+_OFF); -double tmp229 = mRoll(2+_OFF); -double tmp230 = mRoll(3+_OFF); -double tmp231 = x(11+_OFF); -double tmp232 = mRoll(4+_OFF); -double tmp233 = x(13+_OFF); -double tmp234 = tmp226*tmp221*tmp222; -double tmp235 = std::pow(tmp223,2); -double tmp236 = -(tmp221*tmp235); -double tmp237 = std::pow(tmp224,2); -double tmp238 = -(tmp222*tmp237); -double tmp239 = 2*tmp223*tmp224*tmp225; -double tmp240 = -(tmp226*tmp227); -double tmp241 = tmp234 + tmp236 + tmp238 + tmp239 + tmp240; -double tmp242 = 1/tmp241; -double tmp243 = cBar(1+_OFF); -double tmp244 = mPitch(3+_OFF); -double tmp245 = mPitch(4+_OFF); -double tmp246 = x(12+_OFF); -double tmp247 = mYaw(2+_OFF); -double tmp248 = mYaw(3+_OFF); -double tmp249 = tmp221*tmp222; -double tmp250 = -tmp227; -double tmp251 = tmp249 + tmp250; -double tmp252 = -(tmp68*tmp229); -double tmp253 = (tmp228*tmp230*tmp66*tmp231)/2.; -double tmp254 = -(tmp228*tmp232*tmp66*tmp233)/2.; -double tmp255 = mRoll(1+_OFF); -double tmp256 = z(2+_OFF); -double tmp257 = -(tmp255*tmp256); -double tmp258 = tmp252 + tmp253 + tmp254 + tmp257; -double tmp259 = -(tmp222*tmp224); -double tmp260 = tmp223*tmp225; -double tmp261 = tmp259 + tmp260; -double tmp262 = mPitch(1+_OFF); -double tmp263 = -tmp262; -double tmp264 = -(tmp91*tmp244); -double tmp265 = (tmp243*tmp245*tmp66*tmp246)/2.; -double tmp266 = mPitch(2+_OFF); -double tmp267 = z(3+_OFF); -double tmp268 = -(tmp266*tmp267); -double tmp269 = tmp263 + tmp264 + tmp265 + tmp268; -double tmp270 = -(tmp221*tmp223); -double tmp271 = tmp224*tmp225; -double tmp272 = tmp270 + tmp271; -double tmp273 = tmp68*tmp247; -double tmp274 = (tmp228*tmp248*tmp66*tmp233)/2.; -double tmp275 = mYaw(1+_OFF); -double tmp276 = z(4+_OFF); -double tmp277 = tmp275*tmp276; -double tmp278 = tmp273 + tmp274 + tmp277; -double tmp279 = -(tmp229*tmp122*tmp129); -double tmp280 = -(tmp228*tmp230*tmp62*tmp126*tmp231)/4.; -double tmp281 = (tmp228*tmp232*tmp62*tmp126*tmp233)/4.; -double tmp282 = tmp279 + tmp280 + tmp281; -double tmp283 = 0.5*tmp53*tmp228*tmp65*tmp282; -double tmp284 = 0.5*tmp53*tmp228*tmp62*tmp258; -double tmp285 = tmp283 + tmp284; -double tmp286 = -(tmp244*tmp125); -double tmp287 = -(tmp243*tmp245*tmp62*tmp126*tmp246)/4.; -double tmp288 = tmp286 + tmp287; -double tmp289 = 0.5*tmp243*tmp53*tmp65*tmp288; -double tmp290 = 0.5*tmp243*tmp53*tmp62*tmp269; -double tmp291 = tmp289 + tmp290; -double tmp292 = tmp247*tmp122*tmp129; -double tmp293 = -(tmp228*tmp248*tmp62*tmp126*tmp233)/4.; -double tmp294 = tmp292 + tmp293; -double tmp295 = 0.5*tmp53*tmp228*tmp65*tmp294; -double tmp296 = 0.5*tmp53*tmp228*tmp62*tmp278; -double tmp297 = tmp295 + tmp296; -double tmp298 = -(tmp229*tmp122*tmp149); -double tmp299 = -(tmp228*tmp230*tmp137*tmp126*tmp231)/4.; -double tmp300 = (tmp228*tmp232*tmp137*tmp126*tmp233)/4.; -double tmp301 = tmp298 + tmp299 + tmp300; -double tmp302 = 0.5*tmp53*tmp228*tmp65*tmp301; -double tmp303 = 0.5*tmp53*tmp228*tmp137*tmp258; -double tmp304 = tmp302 + tmp303; -double tmp305 = tmp226*tmp222; -double tmp306 = -tmp235; -double tmp307 = tmp305 + tmp306; -double tmp308 = -(tmp244*tmp146); -double tmp309 = -(tmp243*tmp245*tmp137*tmp126*tmp246)/4.; -double tmp310 = tmp308 + tmp309; -double tmp311 = 0.5*tmp243*tmp53*tmp65*tmp310; -double tmp312 = 0.5*tmp243*tmp53*tmp137*tmp269; -double tmp313 = tmp311 + tmp312; -double tmp314 = tmp223*tmp224; -double tmp315 = -(tmp226*tmp225); -double tmp316 = tmp314 + tmp315; -double tmp317 = tmp247*tmp122*tmp149; -double tmp318 = -(tmp228*tmp248*tmp137*tmp126*tmp233)/4.; -double tmp319 = tmp317 + tmp318; -double tmp320 = 0.5*tmp53*tmp228*tmp65*tmp319; -double tmp321 = 0.5*tmp53*tmp228*tmp137*tmp278; -double tmp322 = tmp320 + tmp321; -double tmp323 = -(tmp229*tmp122*tmp172); -double tmp324 = -(tmp228*tmp230*tmp160*tmp126*tmp231)/4.; -double tmp325 = (tmp228*tmp232*tmp160*tmp126*tmp233)/4.; -double tmp326 = tmp323 + tmp324 + tmp325; -double tmp327 = 0.5*tmp53*tmp228*tmp65*tmp326; -double tmp328 = 0.5*tmp53*tmp228*tmp160*tmp258; -double tmp329 = tmp327 + tmp328; -double tmp330 = -(tmp244*tmp169); -double tmp331 = -(tmp243*tmp245*tmp160*tmp126*tmp246)/4.; -double tmp332 = tmp330 + tmp331; -double tmp333 = 0.5*tmp243*tmp53*tmp65*tmp332; -double tmp334 = 0.5*tmp243*tmp53*tmp160*tmp269; -double tmp335 = tmp333 + tmp334; -double tmp336 = tmp247*tmp122*tmp172; -double tmp337 = -(tmp228*tmp248*tmp160*tmp126*tmp233)/4.; -double tmp338 = tmp336 + tmp337; -double tmp339 = 0.5*tmp53*tmp228*tmp65*tmp338; -double tmp340 = 0.5*tmp53*tmp228*tmp160*tmp278; -double tmp341 = tmp339 + tmp340; -double tmp342 = tmp226*tmp221; -double tmp343 = -tmp237; -double tmp344 = tmp342 + tmp343; -J(1+_OFF,1+_OFF) = -(tmp132*(-0.5*tmp53*tmp94*tmp56*tmp106*tmp62 + 0.25*tmp68*tmp53*tmp69*tmp78*tmp88*tmp56*tmp62*tmp66 - 0.25*tmp68*tmp53*tmp69*tmp78*tmp88*tmp96*tmp97*tmp89 + 0.5*tmp68*tmp53*tmp69*tmp214*tmp78*tmp56*tmp89 + 0.5*tmp68*tmp53*tmp69*tmp58*tmp88*tmp56*tmp89 - 0.5*tmp53*tmp94*(tmp162 + 2*tmp2*tmp3)*tmp56*tmp65 + 0.25*tmp53*tmp94*tmp96*tmp97*tmp106*tmp65 - 0.5*tmp53*tmp92*tmp56*tmp106*tmp125*tmp65 + 0.25*tmp53*tmp118*tmp78*tmp56*tmp65*tmp175*tmp122 + 0.5*tmp53*tmp118*tmp78*tmp56*tmp62*tmp121 - 0.25*tmp53*tmp118*tmp78*tmp96*tmp97*tmp65*tmp121 + 0.5*tmp53*tmp118*tmp58*tmp56*tmp65*tmp121 + 0.5*tmp53*tmp69*tmp78*tmp88*tmp56*tmp89*tmp122*tmp129 + 0.5*tmp53*tmp78*tmp56*tmp65*tmp121*tmp188 + 1.*tmp150*(tmp151*tmp152*tmp62 + (tmp153*tmp130*tmp154*tmp62*tmp66*tmp155)/2.))); -J(1+_OFF,2+_OFF) = -(tmp132*(-0.5*tmp53*tmp94*tmp56*tmp106*tmp137 + 0.25*tmp68*tmp53*tmp69*tmp78*tmp88*tmp56*tmp137*tmp66 - 0.25*tmp68*tmp53*tmp69*tmp78*tmp88*tmp140*tmp97*tmp89 + 0.5*tmp68*tmp53*tmp69*tmp44*tmp78*tmp56*tmp89 + 0.5*tmp68*tmp53*tmp69*tmp143*tmp88*tmp56*tmp89 - 0.5*tmp53*tmp94*(tmp196 + tmp161)*tmp56*tmp65 + 0.25*tmp53*tmp94*tmp140*tmp97*tmp106*tmp65 - 0.5*tmp53*tmp92*tmp56*tmp106*tmp146*tmp65 + 0.25*tmp53*tmp118*tmp78*tmp56*tmp65*tmp191*tmp122 + 0.5*tmp53*tmp118*tmp78*tmp56*tmp137*tmp121 - 0.25*tmp53*tmp118*tmp78*tmp140*tmp97*tmp65*tmp121 + 0.5*tmp53*tmp118*tmp143*tmp56*tmp65*tmp121 + 0.5*tmp53*tmp69*tmp78*tmp88*tmp56*tmp89*tmp122*tmp149 + 0.5*tmp53*tmp78*tmp56*tmp65*tmp121*tmp195 + 1.*tmp150*(tmp151*tmp152*tmp137 + (tmp153*tmp130*tmp154*tmp137*tmp66*tmp155)/2.))); -J(1+_OFF,3+_OFF) = -(tmp132*(-0.5*tmp53*tmp94*tmp56*tmp106*tmp160 + 0.25*tmp68*tmp53*tmp69*tmp78*tmp88*tmp56*tmp160*tmp66 - 0.25*tmp68*tmp53*tmp69*tmp78*tmp88*tmp164*tmp97*tmp89 + 0.5*tmp68*tmp53*tmp69*tmp220*tmp78*tmp56*tmp89 + 0.5*tmp68*tmp53*tmp69*tmp166*tmp88*tmp56*tmp89 - 0.5*tmp53*tmp94*tmp33*tmp56*tmp65 + 0.25*tmp53*tmp94*tmp164*tmp97*tmp106*tmp65 - 0.5*tmp53*tmp92*tmp56*tmp106*tmp169*tmp65 + 0.25*tmp53*tmp118*tmp78*tmp56*tmp65*tmp199*tmp122 + 0.5*tmp53*tmp118*tmp78*tmp56*tmp160*tmp121 - 0.25*tmp53*tmp118*tmp78*tmp164*tmp97*tmp65*tmp121 + 0.5*tmp53*tmp118*tmp166*tmp56*tmp65*tmp121 + 0.5*tmp53*tmp69*tmp78*tmp88*tmp56*tmp89*tmp122*tmp172 + 0.5*tmp53*tmp78*tmp56*tmp65*tmp121*tmp203 + 1.*tmp150*(tmp151*tmp152*tmp160 + (tmp153*tmp130*tmp154*tmp160*tmp66*tmp155)/2.))); -J(2+_OFF,1+_OFF) = -(tmp132*(0.25*tmp53*tmp118*tmp184*tmp62*tmp66 + 0.5*tmp53*tmp118*(tmp142 + 2*tmp0*tmp3)*tmp89 + 0.25*tmp68*tmp53*tmp69*tmp65*tmp175*tmp122 + 0.5*tmp68*tmp53*tmp69*tmp62*tmp121 + 0.5*tmp53*tmp69*tmp65*tmp129 + 0.5*tmp53*tmp184*tmp89*tmp188)); -J(2+_OFF,2+_OFF) = -(tmp132*(0.25*tmp53*tmp118*tmp184*tmp137*tmp66 + 0.5*tmp53*tmp118*tmp134*tmp89 + 0.25*tmp68*tmp53*tmp69*tmp65*tmp191*tmp122 + 0.5*tmp68*tmp53*tmp69*tmp137*tmp121 + 0.5*tmp53*tmp69*tmp65*tmp149 + 0.5*tmp53*tmp184*tmp89*tmp195)); -J(2+_OFF,3+_OFF) = -(tmp132*(0.25*tmp53*tmp118*tmp184*tmp160*tmp66 + 0.5*tmp53*tmp118*(tmp196 + tmp218)*tmp89 + 0.25*tmp68*tmp53*tmp69*tmp65*tmp199*tmp122 + 0.5*tmp68*tmp53*tmp69*tmp160*tmp121 + 0.5*tmp53*tmp69*tmp65*tmp172 + 0.5*tmp53*tmp184*tmp89*tmp203)); -J(3+_OFF,1+_OFF) = -(tmp132*(-0.5*tmp53*tmp94*tmp78*tmp56*tmp62 + 0.25*tmp68*tmp53*tmp69*tmp88*tmp56*tmp212*tmp62*tmp66 + 0.5*tmp68*tmp53*tmp69*tmp216*tmp88*tmp56*tmp89 - 0.25*tmp68*tmp53*tmp69*tmp88*tmp96*tmp97*tmp212*tmp89 + 0.5*tmp68*tmp53*tmp69*tmp214*tmp56*tmp212*tmp89 + 0.25*tmp53*tmp94*tmp78*tmp96*tmp97*tmp65 - 0.5*tmp53*tmp94*tmp58*tmp56*tmp65 - 0.5*tmp53*tmp92*tmp78*tmp56*tmp125*tmp65 + 0.25*tmp53*tmp118*tmp56*tmp212*tmp65*tmp175*tmp122 + 0.5*tmp53*tmp118*tmp56*tmp212*tmp62*tmp121 + 0.5*tmp53*tmp118*tmp216*tmp56*tmp65*tmp121 - 0.25*tmp53*tmp118*tmp96*tmp97*tmp212*tmp65*tmp121 + 0.5*tmp53*tmp69*tmp88*tmp56*tmp212*tmp89*tmp122*tmp129 + 0.5*tmp53*tmp56*tmp212*tmp65*tmp121*tmp188)); -J(3+_OFF,2+_OFF) = -(tmp132*(-0.5*tmp53*tmp94*tmp78*tmp56*tmp137 + 0.25*tmp68*tmp53*tmp69*tmp88*tmp56*tmp212*tmp137*tmp66 + 0.5*tmp68*tmp53*tmp69*tmp219*tmp88*tmp56*tmp89 - 0.25*tmp68*tmp53*tmp69*tmp88*tmp140*tmp97*tmp212*tmp89 + 0.5*tmp68*tmp53*tmp69*tmp44*tmp56*tmp212*tmp89 + 0.25*tmp53*tmp94*tmp78*tmp140*tmp97*tmp65 - 0.5*tmp53*tmp94*tmp143*tmp56*tmp65 - 0.5*tmp53*tmp92*tmp78*tmp56*tmp146*tmp65 + 0.25*tmp53*tmp118*tmp56*tmp212*tmp65*tmp191*tmp122 + 0.5*tmp53*tmp118*tmp56*tmp212*tmp137*tmp121 + 0.5*tmp53*tmp118*tmp219*tmp56*tmp65*tmp121 - 0.25*tmp53*tmp118*tmp140*tmp97*tmp212*tmp65*tmp121 + 0.5*tmp53*tmp69*tmp88*tmp56*tmp212*tmp89*tmp122*tmp149 + 0.5*tmp53*tmp56*tmp212*tmp65*tmp121*tmp195)); -J(3+_OFF,3+_OFF) = -(tmp132*(-0.5*tmp53*tmp94*tmp78*tmp56*tmp160 + 0.25*tmp68*tmp53*tmp69*tmp88*tmp56*tmp212*tmp160*tmp66 + 0.5*tmp68*tmp53*tmp69*tmp158*tmp88*tmp56*tmp89 - 0.25*tmp68*tmp53*tmp69*tmp88*tmp164*tmp97*tmp212*tmp89 + 0.5*tmp68*tmp53*tmp69*tmp220*tmp56*tmp212*tmp89 + 0.25*tmp53*tmp94*tmp78*tmp164*tmp97*tmp65 - 0.5*tmp53*tmp94*tmp166*tmp56*tmp65 - 0.5*tmp53*tmp92*tmp78*tmp56*tmp169*tmp65 + 0.25*tmp53*tmp118*tmp56*tmp212*tmp65*tmp199*tmp122 + 0.5*tmp53*tmp118*tmp56*tmp212*tmp160*tmp121 + 0.5*tmp53*tmp118*tmp158*tmp56*tmp65*tmp121 - 0.25*tmp53*tmp118*tmp164*tmp97*tmp212*tmp65*tmp121 + 0.5*tmp53*tmp69*tmp88*tmp56*tmp212*tmp89*tmp122*tmp172 + 0.5*tmp53*tmp56*tmp212*tmp65*tmp121*tmp203)); -J(4+_OFF,1+_OFF) = -(tmp251*tmp242*tmp285) - tmp261*tmp242*tmp291 - tmp272*tmp242*tmp297; -J(4+_OFF,2+_OFF) = -(tmp251*tmp242*tmp304) - tmp261*tmp242*tmp313 - tmp272*tmp242*tmp322; -J(4+_OFF,3+_OFF) = -(tmp251*tmp242*tmp329) - tmp261*tmp242*tmp335 - tmp272*tmp242*tmp341; -J(5+_OFF,1+_OFF) = -(tmp261*tmp242*tmp285) - tmp307*tmp242*tmp291 - tmp316*tmp242*tmp297; -J(5+_OFF,2+_OFF) = -(tmp261*tmp242*tmp304) - tmp307*tmp242*tmp313 - tmp316*tmp242*tmp322; -J(5+_OFF,3+_OFF) = -(tmp261*tmp242*tmp329) - tmp307*tmp242*tmp335 - tmp316*tmp242*tmp341; -J(6+_OFF,1+_OFF) = -(tmp272*tmp242*tmp285) - tmp316*tmp242*tmp291 - tmp344*tmp242*tmp297; -J(6+_OFF,2+_OFF) = -(tmp272*tmp242*tmp304) - tmp316*tmp242*tmp313 - tmp344*tmp242*tmp322; -J(6+_OFF,3+_OFF) = -(tmp272*tmp242*tmp329) - tmp316*tmp242*tmp335 - tmp344*tmp242*tmp341; +double tmp57 = cp(2+_OFF); +double tmp58 = fDrag(1+_OFF); +double tmp59 = fDrag(2+_OFF); +double tmp60 = -(tmp41*tmp59); +double tmp61 = std::pow(tmp41,2); +double tmp62 = fDrag(3+_OFF); +double tmp63 = tmp61*tmp62; +double tmp64 = tmp51*tmp56; +double tmp65 = std::asin(tmp64); +double tmp66 = std::pow(tmp65,2); +double tmp67 = fDrag(4+_OFF); +double tmp68 = tmp66*tmp67; +double tmp69 = tmp58 + tmp60 + tmp63 + tmp68; +double tmp70 = -tmp9; +double tmp71 = tmp70 + tmp19 + tmp11 + tmp12; +double tmp72 = 2*tmp71*tmp30; +double tmp73 = -4*tmp43*tmp51; +double tmp74 = -4*tmp32*tmp40; +double tmp75 = tmp72 + tmp73 + tmp74; +double tmp76 = 1/tmp55; +double tmp77 = fLat(1+_OFF); +double tmp78 = -(tmp53*tmp76); +double tmp79 = 1 + tmp78; +double tmp80 = std::sqrt(tmp55); +double tmp81 = tmp52 + tmp54; +double tmp82 = 1/tmp81; +double tmp83 = -2*tmp32*tmp30*tmp82; +double tmp84 = 2*tmp14*tmp32; +double tmp85 = 2*tmp16*tmp35; +double tmp86 = tmp18*tmp37; +double tmp87 = -tmp39; +double tmp88 = tmp84 + tmp85 + tmp86 + tmp87; +double tmp89 = tmp71*tmp88*tmp82; +double tmp90 = tmp83 + tmp89; +double tmp91 = 1/std::sqrt(tmp79); +double tmp92 = std::pow(tmp55,-1.5); +double tmp93 = -0.5*(tmp51*tmp75*tmp92); +double tmp94 = -2*tmp43*tmp56; +double tmp95 = tmp93 + tmp94; +double tmp96 = std::pow(tmp0,2); +double tmp97 = std::pow(tmp2,2); +double tmp98 = std::pow(tmp1,2); +double tmp99 = -tmp98; +double tmp100 = std::pow(tmp3,2); +double tmp101 = -tmp100; +double tmp102 = tmp96 + tmp97 + tmp99 + tmp101; +double tmp103 = tmp2*tmp1; +double tmp104 = tmp0*tmp3; +double tmp105 = tmp103 + tmp104; +double tmp106 = -(tmp0*tmp1); +double tmp107 = tmp2*tmp3; +double tmp108 = tmp106 + tmp107; +double tmp109 = 1/std::sqrt(tmp81); +double tmp110 = fLift(1+_OFF); +double tmp111 = -tmp110; +double tmp112 = fLift(2+_OFF); +double tmp113 = tmp41*tmp112; +double tmp114 = tmp111 + tmp113; +double tmp115 = tmp72 + tmp74; +double tmp116 = std::pow(tmp81,-1.5); +double tmp117 = std::pow(tmp55,-2); +double tmp118 = tmp53*tmp75*tmp117; +double tmp119 = 4*tmp43*tmp51*tmp76; +double tmp120 = tmp118 + tmp119; +double tmp121 = std::sqrt(tmp79); +double tmp122 = -(tmp59*tmp90); +double tmp123 = 2*tmp41*tmp62*tmp90; +double tmp124 = 2*tmp65*tmp67*tmp91*tmp95; +double tmp125 = tmp122 + tmp123 + tmp124; +double tmp126 = cp(1+_OFF); +double tmp127 = cp(4+_OFF); +double tmp128 = 1/tmp127; +double tmp129 = 2*tmp102*tmp43; +double tmp130 = 4*tmp108*tmp45; +double tmp131 = 2*tmp105*tmp46; +double tmp132 = tmp129 + tmp130 + tmp131; +double tmp133 = tmp70 + tmp10 + tmp20 + tmp12; +double tmp134 = -4*tmp22*tmp30; +double tmp135 = 2*tmp133*tmp51; +double tmp136 = -4*tmp35*tmp40; +double tmp137 = tmp134 + tmp135 + tmp136; +double tmp138 = -2*tmp35*tmp30*tmp82; +double tmp139 = -2*tmp22*tmp88*tmp82; +double tmp140 = tmp138 + tmp139; +double tmp141 = -0.5*(tmp51*tmp137*tmp92); +double tmp142 = tmp133*tmp56; +double tmp143 = tmp141 + tmp142; +double tmp144 = 2*tmp102*tmp32; +double tmp145 = 4*tmp105*tmp35; +double tmp146 = 2*tmp108*tmp37; +double tmp147 = tmp144 + tmp145 + tmp146; +double tmp148 = tmp134 + tmp136; +double tmp149 = tmp53*tmp137*tmp117; +double tmp150 = -2*tmp133*tmp51*tmp76; +double tmp151 = tmp149 + tmp150; +double tmp152 = -(tmp59*tmp140); +double tmp153 = 2*tmp41*tmp62*tmp140; +double tmp154 = 2*tmp65*tmp67*tmp91*tmp143; +double tmp155 = tmp152 + tmp153 + tmp154; +double tmp156 = 4*tmp105*tmp22; +double tmp157 = 4*tmp108*tmp25; +double tmp158 = tmp102*tmp27; +double tmp159 = tmp156 + tmp157 + tmp158; +double tmp160 = std::pow(tmp126,2); +double tmp161 = std::pow(M_PI,-2); +double tmp162 = fThrust(3+_OFF); +double tmp163 = 1/M_PI; +double tmp164 = fThrust(2+_OFF); +double tmp165 = z(1+_OFF); +double tmp166 = -4*tmp25*tmp30; +double tmp167 = -4*tmp45*tmp51; +double tmp168 = tmp70 + tmp10 + tmp11 + tmp13; +double tmp169 = 2*tmp168*tmp40; +double tmp170 = tmp166 + tmp167 + tmp169; +double tmp171 = tmp168*tmp30*tmp82; +double tmp172 = -2*tmp25*tmp88*tmp82; +double tmp173 = tmp171 + tmp172; +double tmp174 = -0.5*(tmp51*tmp170*tmp92); +double tmp175 = -2*tmp45*tmp56; +double tmp176 = tmp174 + tmp175; +double tmp177 = tmp166 + tmp169; +double tmp178 = tmp53*tmp170*tmp117; +double tmp179 = 4*tmp45*tmp51*tmp76; +double tmp180 = tmp178 + tmp179; +double tmp181 = -(tmp59*tmp173); +double tmp182 = 2*tmp41*tmp62*tmp173; +double tmp183 = 2*tmp65*tmp67*tmp91*tmp176; +double tmp184 = tmp181 + tmp182 + tmp183; +double tmp185 = 0.317*tmp57*tmp69*tmp51*tmp75*tmp56; +double tmp186 = -1.268*tmp57*tmp69*tmp43*tmp80; +double tmp187 = 0.317*tmp65*tmp57*tmp77*tmp55*tmp120*tmp91; +double tmp188 = 0.634*tmp65*tmp57*tmp77*tmp75*tmp121; +double tmp189 = 0.634*tmp57*tmp77*tmp55*tmp95; +double tmp190 = 0.634*tmp57*tmp51*tmp80*tmp125; +double tmp191 = tmp185 + tmp186 + tmp187 + tmp188 + tmp189 + tmp190; +double tmp192 = -(tmp0*tmp3); +double tmp193 = tmp103 + tmp192; +double tmp194 = -tmp97; +double tmp195 = tmp96 + tmp194 + tmp98 + tmp101; +double tmp196 = tmp0*tmp2; +double tmp197 = tmp1*tmp3; +double tmp198 = tmp196 + tmp197; +double tmp199 = 0.634*tmp57*tmp114*tmp30*tmp75*tmp109; +double tmp200 = -0.317*tmp65*tmp57*tmp77*tmp51*tmp40*tmp75*tmp109*tmp56; +double tmp201 = 0.317*tmp65*tmp57*tmp77*tmp51*tmp40*tmp115*tmp116*tmp80; +double tmp202 = 1.268*tmp65*tmp57*tmp77*tmp32*tmp51*tmp109*tmp80; +double tmp203 = 1.268*tmp65*tmp57*tmp77*tmp43*tmp40*tmp109*tmp80; +double tmp204 = -0.317*tmp57*tmp114*tmp30*tmp115*tmp116*tmp55; +double tmp205 = 0.634*tmp57*tmp114*tmp71*tmp109*tmp55; +double tmp206 = 0.634*tmp57*tmp112*tmp30*tmp109*tmp55*tmp90; +double tmp207 = 0.317*tmp57*tmp69*tmp40*tmp109*tmp55*tmp120*tmp91; +double tmp208 = 0.634*tmp57*tmp69*tmp40*tmp75*tmp109*tmp121; +double tmp209 = -0.317*tmp57*tmp69*tmp40*tmp115*tmp116*tmp55*tmp121; +double tmp210 = -1.268*tmp57*tmp69*tmp32*tmp109*tmp55*tmp121; +double tmp211 = -0.634*tmp57*tmp77*tmp51*tmp40*tmp109*tmp80*tmp91*tmp95; +double tmp212 = 0.634*tmp57*tmp40*tmp109*tmp55*tmp121*tmp125; +double tmp213 = tmp199 + tmp200 + tmp201 + tmp202 + tmp203 + tmp204 + tmp205 + tmp206 + tmp207 + tmp208 + tmp209 + tmp210 + tmp211 + tmp212; +double tmp214 = -0.634*tmp57*tmp114*tmp40*tmp75*tmp109; +double tmp215 = -0.317*tmp65*tmp57*tmp77*tmp30*tmp51*tmp75*tmp109*tmp56; +double tmp216 = 0.317*tmp65*tmp57*tmp77*tmp30*tmp51*tmp115*tmp116*tmp80; +double tmp217 = 1.268*tmp65*tmp57*tmp77*tmp43*tmp30*tmp109*tmp80; +double tmp218 = -0.634*tmp65*tmp57*tmp77*tmp71*tmp51*tmp109*tmp80; +double tmp219 = 0.317*tmp57*tmp114*tmp40*tmp115*tmp116*tmp55; +double tmp220 = 1.268*tmp57*tmp114*tmp32*tmp109*tmp55; +double tmp221 = -0.634*tmp57*tmp112*tmp40*tmp109*tmp55*tmp90; +double tmp222 = 0.317*tmp57*tmp69*tmp30*tmp109*tmp55*tmp120*tmp91; +double tmp223 = 0.634*tmp57*tmp69*tmp30*tmp75*tmp109*tmp121; +double tmp224 = -0.317*tmp57*tmp69*tmp30*tmp115*tmp116*tmp55*tmp121; +double tmp225 = 0.634*tmp57*tmp69*tmp71*tmp109*tmp55*tmp121; +double tmp226 = -0.634*tmp57*tmp77*tmp30*tmp51*tmp109*tmp80*tmp91*tmp95; +double tmp227 = 0.634*tmp57*tmp30*tmp109*tmp55*tmp121*tmp125; +double tmp228 = tmp161*tmp162*tmp75; +double tmp229 = (tmp163*tmp126*tmp164*tmp75*tmp56*tmp165)/2.; +double tmp230 = tmp228 + tmp229; +double tmp231 = 1.268*tmp160*tmp230; +double tmp232 = tmp214 + tmp215 + tmp216 + tmp217 + tmp218 + tmp219 + tmp220 + tmp221 + tmp222 + tmp223 + tmp224 + tmp225 + tmp226 + tmp227 + tmp231; +double tmp233 = 4*tmp193*tmp43; +double tmp234 = 4*tmp198*tmp45; +double tmp235 = tmp195*tmp46; +double tmp236 = tmp233 + tmp234 + tmp235; +double tmp237 = 0.317*tmp57*tmp69*tmp51*tmp137*tmp56; +double tmp238 = 0.634*tmp57*tmp69*tmp133*tmp80; +double tmp239 = 0.317*tmp65*tmp57*tmp77*tmp55*tmp151*tmp91; +double tmp240 = 0.634*tmp65*tmp57*tmp77*tmp137*tmp121; +double tmp241 = 0.634*tmp57*tmp77*tmp55*tmp143; +double tmp242 = 0.634*tmp57*tmp51*tmp80*tmp155; +double tmp243 = tmp237 + tmp238 + tmp239 + tmp240 + tmp241 + tmp242; +double tmp244 = 4*tmp193*tmp32; +double tmp245 = 2*tmp195*tmp35; +double tmp246 = 2*tmp198*tmp37; +double tmp247 = tmp244 + tmp245 + tmp246; +double tmp248 = 0.634*tmp57*tmp114*tmp30*tmp137*tmp109; +double tmp249 = -0.317*tmp65*tmp57*tmp77*tmp51*tmp40*tmp137*tmp109*tmp56; +double tmp250 = 0.317*tmp65*tmp57*tmp77*tmp51*tmp40*tmp148*tmp116*tmp80; +double tmp251 = 1.268*tmp65*tmp57*tmp77*tmp35*tmp51*tmp109*tmp80; +double tmp252 = -0.634*tmp65*tmp57*tmp77*tmp133*tmp40*tmp109*tmp80; +double tmp253 = -0.317*tmp57*tmp114*tmp30*tmp148*tmp116*tmp55; +double tmp254 = -1.268*tmp57*tmp114*tmp22*tmp109*tmp55; +double tmp255 = 0.634*tmp57*tmp112*tmp30*tmp109*tmp55*tmp140; +double tmp256 = 0.317*tmp57*tmp69*tmp40*tmp109*tmp55*tmp151*tmp91; +double tmp257 = 0.634*tmp57*tmp69*tmp40*tmp137*tmp109*tmp121; +double tmp258 = -0.317*tmp57*tmp69*tmp40*tmp148*tmp116*tmp55*tmp121; +double tmp259 = -1.268*tmp57*tmp69*tmp35*tmp109*tmp55*tmp121; +double tmp260 = -0.634*tmp57*tmp77*tmp51*tmp40*tmp109*tmp80*tmp91*tmp143; +double tmp261 = 0.634*tmp57*tmp40*tmp109*tmp55*tmp121*tmp155; +double tmp262 = tmp248 + tmp249 + tmp250 + tmp251 + tmp252 + tmp253 + tmp254 + tmp255 + tmp256 + tmp257 + tmp258 + tmp259 + tmp260 + tmp261; +double tmp263 = 2*tmp195*tmp22; +double tmp264 = 4*tmp198*tmp25; +double tmp265 = 2*tmp193*tmp27; +double tmp266 = tmp263 + tmp264 + tmp265; +double tmp267 = -0.634*tmp57*tmp114*tmp40*tmp137*tmp109; +double tmp268 = -0.317*tmp65*tmp57*tmp77*tmp30*tmp51*tmp137*tmp109*tmp56; +double tmp269 = 0.317*tmp65*tmp57*tmp77*tmp30*tmp51*tmp148*tmp116*tmp80; +double tmp270 = -0.634*tmp65*tmp57*tmp77*tmp133*tmp30*tmp109*tmp80; +double tmp271 = 1.268*tmp65*tmp57*tmp77*tmp22*tmp51*tmp109*tmp80; +double tmp272 = 0.317*tmp57*tmp114*tmp40*tmp148*tmp116*tmp55; +double tmp273 = 1.268*tmp57*tmp114*tmp35*tmp109*tmp55; +double tmp274 = -0.634*tmp57*tmp112*tmp40*tmp109*tmp55*tmp140; +double tmp275 = 0.317*tmp57*tmp69*tmp30*tmp109*tmp55*tmp151*tmp91; +double tmp276 = 0.634*tmp57*tmp69*tmp30*tmp137*tmp109*tmp121; +double tmp277 = -0.317*tmp57*tmp69*tmp30*tmp148*tmp116*tmp55*tmp121; +double tmp278 = -1.268*tmp57*tmp69*tmp22*tmp109*tmp55*tmp121; +double tmp279 = -0.634*tmp57*tmp77*tmp30*tmp51*tmp109*tmp80*tmp91*tmp143; +double tmp280 = 0.634*tmp57*tmp30*tmp109*tmp55*tmp121*tmp155; +double tmp281 = tmp161*tmp162*tmp137; +double tmp282 = (tmp163*tmp126*tmp164*tmp137*tmp56*tmp165)/2.; +double tmp283 = tmp281 + tmp282; +double tmp284 = 1.268*tmp160*tmp283; +double tmp285 = tmp267 + tmp268 + tmp269 + tmp270 + tmp271 + tmp272 + tmp273 + tmp274 + tmp275 + tmp276 + tmp277 + tmp278 + tmp279 + tmp280 + tmp284; +double tmp286 = 0.317*tmp57*tmp69*tmp51*tmp170*tmp56; +double tmp287 = -1.268*tmp57*tmp69*tmp45*tmp80; +double tmp288 = 0.317*tmp65*tmp57*tmp77*tmp55*tmp180*tmp91; +double tmp289 = 0.634*tmp65*tmp57*tmp77*tmp170*tmp121; +double tmp290 = 0.634*tmp57*tmp77*tmp55*tmp176; +double tmp291 = 0.634*tmp57*tmp51*tmp80*tmp184; +double tmp292 = tmp286 + tmp287 + tmp288 + tmp289 + tmp290 + tmp291; +double tmp293 = 0.634*tmp57*tmp114*tmp30*tmp170*tmp109; +double tmp294 = -0.317*tmp65*tmp57*tmp77*tmp51*tmp40*tmp170*tmp109*tmp56; +double tmp295 = 0.317*tmp65*tmp57*tmp77*tmp51*tmp40*tmp177*tmp116*tmp80; +double tmp296 = -0.634*tmp65*tmp57*tmp77*tmp168*tmp51*tmp109*tmp80; +double tmp297 = 1.268*tmp65*tmp57*tmp77*tmp45*tmp40*tmp109*tmp80; +double tmp298 = -0.317*tmp57*tmp114*tmp30*tmp177*tmp116*tmp55; +double tmp299 = -1.268*tmp57*tmp114*tmp25*tmp109*tmp55; +double tmp300 = 0.634*tmp57*tmp112*tmp30*tmp109*tmp55*tmp173; +double tmp301 = 0.317*tmp57*tmp69*tmp40*tmp109*tmp55*tmp180*tmp91; +double tmp302 = 0.634*tmp57*tmp69*tmp40*tmp170*tmp109*tmp121; +double tmp303 = -0.317*tmp57*tmp69*tmp40*tmp177*tmp116*tmp55*tmp121; +double tmp304 = 0.634*tmp57*tmp69*tmp168*tmp109*tmp55*tmp121; +double tmp305 = -0.634*tmp57*tmp77*tmp51*tmp40*tmp109*tmp80*tmp91*tmp176; +double tmp306 = 0.634*tmp57*tmp40*tmp109*tmp55*tmp121*tmp184; +double tmp307 = tmp293 + tmp294 + tmp295 + tmp296 + tmp297 + tmp298 + tmp299 + tmp300 + tmp301 + tmp302 + tmp303 + tmp304 + tmp305 + tmp306; +double tmp308 = -0.634*tmp57*tmp114*tmp40*tmp170*tmp109; +double tmp309 = -0.317*tmp65*tmp57*tmp77*tmp30*tmp51*tmp170*tmp109*tmp56; +double tmp310 = 0.317*tmp65*tmp57*tmp77*tmp30*tmp51*tmp177*tmp116*tmp80; +double tmp311 = 1.268*tmp65*tmp57*tmp77*tmp45*tmp30*tmp109*tmp80; +double tmp312 = 1.268*tmp65*tmp57*tmp77*tmp25*tmp51*tmp109*tmp80; +double tmp313 = 0.317*tmp57*tmp114*tmp40*tmp177*tmp116*tmp55; +double tmp314 = -0.634*tmp57*tmp114*tmp168*tmp109*tmp55; +double tmp315 = -0.634*tmp57*tmp112*tmp40*tmp109*tmp55*tmp173; +double tmp316 = 0.317*tmp57*tmp69*tmp30*tmp109*tmp55*tmp180*tmp91; +double tmp317 = 0.634*tmp57*tmp69*tmp30*tmp170*tmp109*tmp121; +double tmp318 = -0.317*tmp57*tmp69*tmp30*tmp177*tmp116*tmp55*tmp121; +double tmp319 = -1.268*tmp57*tmp69*tmp25*tmp109*tmp55*tmp121; +double tmp320 = -0.634*tmp57*tmp77*tmp30*tmp51*tmp109*tmp80*tmp91*tmp176; +double tmp321 = 0.634*tmp57*tmp30*tmp109*tmp55*tmp121*tmp184; +double tmp322 = tmp161*tmp162*tmp170; +double tmp323 = (tmp163*tmp126*tmp164*tmp170*tmp56*tmp165)/2.; +double tmp324 = tmp322 + tmp323; +double tmp325 = 1.268*tmp160*tmp324; +double tmp326 = tmp308 + tmp309 + tmp310 + tmp311 + tmp312 + tmp313 + tmp314 + tmp315 + tmp316 + tmp317 + tmp318 + tmp319 + tmp320 + tmp321 + tmp325; +double tmp327 = tmp0*tmp1; +double tmp328 = tmp327 + tmp107; +double tmp329 = -(tmp0*tmp2); +double tmp330 = tmp329 + tmp197; +double tmp331 = tmp96 + tmp194 + tmp99 + tmp100; +double tmp332 = 4*tmp328*tmp43; +double tmp333 = 2*tmp331*tmp45; +double tmp334 = 2*tmp330*tmp46; +double tmp335 = tmp332 + tmp333 + tmp334; +double tmp336 = 4*tmp328*tmp32; +double tmp337 = 4*tmp330*tmp35; +double tmp338 = tmp331*tmp37; +double tmp339 = tmp336 + tmp337 + tmp338; +double tmp340 = 4*tmp330*tmp22; +double tmp341 = 2*tmp331*tmp25; +double tmp342 = 2*tmp328*tmp27; +double tmp343 = tmp340 + tmp341 + tmp342; +double tmp344 = ibd(2+_OFF); +double tmp345 = ibd(3+_OFF); +double tmp346 = ibod(1+_OFF); +double tmp347 = ibod(2+_OFF); +double tmp348 = ibod(3+_OFF); +double tmp349 = ibd(1+_OFF); +double tmp350 = std::pow(tmp348,2); +double tmp351 = cBar(1+_OFF); +double tmp352 = cp(3+_OFF); +double tmp353 = x(39+_OFF); +double tmp354 = mRoll(2+_OFF); +double tmp355 = mRoll(3+_OFF); +double tmp356 = x(37+_OFF); +double tmp357 = mRoll(4+_OFF); +double tmp358 = mPitch(3+_OFF); +double tmp359 = mPitch(4+_OFF); +double tmp360 = x(38+_OFF); +double tmp361 = mYaw(2+_OFF); +double tmp362 = mYaw(3+_OFF); +double tmp363 = tmp349*tmp344*tmp345; +double tmp364 = std::pow(tmp346,2); +double tmp365 = -(tmp344*tmp364); +double tmp366 = std::pow(tmp347,2); +double tmp367 = -(tmp345*tmp366); +double tmp368 = 2*tmp346*tmp347*tmp348; +double tmp369 = -(tmp349*tmp350); +double tmp370 = tmp363 + tmp365 + tmp367 + tmp368 + tmp369; +double tmp371 = 1/tmp370; +double tmp372 = tmp358*tmp90; +double tmp373 = -0.25*(tmp351*tmp359*tmp75*tmp92*tmp360); +double tmp374 = tmp372 + tmp373; +double tmp375 = -(tmp354*tmp91*tmp95); +double tmp376 = -0.25*(tmp352*tmp355*tmp75*tmp92*tmp356); +double tmp377 = (tmp352*tmp357*tmp75*tmp92*tmp353)/4.; +double tmp378 = tmp375 + tmp376 + tmp377; +double tmp379 = tmp361*tmp91*tmp95; +double tmp380 = -0.25*(tmp352*tmp362*tmp75*tmp92*tmp353); +double tmp381 = tmp379 + tmp380; +double tmp382 = -(tmp65*tmp354); +double tmp383 = (tmp352*tmp355*tmp56*tmp356)/2.; +double tmp384 = -0.5*(tmp352*tmp357*tmp56*tmp353); +double tmp385 = mRoll(1+_OFF); +double tmp386 = z(2+_OFF); +double tmp387 = tmp385*tmp386; +double tmp388 = tmp382 + tmp383 + tmp384 + tmp387; +double tmp389 = mPitch(1+_OFF); +double tmp390 = -tmp389; +double tmp391 = tmp41*tmp358; +double tmp392 = (tmp351*tmp359*tmp56*tmp360)/2.; +double tmp393 = mPitch(2+_OFF); +double tmp394 = z(3+_OFF); +double tmp395 = -(tmp393*tmp394); +double tmp396 = tmp390 + tmp391 + tmp392 + tmp395; +double tmp397 = tmp65*tmp361; +double tmp398 = (tmp352*tmp362*tmp56*tmp353)/2.; +double tmp399 = mYaw(1+_OFF); +double tmp400 = z(4+_OFF); +double tmp401 = -(tmp399*tmp400); +double tmp402 = tmp397 + tmp398 + tmp401; +double tmp403 = tmp344*tmp345; +double tmp404 = -tmp350; +double tmp405 = tmp403 + tmp404; +double tmp406 = -(tmp345*tmp347); +double tmp407 = tmp346*tmp348; +double tmp408 = tmp406 + tmp407; +double tmp409 = tmp358*tmp140; +double tmp410 = -0.25*(tmp351*tmp359*tmp137*tmp92*tmp360); +double tmp411 = tmp409 + tmp410; +double tmp412 = -(tmp354*tmp91*tmp143); +double tmp413 = -0.25*(tmp352*tmp355*tmp137*tmp92*tmp356); +double tmp414 = (tmp352*tmp357*tmp137*tmp92*tmp353)/4.; +double tmp415 = tmp412 + tmp413 + tmp414; +double tmp416 = tmp361*tmp91*tmp143; +double tmp417 = -0.25*(tmp352*tmp362*tmp137*tmp92*tmp353); +double tmp418 = tmp416 + tmp417; +double tmp419 = -(tmp344*tmp346); +double tmp420 = tmp347*tmp348; +double tmp421 = tmp419 + tmp420; +double tmp422 = tmp358*tmp173; +double tmp423 = -0.25*(tmp351*tmp359*tmp170*tmp92*tmp360); +double tmp424 = tmp422 + tmp423; +double tmp425 = -(tmp354*tmp91*tmp176); +double tmp426 = -0.25*(tmp352*tmp355*tmp170*tmp92*tmp356); +double tmp427 = (tmp352*tmp357*tmp170*tmp92*tmp353)/4.; +double tmp428 = tmp425 + tmp426 + tmp427; +double tmp429 = tmp361*tmp91*tmp176; +double tmp430 = -0.25*(tmp352*tmp362*tmp170*tmp92*tmp353); +double tmp431 = tmp429 + tmp430; +double tmp432 = 0.634*tmp351*tmp57*tmp132*tmp55*tmp374; +double tmp433 = 0.634*tmp57*tmp352*tmp159*tmp55*tmp378; +double tmp434 = 0.634*tmp57*tmp352*tmp147*tmp55*tmp381; +double tmp435 = 0.634*tmp57*tmp352*tmp159*tmp75*tmp388; +double tmp436 = 0.634*tmp351*tmp57*tmp132*tmp75*tmp396; +double tmp437 = 0.634*tmp57*tmp352*tmp147*tmp75*tmp402; +double tmp438 = tmp432 + tmp433 + tmp434 + tmp435 + tmp436 + tmp437; +double tmp439 = 0.634*tmp351*tmp57*tmp236*tmp55*tmp374; +double tmp440 = 0.634*tmp57*tmp352*tmp266*tmp55*tmp378; +double tmp441 = 0.634*tmp57*tmp352*tmp247*tmp55*tmp381; +double tmp442 = 0.634*tmp57*tmp352*tmp266*tmp75*tmp388; +double tmp443 = 0.634*tmp351*tmp57*tmp236*tmp75*tmp396; +double tmp444 = 0.634*tmp57*tmp352*tmp247*tmp75*tmp402; +double tmp445 = tmp439 + tmp440 + tmp441 + tmp442 + tmp443 + tmp444; +double tmp446 = 0.634*tmp351*tmp57*tmp335*tmp55*tmp374; +double tmp447 = 0.634*tmp57*tmp352*tmp343*tmp55*tmp378; +double tmp448 = 0.634*tmp57*tmp352*tmp339*tmp55*tmp381; +double tmp449 = 0.634*tmp57*tmp352*tmp343*tmp75*tmp388; +double tmp450 = 0.634*tmp351*tmp57*tmp335*tmp75*tmp396; +double tmp451 = 0.634*tmp57*tmp352*tmp339*tmp75*tmp402; +double tmp452 = tmp446 + tmp447 + tmp448 + tmp449 + tmp450 + tmp451; +double tmp453 = 0.634*tmp351*tmp57*tmp132*tmp55*tmp411; +double tmp454 = 0.634*tmp57*tmp352*tmp159*tmp55*tmp415; +double tmp455 = 0.634*tmp57*tmp352*tmp147*tmp55*tmp418; +double tmp456 = 0.634*tmp57*tmp352*tmp159*tmp137*tmp388; +double tmp457 = 0.634*tmp351*tmp57*tmp132*tmp137*tmp396; +double tmp458 = 0.634*tmp57*tmp352*tmp147*tmp137*tmp402; +double tmp459 = tmp453 + tmp454 + tmp455 + tmp456 + tmp457 + tmp458; +double tmp460 = tmp349*tmp345; +double tmp461 = -tmp364; +double tmp462 = tmp460 + tmp461; +double tmp463 = 0.634*tmp351*tmp57*tmp236*tmp55*tmp411; +double tmp464 = 0.634*tmp57*tmp352*tmp266*tmp55*tmp415; +double tmp465 = 0.634*tmp57*tmp352*tmp247*tmp55*tmp418; +double tmp466 = 0.634*tmp57*tmp352*tmp266*tmp137*tmp388; +double tmp467 = 0.634*tmp351*tmp57*tmp236*tmp137*tmp396; +double tmp468 = 0.634*tmp57*tmp352*tmp247*tmp137*tmp402; +double tmp469 = tmp463 + tmp464 + tmp465 + tmp466 + tmp467 + tmp468; +double tmp470 = tmp346*tmp347; +double tmp471 = -(tmp349*tmp348); +double tmp472 = tmp470 + tmp471; +double tmp473 = 0.634*tmp351*tmp57*tmp335*tmp55*tmp411; +double tmp474 = 0.634*tmp57*tmp352*tmp343*tmp55*tmp415; +double tmp475 = 0.634*tmp57*tmp352*tmp339*tmp55*tmp418; +double tmp476 = 0.634*tmp57*tmp352*tmp343*tmp137*tmp388; +double tmp477 = 0.634*tmp351*tmp57*tmp335*tmp137*tmp396; +double tmp478 = 0.634*tmp57*tmp352*tmp339*tmp137*tmp402; +double tmp479 = tmp473 + tmp474 + tmp475 + tmp476 + tmp477 + tmp478; +double tmp480 = 0.634*tmp351*tmp57*tmp132*tmp55*tmp424; +double tmp481 = 0.634*tmp57*tmp352*tmp159*tmp55*tmp428; +double tmp482 = 0.634*tmp57*tmp352*tmp147*tmp55*tmp431; +double tmp483 = 0.634*tmp57*tmp352*tmp159*tmp170*tmp388; +double tmp484 = 0.634*tmp351*tmp57*tmp132*tmp170*tmp396; +double tmp485 = 0.634*tmp57*tmp352*tmp147*tmp170*tmp402; +double tmp486 = tmp480 + tmp481 + tmp482 + tmp483 + tmp484 + tmp485; +double tmp487 = 0.634*tmp351*tmp57*tmp236*tmp55*tmp424; +double tmp488 = 0.634*tmp57*tmp352*tmp266*tmp55*tmp428; +double tmp489 = 0.634*tmp57*tmp352*tmp247*tmp55*tmp431; +double tmp490 = 0.634*tmp57*tmp352*tmp266*tmp170*tmp388; +double tmp491 = 0.634*tmp351*tmp57*tmp236*tmp170*tmp396; +double tmp492 = 0.634*tmp57*tmp352*tmp247*tmp170*tmp402; +double tmp493 = tmp487 + tmp488 + tmp489 + tmp490 + tmp491 + tmp492; +double tmp494 = 0.634*tmp351*tmp57*tmp335*tmp55*tmp424; +double tmp495 = 0.634*tmp57*tmp352*tmp343*tmp55*tmp428; +double tmp496 = 0.634*tmp57*tmp352*tmp339*tmp55*tmp431; +double tmp497 = 0.634*tmp57*tmp352*tmp343*tmp170*tmp388; +double tmp498 = 0.634*tmp351*tmp57*tmp335*tmp170*tmp396; +double tmp499 = 0.634*tmp57*tmp352*tmp339*tmp170*tmp402; +double tmp500 = tmp494 + tmp495 + tmp496 + tmp497 + tmp498 + tmp499; +double tmp501 = tmp349*tmp344; +double tmp502 = -tmp366; +double tmp503 = tmp501 + tmp502; +J(1+_OFF,1+_OFF) = -(tmp128*(tmp132*tmp191 + tmp147*tmp213 + tmp159*tmp232)); +J(1+_OFF,2+_OFF) = -(tmp128*(tmp132*tmp243 + tmp147*tmp262 + tmp159*tmp285)); +J(1+_OFF,3+_OFF) = -(tmp128*(tmp132*tmp292 + tmp147*tmp307 + tmp159*tmp326)); +J(2+_OFF,1+_OFF) = -(tmp128*(tmp236*tmp191 + tmp247*tmp213 + tmp266*tmp232)); +J(2+_OFF,2+_OFF) = -(tmp128*(tmp236*tmp243 + tmp247*tmp262 + tmp266*tmp285)); +J(2+_OFF,3+_OFF) = -(tmp128*(tmp236*tmp292 + tmp247*tmp307 + tmp266*tmp326)); +J(3+_OFF,1+_OFF) = -(tmp128*(tmp335*tmp191 + tmp339*tmp213 + tmp343*tmp232)); +J(3+_OFF,2+_OFF) = -(tmp128*(tmp335*tmp243 + tmp339*tmp262 + tmp343*tmp285)); +J(3+_OFF,3+_OFF) = -(tmp128*(tmp335*tmp292 + tmp339*tmp307 + tmp343*tmp326)); +J(4+_OFF,1+_OFF) = -(tmp405*tmp371*tmp438) - tmp408*tmp371*tmp445 - tmp421*tmp371*tmp452; +J(4+_OFF,2+_OFF) = -(tmp405*tmp371*tmp459) - tmp408*tmp371*tmp469 - tmp421*tmp371*tmp479; +J(4+_OFF,3+_OFF) = -(tmp405*tmp371*tmp486) - tmp408*tmp371*tmp493 - tmp421*tmp371*tmp500; +J(5+_OFF,1+_OFF) = -(tmp408*tmp371*tmp438) - tmp462*tmp371*tmp445 - tmp472*tmp371*tmp452; +J(5+_OFF,2+_OFF) = -(tmp408*tmp371*tmp459) - tmp462*tmp371*tmp469 - tmp472*tmp371*tmp479; +J(5+_OFF,3+_OFF) = -(tmp408*tmp371*tmp486) - tmp462*tmp371*tmp493 - tmp472*tmp371*tmp500; +J(6+_OFF,1+_OFF) = -(tmp421*tmp371*tmp438) - tmp472*tmp371*tmp445 - tmp503*tmp371*tmp452; +J(6+_OFF,2+_OFF) = -(tmp421*tmp371*tmp459) - tmp472*tmp371*tmp469 - tmp503*tmp371*tmp479; +J(6+_OFF,3+_OFF) = -(tmp421*tmp371*tmp486) - tmp472*tmp371*tmp493 - tmp503*tmp371*tmp500; diff --git a/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrWprev.cppready b/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrWprev.cppready new file mode 100644 index 0000000..0f6d7f4 --- /dev/null +++ b/roamfree/ROAMfunctions/generated/PlaneDynamicModel_JErrWprev.cppready @@ -0,0 +1,187 @@ +double tmp0 = ibd(3+_OFF); +double tmp1 = ibd(2+_OFF); +double tmp2 = ibod(1+_OFF); +double tmp3 = ibod(2+_OFF); +double tmp4 = ibod(3+_OFF); +double tmp5 = ibd(1+_OFF); +double tmp6 = x(4+_OFF); +double tmp7 = x(5+_OFF); +double tmp8 = x(6+_OFF); +double tmp9 = x(7+_OFF); +double tmp10 = x(30+_OFF); +double tmp11 = x(32+_OFF); +double tmp12 = x(31+_OFF); +double tmp13 = x(33+_OFF); +double tmp14 = tmp12*tmp11; +double tmp15 = tmp10*tmp13; +double tmp16 = tmp14 + tmp15; +double tmp17 = -(tmp10*tmp11); +double tmp18 = tmp12*tmp13; +double tmp19 = tmp17 + tmp18; +double tmp20 = std::pow(tmp10,2); +double tmp21 = std::pow(tmp12,2); +double tmp22 = std::pow(tmp11,2); +double tmp23 = -tmp22; +double tmp24 = std::pow(tmp13,2); +double tmp25 = -tmp24; +double tmp26 = tmp20 + tmp21 + tmp23 + tmp25; +double tmp27 = wind(1+_OFF); +double tmp28 = wind(3+_OFF); +double tmp29 = wind(2+_OFF); +double tmp30 = tmp11*tmp13; +double tmp31 = -tmp21; +double tmp32 = cp(2+_OFF); +double tmp33 = cp(3+_OFF); +double tmp34 = std::pow(tmp33,2); +double tmp35 = tmp5*tmp1*tmp0; +double tmp36 = std::pow(tmp2,2); +double tmp37 = -(tmp1*tmp36); +double tmp38 = std::pow(tmp3,2); +double tmp39 = -(tmp0*tmp38); +double tmp40 = 2*tmp2*tmp3*tmp4; +double tmp41 = std::pow(tmp4,2); +double tmp42 = -(tmp5*tmp41); +double tmp43 = tmp35 + tmp37 + tmp39 + tmp40 + tmp42; +double tmp44 = 1/tmp43; +double tmp45 = mRoll(3+_OFF); +double tmp46 = tmp8*tmp9; +double tmp47 = std::pow(tmp6,2); +double tmp48 = std::pow(tmp7,2); +double tmp49 = -tmp48; +double tmp50 = std::pow(tmp8,2); +double tmp51 = std::pow(tmp9,2); +double tmp52 = -2*tmp29*tmp16; +double tmp53 = -2*tmp28*tmp19; +double tmp54 = -(tmp27*tmp26); +double tmp55 = x(34+_OFF); +double tmp56 = tmp52 + tmp53 + tmp54 + tmp55; +double tmp57 = std::pow(tmp56,2); +double tmp58 = -(tmp10*tmp13); +double tmp59 = tmp14 + tmp58; +double tmp60 = -2*tmp27*tmp59; +double tmp61 = tmp10*tmp12; +double tmp62 = tmp61 + tmp30; +double tmp63 = -2*tmp28*tmp62; +double tmp64 = tmp20 + tmp31 + tmp22 + tmp25; +double tmp65 = -(tmp29*tmp64); +double tmp66 = x(35+_OFF); +double tmp67 = tmp60 + tmp63 + tmp65 + tmp66; +double tmp68 = std::pow(tmp67,2); +double tmp69 = tmp10*tmp11; +double tmp70 = tmp69 + tmp18; +double tmp71 = -2*tmp27*tmp70; +double tmp72 = -(tmp10*tmp12); +double tmp73 = tmp72 + tmp30; +double tmp74 = -2*tmp29*tmp73; +double tmp75 = tmp20 + tmp31 + tmp23 + tmp24; +double tmp76 = -(tmp28*tmp75); +double tmp77 = x(36+_OFF); +double tmp78 = tmp71 + tmp74 + tmp76 + tmp77; +double tmp79 = std::pow(tmp78,2); +double tmp80 = tmp57 + tmp68 + tmp79; +double tmp81 = std::sqrt(tmp80); +double tmp82 = tmp7*tmp8; +double tmp83 = tmp7*tmp9; +double tmp84 = -tmp50; +double tmp85 = -tmp51; +double tmp86 = tmp1*tmp0; +double tmp87 = -tmp41; +double tmp88 = tmp86 + tmp87; +double tmp89 = tmp47 + tmp48 + tmp84 + tmp85; +double tmp90 = -(tmp6*tmp8); +double tmp91 = tmp90 + tmp83; +double tmp92 = tmp6*tmp9; +double tmp93 = tmp82 + tmp92; +double tmp94 = cBar(1+_OFF); +double tmp95 = std::pow(tmp94,2); +double tmp96 = -(tmp1*tmp2); +double tmp97 = tmp3*tmp4; +double tmp98 = tmp96 + tmp97; +double tmp99 = mPitch(4+_OFF); +double tmp100 = tmp6*tmp8; +double tmp101 = tmp100 + tmp83; +double tmp102 = tmp47 + tmp49 + tmp84 + tmp51; +double tmp103 = -(tmp6*tmp7); +double tmp104 = tmp103 + tmp46; +double tmp105 = -(tmp0*tmp3); +double tmp106 = tmp2*tmp4; +double tmp107 = tmp105 + tmp106; +double tmp108 = -(tmp6*tmp9); +double tmp109 = tmp82 + tmp108; +double tmp110 = tmp6*tmp7; +double tmp111 = tmp110 + tmp46; +double tmp112 = tmp47 + tmp49 + tmp50 + tmp85; +double tmp113 = 4*tmp93*tmp16; +double tmp114 = 4*tmp91*tmp19; +double tmp115 = tmp89*tmp26; +double tmp116 = tmp113 + tmp114 + tmp115; +double tmp117 = mRoll(4+_OFF); +double tmp118 = 2*tmp112*tmp16; +double tmp119 = 4*tmp111*tmp19; +double tmp120 = 2*tmp109*tmp26; +double tmp121 = tmp118 + tmp119 + tmp120; +double tmp122 = mYaw(3+_OFF); +double tmp123 = 4*tmp104*tmp16; +double tmp124 = 2*tmp102*tmp19; +double tmp125 = 2*tmp101*tmp26; +double tmp126 = tmp123 + tmp124 + tmp125; +double tmp127 = 2*tmp89*tmp59; +double tmp128 = 4*tmp91*tmp62; +double tmp129 = 2*tmp93*tmp64; +double tmp130 = tmp127 + tmp128 + tmp129; +double tmp131 = tmp2*tmp3; +double tmp132 = -(tmp5*tmp4); +double tmp133 = tmp131 + tmp132; +double tmp134 = 4*tmp101*tmp59; +double tmp135 = 2*tmp102*tmp62; +double tmp136 = 2*tmp104*tmp64; +double tmp137 = tmp134 + tmp135 + tmp136; +double tmp138 = tmp5*tmp0; +double tmp139 = -tmp36; +double tmp140 = tmp138 + tmp139; +double tmp141 = 4*tmp109*tmp59; +double tmp142 = 4*tmp111*tmp62; +double tmp143 = tmp112*tmp64; +double tmp144 = tmp141 + tmp142 + tmp143; +double tmp145 = -0.317*tmp32*tmp34*tmp117*tmp116*tmp81; +double tmp146 = 2*tmp89*tmp70; +double tmp147 = 4*tmp93*tmp73; +double tmp148 = 2*tmp91*tmp75; +double tmp149 = tmp146 + tmp147 + tmp148; +double tmp150 = 0.317*tmp32*tmp34*tmp122*tmp149*tmp81; +double tmp151 = tmp145 + tmp150; +double tmp152 = -0.317*tmp32*tmp34*tmp117*tmp121*tmp81; +double tmp153 = 4*tmp109*tmp70; +double tmp154 = 2*tmp112*tmp73; +double tmp155 = 2*tmp111*tmp75; +double tmp156 = tmp153 + tmp154 + tmp155; +double tmp157 = 0.317*tmp32*tmp34*tmp122*tmp156*tmp81; +double tmp158 = tmp152 + tmp157; +double tmp159 = -0.317*tmp32*tmp34*tmp117*tmp126*tmp81; +double tmp160 = 4*tmp101*tmp70; +double tmp161 = 4*tmp104*tmp73; +double tmp162 = tmp102*tmp75; +double tmp163 = tmp160 + tmp161 + tmp162; +double tmp164 = 0.317*tmp32*tmp34*tmp122*tmp163*tmp81; +double tmp165 = tmp159 + tmp164; +double tmp166 = tmp5*tmp1; +double tmp167 = -tmp38; +double tmp168 = tmp166 + tmp167; +J(1+_OFF,1+_OFF) = 0; +J(1+_OFF,2+_OFF) = 0; +J(1+_OFF,3+_OFF) = 0; +J(2+_OFF,1+_OFF) = 0; +J(2+_OFF,2+_OFF) = 0; +J(2+_OFF,3+_OFF) = 0; +J(3+_OFF,1+_OFF) = 0; +J(3+_OFF,2+_OFF) = 0; +J(3+_OFF,3+_OFF) = 0; +J(4+_OFF,1+_OFF) = -0.317*tmp32*tmp34*tmp107*tmp44*tmp45*tmp121*tmp81 - 0.317*tmp32*tmp34*tmp98*tmp44*tmp45*tmp126*tmp81 - 0.317*tmp32*tmp34*tmp88*tmp44*tmp45*tmp116*tmp81; +J(4+_OFF,2+_OFF) = -0.317*tmp95*tmp32*tmp88*tmp44*tmp99*tmp130*tmp81 - 0.317*tmp95*tmp32*tmp98*tmp44*tmp99*tmp137*tmp81 - 0.317*tmp95*tmp32*tmp107*tmp44*tmp99*tmp144*tmp81; +J(4+_OFF,3+_OFF) = -(tmp88*tmp44*tmp151) - tmp107*tmp44*tmp158 - tmp98*tmp44*tmp165; +J(5+_OFF,1+_OFF) = -0.317*tmp32*tmp34*tmp140*tmp44*tmp45*tmp121*tmp81 - 0.317*tmp32*tmp34*tmp133*tmp44*tmp45*tmp126*tmp81 - 0.317*tmp32*tmp34*tmp107*tmp44*tmp45*tmp116*tmp81; +J(5+_OFF,2+_OFF) = -0.317*tmp95*tmp32*tmp107*tmp44*tmp99*tmp130*tmp81 - 0.317*tmp95*tmp32*tmp133*tmp44*tmp99*tmp137*tmp81 - 0.317*tmp95*tmp32*tmp140*tmp44*tmp99*tmp144*tmp81; +J(5+_OFF,3+_OFF) = -(tmp107*tmp44*tmp151) - tmp140*tmp44*tmp158 - tmp133*tmp44*tmp165; +J(6+_OFF,1+_OFF) = -0.317*tmp32*tmp34*tmp133*tmp44*tmp45*tmp121*tmp81 - 0.317*tmp32*tmp34*tmp168*tmp44*tmp45*tmp126*tmp81 - 0.317*tmp32*tmp34*tmp98*tmp44*tmp45*tmp116*tmp81; +J(6+_OFF,2+_OFF) = -0.317*tmp95*tmp32*tmp98*tmp44*tmp99*tmp130*tmp81 - 0.317*tmp95*tmp32*tmp168*tmp44*tmp99*tmp137*tmp81 - 0.317*tmp95*tmp32*tmp133*tmp44*tmp99*tmp144*tmp81; +J(6+_OFF,3+_OFF) = -(tmp98*tmp44*tmp151) - tmp133*tmp44*tmp158 - tmp168*tmp44*tmp165; diff --git a/roamfree/ROAMfunctions/generated/PlaneDynamicModel_debug.cppready b/roamfree/ROAMfunctions/generated/PlaneDynamicModel_debug.cppready index 07f3bb9..094c3c0 100644 --- a/roamfree/ROAMfunctions/generated/PlaneDynamicModel_debug.cppready +++ b/roamfree/ROAMfunctions/generated/PlaneDynamicModel_debug.cppready @@ -1,7 +1,7 @@ -double tmp0 = x(4+_OFF); -double tmp1 = x(6+_OFF); -double tmp2 = x(5+_OFF); -double tmp3 = x(7+_OFF); +double tmp0 = x(30+_OFF); +double tmp1 = x(32+_OFF); +double tmp2 = x(31+_OFF); +double tmp3 = x(33+_OFF); double tmp4 = wind(1+_OFF); double tmp5 = tmp2*tmp3; double tmp6 = wind(2+_OFF); @@ -21,54 +21,49 @@ double tmp19 = -2*tmp7*tmp18; double tmp20 = -tmp12; double tmp21 = tmp8 + tmp9 + tmp11 + tmp20; double tmp22 = -(tmp4*tmp21); -double tmp23 = x(8+_OFF); +double tmp23 = x(34+_OFF); double tmp24 = tmp16 + tmp19 + tmp22 + tmp23; double tmp25 = tmp0*tmp1; double tmp26 = tmp25 + tmp5; -double tmp27 = 2*tmp4*tmp26; +double tmp27 = -2*tmp4*tmp26; double tmp28 = -(tmp0*tmp2); double tmp29 = tmp1*tmp3; double tmp30 = tmp28 + tmp29; -double tmp31 = 2*tmp6*tmp30; +double tmp31 = -2*tmp6*tmp30; double tmp32 = -tmp9; double tmp33 = tmp8 + tmp32 + tmp11 + tmp12; -double tmp34 = tmp7*tmp33; -double tmp35 = x(10+_OFF); -double tmp36 = -tmp35; -double tmp37 = tmp27 + tmp31 + tmp34 + tmp36; -double tmp38 = std::atan2(tmp37,tmp24); -double tmp39 = -(tmp0*tmp3); -double tmp40 = tmp13 + tmp39; -double tmp41 = -2*tmp4*tmp40; -double tmp42 = tmp0*tmp2; -double tmp43 = tmp42 + tmp29; -double tmp44 = -2*tmp7*tmp43; -double tmp45 = tmp8 + tmp32 + tmp10 + tmp20; -double tmp46 = -(tmp6*tmp45); -double tmp47 = x(9+_OFF); -double tmp48 = tmp41 + tmp44 + tmp46 + tmp47; -double tmp49 = std::pow(tmp24,2); -double tmp50 = std::pow(tmp48,2); -double tmp51 = -2*tmp4*tmp26; -double tmp52 = -2*tmp6*tmp30; -double tmp53 = -(tmp7*tmp33); -double tmp54 = tmp51 + tmp52 + tmp53 + tmp35; -double tmp55 = std::pow(tmp54,2); -double tmp56 = tmp49 + tmp50 + tmp55; -double tmp57 = 1/std::sqrt(tmp56); -double tmp58 = tmp48*tmp57; -double tmp59 = std::asin(tmp58); -double tmp60 = cp(2+_OFF); -double tmp61 = cp(1+_OFF); -double tmp62 = std::pow(tmp61,2); -double tmp63 = z(1+_OFF); -double tmp64 = cp(3+_OFF); -double tmp65 = cBar(1+_OFF); -double tmp66 = x(13+_OFF); -dbg(1+_OFF) = 0.5*tmp60*(fDrag(1+_OFF) + tmp38*fDrag(2+_OFF) + std::pow(tmp38,2)*fDrag(3+_OFF) + std::pow(tmp59,2)*fDrag(4+_OFF))*tmp56; -dbg(2+_OFF) = 0.5*tmp59*tmp60*fLat(1+_OFF)*tmp56; -dbg(3+_OFF) = -0.5*tmp60*(fLift(1+_OFF) + tmp38*fLift(2+_OFF))*tmp56; -dbg(4+_OFF) = 1.*tmp62*((fThrust(3+_OFF)*tmp56)/std::pow(M_PI,2) + (tmp61*fThrust(2+_OFF)*std::sqrt(tmp56)*tmp63)/M_PI + tmp62*fThrust(1+_OFF)*std::pow(tmp63,2)); -dbg(5+_OFF) = 0.5*tmp60*tmp64*tmp56*(-(tmp59*mRoll(2+_OFF)) + (tmp64*mRoll(3+_OFF)*tmp57*x(11+_OFF))/2. - (tmp64*mRoll(4+_OFF)*tmp57*tmp66)/2. - mRoll(1+_OFF)*z(2+_OFF)); -dbg(6+_OFF) = 0.5*tmp65*tmp60*tmp56*(-mPitch(1+_OFF) - tmp38*mPitch(3+_OFF) + (tmp65*mPitch(4+_OFF)*tmp57*x(12+_OFF))/2. - mPitch(2+_OFF)*z(3+_OFF)); -dbg(7+_OFF) = 0.5*tmp60*tmp64*tmp56*(tmp59*mYaw(2+_OFF) + (tmp64*mYaw(3+_OFF)*tmp57*tmp66)/2. + mYaw(1+_OFF)*z(4+_OFF)); +double tmp34 = -(tmp7*tmp33); +double tmp35 = x(36+_OFF); +double tmp36 = tmp27 + tmp31 + tmp34 + tmp35; +double tmp37 = std::atan2(tmp36,tmp24); +double tmp38 = -(tmp0*tmp3); +double tmp39 = tmp13 + tmp38; +double tmp40 = -2*tmp4*tmp39; +double tmp41 = tmp0*tmp2; +double tmp42 = tmp41 + tmp29; +double tmp43 = -2*tmp7*tmp42; +double tmp44 = tmp8 + tmp32 + tmp10 + tmp20; +double tmp45 = -(tmp6*tmp44); +double tmp46 = x(35+_OFF); +double tmp47 = tmp40 + tmp43 + tmp45 + tmp46; +double tmp48 = std::pow(tmp24,2); +double tmp49 = std::pow(tmp47,2); +double tmp50 = std::pow(tmp36,2); +double tmp51 = tmp48 + tmp49 + tmp50; +double tmp52 = 1/std::sqrt(tmp51); +double tmp53 = tmp47*tmp52; +double tmp54 = std::asin(tmp53); +double tmp55 = cp(2+_OFF); +double tmp56 = cp(1+_OFF); +double tmp57 = std::pow(tmp56,2); +double tmp58 = z(1+_OFF); +double tmp59 = cp(3+_OFF); +double tmp60 = cBar(1+_OFF); +double tmp61 = x(39+_OFF); +dbg(1+_OFF) = 0.634*tmp55*(fDrag(1+_OFF) - tmp37*fDrag(2+_OFF) + std::pow(tmp37,2)*fDrag(3+_OFF) + std::pow(tmp54,2)*fDrag(4+_OFF))*tmp51; +dbg(2+_OFF) = 0.634*tmp54*tmp55*fLat(1+_OFF)*tmp51; +dbg(3+_OFF) = 0.634*tmp55*(-fLift(1+_OFF) + tmp37*fLift(2+_OFF))*tmp51; +dbg(4+_OFF) = 1.268*tmp57*((fThrust(3+_OFF)*tmp51)/std::pow(M_PI,2) + (tmp56*fThrust(2+_OFF)*std::sqrt(tmp51)*tmp58)/M_PI + tmp57*fThrust(1+_OFF)*std::pow(tmp58,2)); +dbg(5+_OFF) = 0.634*tmp55*tmp59*tmp51*(-(tmp54*mRoll(2+_OFF)) + (tmp59*mRoll(3+_OFF)*tmp52*x(37+_OFF))/2. - (tmp59*mRoll(4+_OFF)*tmp52*tmp61)/2. + mRoll(1+_OFF)*z(2+_OFF)); +dbg(6+_OFF) = 0.634*tmp60*tmp55*tmp51*(-mPitch(1+_OFF) + tmp37*mPitch(3+_OFF) + (tmp60*mPitch(4+_OFF)*tmp52*x(38+_OFF))/2. - mPitch(2+_OFF)*z(3+_OFF)); +dbg(7+_OFF) = 0.634*tmp55*tmp59*tmp51*(tmp54*mYaw(2+_OFF) + (tmp59*mYaw(3+_OFF)*tmp52*tmp61)/2. - mYaw(1+_OFF)*z(4+_OFF)); diff --git a/roamfree/ROAMfunctions/include/ROAMfunctions/PlaneDynamicModelM.h b/roamfree/ROAMfunctions/include/ROAMfunctions/PlaneDynamicModelM.h index dbabd3c..6e1dc94 100644 --- a/roamfree/ROAMfunctions/include/ROAMfunctions/PlaneDynamicModelM.h +++ b/roamfree/ROAMfunctions/include/ROAMfunctions/PlaneDynamicModelM.h @@ -124,6 +124,27 @@ class PlaneDynamicModelM { switch (wrt) { + case -12: // jacobian wrt to previous omega + { +# include "generated/PlaneDynamicModel_JErrWprev.cppready" + return true; + break; + } + + case -11: // jacobian wrt to previous v + { +# include "generated/PlaneDynamicModel_JErrVprev.cppready" + return true; + break; + } + + case -10: // jacobian wrt to previous q + { +# include "generated/PlaneDynamicModel_JErrQprev.cppready" + return true; + break; + } + case -6: // jacobian wrt to alpha { # include "generated/PlaneDynamicModel_JErrAlpha.cppready" diff --git a/roamfree/ROAMfunctions/src/AbsolutePoseM.cpp b/roamfree/ROAMfunctions/src/AbsolutePoseM.cpp index ec21685..0978f33 100644 --- a/roamfree/ROAMfunctions/src/AbsolutePoseM.cpp +++ b/roamfree/ROAMfunctions/src/AbsolutePoseM.cpp @@ -21,7 +21,7 @@ which accompanies this distribution, and is available at namespace ROAMfunctions { const bool AbsolutePoseM::_usedComponents[] = { true, true, false, false, - false, false, false, false, false, false }; + false, false, false, false, false, false, false, false}; const std::string AbsolutePoseM::_paramsNames[] = { "empty" }; diff --git a/roamfree/ROAMfunctions/src/AbsolutePositionM.cpp b/roamfree/ROAMfunctions/src/AbsolutePositionM.cpp index b5a4188..94db59c 100644 --- a/roamfree/ROAMfunctions/src/AbsolutePositionM.cpp +++ b/roamfree/ROAMfunctions/src/AbsolutePositionM.cpp @@ -20,7 +20,7 @@ which accompanies this distribution, and is available at namespace ROAMfunctions { const bool AbsolutePositionM::_usedComponents[] = { true, false, false, false, - false, false, false, false, false, false }; + false, false, false, false, false, false, false, false}; const std::string AbsolutePositionM::_paramsNames[] = { "empty" }; diff --git a/roamfree/ROAMfunctions/src/AccelerationInEarthFrameM.cpp b/roamfree/ROAMfunctions/src/AccelerationInEarthFrameM.cpp index 095841d..3fc836c 100644 --- a/roamfree/ROAMfunctions/src/AccelerationInEarthFrameM.cpp +++ b/roamfree/ROAMfunctions/src/AccelerationInEarthFrameM.cpp @@ -21,7 +21,7 @@ which accompanies this distribution, and is available at namespace ROAMfunctions { const bool AccelerationInEarthFrameM::_usedComponents[] = { true, true, true, false, true, - false, false, false, false, false }; + false, false, false, false, false, false, false}; const std::string AccelerationInEarthFrameM::_paramsNames[] = { "G", "B", "Gravity", "EarthRate", "EP", "EPShift" }; diff --git a/roamfree/ROAMfunctions/src/AccelerationM.cpp b/roamfree/ROAMfunctions/src/AccelerationM.cpp index f61a1b5..c6a3d01 100644 --- a/roamfree/ROAMfunctions/src/AccelerationM.cpp +++ b/roamfree/ROAMfunctions/src/AccelerationM.cpp @@ -21,7 +21,7 @@ which accompanies this distribution, and is available at namespace ROAMfunctions { const bool AccelerationM::_usedComponents[] = { false, true, false, false, true, - false, false, false, false, false }; + false, false, false, false, false, false, false}; const std::string AccelerationM::_paramsNames[] = { "G", "B", "Gravity" }; diff --git a/roamfree/ROAMfunctions/src/AckermannKinematicM.cpp b/roamfree/ROAMfunctions/src/AckermannKinematicM.cpp index f14733c..af3bdcb 100644 --- a/roamfree/ROAMfunctions/src/AckermannKinematicM.cpp +++ b/roamfree/ROAMfunctions/src/AckermannKinematicM.cpp @@ -21,7 +21,7 @@ which accompanies this distribution, and is available at namespace ROAMfunctions { const bool AckermannM::_usedComponents[] = { false, false, true, true, false, - false, false, false, false, false }; + false, false, false, false, false, false, false }; const std::string AckermannM::_paramsNames[] = { "kSpeed", "kSteer", "psiSteer", "L" }; diff --git a/roamfree/ROAMfunctions/src/AckermannKinematicNoInputsM.cpp b/roamfree/ROAMfunctions/src/AckermannKinematicNoInputsM.cpp index e4690da..ebcd9dd 100644 --- a/roamfree/ROAMfunctions/src/AckermannKinematicNoInputsM.cpp +++ b/roamfree/ROAMfunctions/src/AckermannKinematicNoInputsM.cpp @@ -21,7 +21,7 @@ which accompanies this distribution, and is available at namespace ROAMfunctions { const bool AckermannKinematicNoInputsM::_usedComponents[] = { false, false, true, true, false, - false, false, false, false, false }; + false, false, false, false, false, false, false }; const std::string AckermannKinematicNoInputsM::_paramsNames[] = { "CV", "CDelta", "L" }; diff --git a/roamfree/ROAMfunctions/src/AnchoredRectangularObjectFirstM.cpp b/roamfree/ROAMfunctions/src/AnchoredRectangularObjectFirstM.cpp index 4dc3b30..e13c200 100644 --- a/roamfree/ROAMfunctions/src/AnchoredRectangularObjectFirstM.cpp +++ b/roamfree/ROAMfunctions/src/AnchoredRectangularObjectFirstM.cpp @@ -20,7 +20,7 @@ which accompanies this distribution, and is available at namespace ROAMfunctions { -const bool AnchoredRectangularObjectFirstM::_usedComponents[] = {true, true, false, false, false, false, false, false, false, false}; +const bool AnchoredRectangularObjectFirstM::_usedComponents[] = {true, true, false, false, false, false, false, false, false, false, false, false}; const std::string AnchoredRectangularObjectFirstM::_paramsNames[] = {"Dim", "FOhp", "FOq", "CM"}; diff --git a/roamfree/ROAMfunctions/src/AnchoredRectangularObjectM.cpp b/roamfree/ROAMfunctions/src/AnchoredRectangularObjectM.cpp index 83648cc..5283ed4 100644 --- a/roamfree/ROAMfunctions/src/AnchoredRectangularObjectM.cpp +++ b/roamfree/ROAMfunctions/src/AnchoredRectangularObjectM.cpp @@ -20,7 +20,7 @@ which accompanies this distribution, and is available at namespace ROAMfunctions { -const bool AnchoredRectangularObjectM::_usedComponents[] = {true, true, false, false, false, false, false, false, false, false}; +const bool AnchoredRectangularObjectM::_usedComponents[] = {true, true, false, false, false, false, false, false, false, false, false, false}; const std::string AnchoredRectangularObjectM::_paramsNames[] = {"Dim", "F", "FOhp", "FOq", "CM"}; diff --git a/roamfree/ROAMfunctions/src/AngularVelocityInEarthFrameM.cpp b/roamfree/ROAMfunctions/src/AngularVelocityInEarthFrameM.cpp index b0e19be..2e85187 100644 --- a/roamfree/ROAMfunctions/src/AngularVelocityInEarthFrameM.cpp +++ b/roamfree/ROAMfunctions/src/AngularVelocityInEarthFrameM.cpp @@ -20,7 +20,7 @@ which accompanies this distribution, and is available at namespace ROAMfunctions { const bool AngularVelocityInEarthFrameM::_usedComponents[] = { false, true, false, true, - false, false, false, false, false, false }; + false, false, false, false, false, false, false, false }; const std::string AngularVelocityInEarthFrameM::_paramsNames[] = { "G", "B", "EarthRate" }; diff --git a/roamfree/ROAMfunctions/src/AngularVelocityM.cpp b/roamfree/ROAMfunctions/src/AngularVelocityM.cpp index 8577182..3b8fa3c 100644 --- a/roamfree/ROAMfunctions/src/AngularVelocityM.cpp +++ b/roamfree/ROAMfunctions/src/AngularVelocityM.cpp @@ -20,7 +20,7 @@ which accompanies this distribution, and is available at namespace ROAMfunctions { const bool AngularVelocityM::_usedComponents[] = { false, false, false, true, - false, false, false, false, false, false }; + false, false, false, false, false, false, false, false }; const std::string AngularVelocityM::_paramsNames[] = { "G", "B" }; diff --git a/roamfree/ROAMfunctions/src/DifferentialDriveKinematicM.cpp b/roamfree/ROAMfunctions/src/DifferentialDriveKinematicM.cpp index 62b2293..6eee71d 100644 --- a/roamfree/ROAMfunctions/src/DifferentialDriveKinematicM.cpp +++ b/roamfree/ROAMfunctions/src/DifferentialDriveKinematicM.cpp @@ -20,7 +20,7 @@ which accompanies this distribution, and is available at namespace ROAMfunctions { const bool DifferentialDriveKinematicM::_usedComponents[] = { false, false, - true, true, false, false, false, false, false, false }; + true, true, false, false, false, false, false, false, false, false }; const std::string DifferentialDriveKinematicM::_paramsNames[] = { "R", "L" }; diff --git a/roamfree/ROAMfunctions/src/DisplacementM.cpp b/roamfree/ROAMfunctions/src/DisplacementM.cpp index e3da5f5..994fc07 100644 --- a/roamfree/ROAMfunctions/src/DisplacementM.cpp +++ b/roamfree/ROAMfunctions/src/DisplacementM.cpp @@ -20,7 +20,7 @@ which accompanies this distribution, and is available at namespace ROAMfunctions { const bool DisplacementM::_usedComponents[] = { false, false, false, false, - false, false, true, true, false, false }; + false, false, true, true, false, false, false, false }; const std::string DisplacementM::_paramsNames[] = { "empty" }; diff --git a/roamfree/ROAMfunctions/src/FixedFeaturePoseM.cpp b/roamfree/ROAMfunctions/src/FixedFeaturePoseM.cpp index d930122..d2ce737 100644 --- a/roamfree/ROAMfunctions/src/FixedFeaturePoseM.cpp +++ b/roamfree/ROAMfunctions/src/FixedFeaturePoseM.cpp @@ -20,7 +20,7 @@ which accompanies this distribution, and is available at namespace ROAMfunctions { const bool FixedFeaturePoseM::_usedComponents[] = { true, true, false, false, - false, false, false, false, false, false }; + false, false, false, false, false, false, false, false }; const std::string FixedFeaturePoseM::_paramsNames[] = { "Fposition", "Forientation" }; diff --git a/roamfree/ROAMfunctions/src/FixedFeaturePositionM.cpp b/roamfree/ROAMfunctions/src/FixedFeaturePositionM.cpp index 6103650..5170107 100644 --- a/roamfree/ROAMfunctions/src/FixedFeaturePositionM.cpp +++ b/roamfree/ROAMfunctions/src/FixedFeaturePositionM.cpp @@ -20,7 +20,7 @@ which accompanies this distribution, and is available at namespace ROAMfunctions { const bool FixedFeaturePositionM::_usedComponents[] = { true, true, false, - false, false, false, false, false, false, false }; + false, false, false, false, false, false, false, false, false }; const std::string FixedFeaturePositionM::_paramsNames[] = { "Fposition" }; diff --git a/roamfree/ROAMfunctions/src/FramedHomogeneousPointM.cpp b/roamfree/ROAMfunctions/src/FramedHomogeneousPointM.cpp index ba3ff0c..9f17797 100644 --- a/roamfree/ROAMfunctions/src/FramedHomogeneousPointM.cpp +++ b/roamfree/ROAMfunctions/src/FramedHomogeneousPointM.cpp @@ -19,7 +19,7 @@ which accompanies this distribution, and is available at #include "FramedHomogeneousPointM.h" namespace ROAMfunctions { -const bool FramedHomogeneousPointM::_usedComponents[] = {true, true, false, false, false, false, false, false, false, false}; +const bool FramedHomogeneousPointM::_usedComponents[] = {true, true, false, false, false, false, false, false, false, false, false, false}; const std::string FramedHomogeneousPointM::_paramsNames[] = {"HP", "F", "CM"}; diff --git a/roamfree/ROAMfunctions/src/GenericOdometerM.cpp b/roamfree/ROAMfunctions/src/GenericOdometerM.cpp index ff3cae1..1d5cb5f 100644 --- a/roamfree/ROAMfunctions/src/GenericOdometerM.cpp +++ b/roamfree/ROAMfunctions/src/GenericOdometerM.cpp @@ -20,7 +20,7 @@ which accompanies this distribution, and is available at namespace ROAMfunctions { const bool GenericOdometerM::_usedComponents[] = { false, false, true, true, - false, false, false, false, false, false }; + false, false, false, false, false, false, false, false }; const std::string GenericOdometerM::_paramsNames[] = { "empty" }; diff --git a/roamfree/ROAMfunctions/src/IMUIntegralDeltaPM.cpp b/roamfree/ROAMfunctions/src/IMUIntegralDeltaPM.cpp index 1da419a..46884aa 100644 --- a/roamfree/ROAMfunctions/src/IMUIntegralDeltaPM.cpp +++ b/roamfree/ROAMfunctions/src/IMUIntegralDeltaPM.cpp @@ -19,7 +19,7 @@ which accompanies this distribution, and is available at #include "IMUIntegralDeltaPM.h" namespace ROAMfunctions { -const bool IMUImtegralDeltaPM::_usedComponents[] = {false, false, false, false, false, false, false, false, true, false}; +const bool IMUImtegralDeltaPM::_usedComponents[] = {false, false, false, false, false, false, false, false, true, false, false, false}; const std::string IMUImtegralDeltaPM::_paramsNames[] = {"Ba", "Bw"}; diff --git a/roamfree/ROAMfunctions/src/IMUIntegralDeltaQM.cpp b/roamfree/ROAMfunctions/src/IMUIntegralDeltaQM.cpp index b1a4d9c..b93d387 100644 --- a/roamfree/ROAMfunctions/src/IMUIntegralDeltaQM.cpp +++ b/roamfree/ROAMfunctions/src/IMUIntegralDeltaQM.cpp @@ -19,7 +19,7 @@ which accompanies this distribution, and is available at #include "IMUIntegralDeltaQM.h" namespace ROAMfunctions { -const bool IMUImtegralDeltaQM::_usedComponents[] = {false, false, false, false, false, false, false, true, false, false}; +const bool IMUImtegralDeltaQM::_usedComponents[] = {false, false, false, false, false, false, false, true, false, false, false, false}; const std::string IMUImtegralDeltaQM::_paramsNames[] = {"Bw"}; diff --git a/roamfree/ROAMfunctions/src/ImagePlaneProjectionM.cpp b/roamfree/ROAMfunctions/src/ImagePlaneProjectionM.cpp index ae41da7..1e273d7 100644 --- a/roamfree/ROAMfunctions/src/ImagePlaneProjectionM.cpp +++ b/roamfree/ROAMfunctions/src/ImagePlaneProjectionM.cpp @@ -19,7 +19,7 @@ which accompanies this distribution, and is available at #include "ImagePlaneProjectionM.h" namespace ROAMfunctions { -const bool ImagePlaneProjectionM::_usedComponents[] = {true, true, false, false, false, false, false, false, false, false}; +const bool ImagePlaneProjectionM::_usedComponents[] = {true, true, false, false, false, false, false, false, false, false, false, false}; const std::string ImagePlaneProjectionM::_paramsNames[] = {"Lw", "CM", "RD", "TD"}; diff --git a/roamfree/ROAMfunctions/src/LinearVelocityM.cpp b/roamfree/ROAMfunctions/src/LinearVelocityM.cpp index e52abef..7f5ca21 100644 --- a/roamfree/ROAMfunctions/src/LinearVelocityM.cpp +++ b/roamfree/ROAMfunctions/src/LinearVelocityM.cpp @@ -21,7 +21,7 @@ which accompanies this distribution, and is available at namespace ROAMfunctions { const bool LinearVelocityM::_usedComponents[] = { false, false, true, false, - false, false, false, false, false, false }; + false, false, false, false, false, false, false, false }; const std::string LinearVelocityM::_paramsNames[] = { "empty" }; diff --git a/roamfree/ROAMfunctions/src/PlaneDynamicModelM.cpp b/roamfree/ROAMfunctions/src/PlaneDynamicModelM.cpp index bdc25d2..b3a3eec 100644 --- a/roamfree/ROAMfunctions/src/PlaneDynamicModelM.cpp +++ b/roamfree/ROAMfunctions/src/PlaneDynamicModelM.cpp @@ -19,7 +19,7 @@ which accompanies this distribution, and is available at #include "PlaneDynamicModelM.h" namespace ROAMfunctions { -const bool PlaneDynamicModelM::_usedComponents[] = {true, true, true, true, true, true, false, false, false, false}; +const bool PlaneDynamicModelM::_usedComponents[] = {true, true, true, true, true, true, false, false, false, true, true, true}; const std::string PlaneDynamicModelM::_paramsNames[] = {"AirDensity", "FThrust", "FDrag", "FLat", "FLift", "MRoll", "MPitch", "MYaw", "CBar", "Cp", "Ibd", "Ibod", "Wind", "Gravity"}; diff --git a/roamfree/ROAMfunctions/src/QuadDynamicModelM.cpp b/roamfree/ROAMfunctions/src/QuadDynamicModelM.cpp index 44c9d0a..a005324 100644 --- a/roamfree/ROAMfunctions/src/QuadDynamicModelM.cpp +++ b/roamfree/ROAMfunctions/src/QuadDynamicModelM.cpp @@ -19,7 +19,7 @@ which accompanies this distribution, and is available at #include "QuadDynamicModelM.h" namespace ROAMfunctions { // x q v w a alpha -const bool QuadDynamicModelM::_usedComponents[] = {false, true, true, true, true, true, false, false, false, false}; +const bool QuadDynamicModelM::_usedComponents[] = {false, true, true, true, true, true, false, false, false, false, false, false}; const std::string QuadDynamicModelM::_paramsNames[] = {"Wnd", "Drag", "Mc", "Cp", "Ibd", "Ibod", "Gravity"}; diff --git a/roamfree/ROAMfunctions/src/RectangularObjectM.cpp b/roamfree/ROAMfunctions/src/RectangularObjectM.cpp index 3615ec1..fb1b416 100644 --- a/roamfree/ROAMfunctions/src/RectangularObjectM.cpp +++ b/roamfree/ROAMfunctions/src/RectangularObjectM.cpp @@ -19,7 +19,7 @@ which accompanies this distribution, and is available at #include "RectangularObjectM.h" namespace ROAMfunctions { -const bool RectangularObjectM::_usedComponents[] = {true, true, false, false, false, false, false, false, false, false}; +const bool RectangularObjectM::_usedComponents[] = {true, true, false, false, false, false, false, false, false, false, false, false}; const std::string RectangularObjectM::_paramsNames[] = {"Dim", "F", "CM"}; diff --git a/roamfree/ROAMfunctions/src/TriskarKinematicM.cpp b/roamfree/ROAMfunctions/src/TriskarKinematicM.cpp index f79d278..4a089b1 100644 --- a/roamfree/ROAMfunctions/src/TriskarKinematicM.cpp +++ b/roamfree/ROAMfunctions/src/TriskarKinematicM.cpp @@ -21,7 +21,7 @@ which accompanies this distribution, and is available at namespace ROAMfunctions { const bool TriskarKinematicM::_usedComponents[] = { false, false, true, true, - false, false, false, false, false, false }; + false, false, false, false, false, false, false, false }; const std::string TriskarKinematicM::_paramsNames[] = { "R", "L" }; diff --git a/roamfree/ROAMfunctions/src/VectorFieldM.cpp b/roamfree/ROAMfunctions/src/VectorFieldM.cpp index e9c92d3..90ed9e8 100644 --- a/roamfree/ROAMfunctions/src/VectorFieldM.cpp +++ b/roamfree/ROAMfunctions/src/VectorFieldM.cpp @@ -21,7 +21,7 @@ which accompanies this distribution, and is available at namespace ROAMfunctions { const bool VectorFieldM::_usedComponents[] = { false, true, false, false, false, - false, false, false, false, false }; + false, false, false, false, false, false, false }; const std::string VectorFieldM::_paramsNames[] = { "R", "S", "h" };