small formating-fixes
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
This commit is contained in:
parent
edb64d2ec5
commit
8b6d3c9835
|
|
@ -8,9 +8,9 @@ AcsParameters::AcsParameters() {
|
|||
|
||||
onBoardParams.sampleTime = 0.1;
|
||||
|
||||
inertiaEIVE.inertiaMatrix = {{ 1, 0, 0},
|
||||
{ 0, 1, 0},
|
||||
{ 0,0.5, 1}};
|
||||
inertiaEIVE.inertiaMatrix = {{ 1.0, 0.0, 0.0},
|
||||
{ 0.0, 1.0, 0.0},
|
||||
{ 0.0, 0.5, 1.0}};
|
||||
|
||||
mgmHandlingParameters.mgm0orientationMatrix = {{ 0, 0,-1},
|
||||
{ 0, 1, 0},
|
||||
|
|
@ -32,16 +32,16 @@ AcsParameters::AcsParameters() {
|
|||
rwHandlingParameters.maxTrq = 0.0032; //3.2 [mNm]
|
||||
|
||||
//Geometry frame
|
||||
rwMatrices.alignmentMatrix = {{ 0.9205, 0,-0.9205, 0},
|
||||
{ 0,-0.9205, 0, 0.9205},
|
||||
{ 0.3907, 0.3907, 0.3907, 0.3907}};
|
||||
rwMatrices.alignmentMatrix = {{ 0.9205, 0.0000, -0.9205, 0.0000},
|
||||
{ 0.0000, -0.9205, 0.0000, 0.9205},
|
||||
{ 0.3907, 0.3907, 0.3907, 0.3907}};
|
||||
|
||||
rwMatrices.pseudoInverse = {{ 0.4434,-0.2845, 0.3597},
|
||||
{ 0.2136,-0.3317, 1.0123},
|
||||
{-0.8672,-0.1406, 0.1778},
|
||||
{ 0.6426, 0.4794, 1.3603}};
|
||||
rwMatrices.pseudoInverse = {{ 0.4434, -0.2845, 0.3597},
|
||||
{ 0.2136, -0.3317, 1.0123},
|
||||
{ -0.8672, -0.1406, 0.1778},
|
||||
{ 0.6426, 0.4794, 1.3603}};
|
||||
|
||||
rwMatrices.nullspace = {-0.7358, 0.5469,-0.3637,-0.1649};
|
||||
rwMatrices.nullspace = { -0.7358, 0.5469, -0.3637, -0.1649};
|
||||
|
||||
kalmanFilterParameters.sensorNoiseSS = 8 * Math::PI / 180;
|
||||
kalmanFilterParameters.sensorNoiseMAG = 4 * Math::PI / 180;
|
||||
|
|
|
|||
|
|
@ -52,63 +52,63 @@ public:
|
|||
private:
|
||||
const double coeffG[14][13] = {{-29404.8,-2499.6, 1363.2, 903.0,-234.3, 66.0, 80.6, 23.7, 5.0,-1.9, 3.0,-2.0, 0.1},
|
||||
{ -1450.9, 2982.0,-2381.2, 809.5, 363.2, 65.5,-76.7, 9.7, 8.4,-6.2,-1.4,-0.1,-0.9},
|
||||
{ 0 , 1677.0, 1236.2, 86.3, 187.8, 72.9, -8.2,-17.6, 2.9,-0.1,-2.5, 0.5, 0.5},
|
||||
{ 0 , 0 , 525.7,-309.4,-140.7,-121.5, 56.5, -0.5, -1.5, 1.7, 2.3, 1.3, 0.7},
|
||||
{ 0 , 0 , 0 , 48.0,-151.2, -36.2, 15.8,-21.1, -1.1,-0.9,-0.9,-1.2,-0.3},
|
||||
{ 0 , 0 , 0 , 0 , 13.5, 13.5, 6.4, 15.3,-13.2, 0.7, 0.3, 0.7, 0.8},
|
||||
{ 0 , 0 , 0 , 0 , 0 , -64.7, -7.2, 13.7, 1.1,-0.9,-0.7, 0.3, 0.0},
|
||||
{ 0 , 0 , 0 , 0 , 0 , 0 , 9.8,-16.5, 8.8, 1.9,-0.1, 0.5, 0.8},
|
||||
{ 0 , 0 , 0 , 0 , 0 , 0 , 0 , -0.3, -9.3, 1.4, 1.4,-0.3, 0.0},
|
||||
{ 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 ,-11.9,-2.4,-0.6,-0.5, 0.4},
|
||||
{ 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 ,-3.8, 0.2, 0.1, 0.1},
|
||||
{ 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 3.1,-1.1, 0.5},
|
||||
{ 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 ,-0.3,-0.5},
|
||||
{ 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 ,-0.4}}; // [m][n] in nT
|
||||
{ 0.0, 1677.0, 1236.2, 86.3, 187.8, 72.9, -8.2,-17.6, 2.9,-0.1,-2.5, 0.5, 0.5},
|
||||
{ 0.0, 0.0, 525.7,-309.4,-140.7,-121.5, 56.5, -0.5, -1.5, 1.7, 2.3, 1.3, 0.7},
|
||||
{ 0.0, 0.0, 0.0, 48.0,-151.2, -36.2, 15.8,-21.1, -1.1,-0.9,-0.9,-1.2,-0.3},
|
||||
{ 0.0, 0.0, 0.0, 0.0, 13.5, 13.5, 6.4, 15.3,-13.2, 0.7, 0.3, 0.7, 0.8},
|
||||
{ 0.0, 0.0, 0.0, 0.0, 0.0, -64.7, -7.2, 13.7, 1.1,-0.9,-0.7, 0.3, 0.0},
|
||||
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 9.8,-16.5, 8.8, 1.9,-0.1, 0.5, 0.8},
|
||||
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.3, -9.3, 1.4, 1.4,-0.3, 0.0},
|
||||
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,-11.9,-2.4,-0.6,-0.5, 0.4},
|
||||
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,-3.8, 0.2, 0.1, 0.1},
|
||||
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3.1,-1.1, 0.5},
|
||||
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,-0.3,-0.5},
|
||||
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,-0.4}}; // [m][n] in nT
|
||||
|
||||
const double coeffH[14][13] = {{ 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 },
|
||||
const double coeffH[14][13] = {{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0 },
|
||||
{4652.5,-2991.6, -82.1, 281.9, 47.7,-19.1,-51.5, 8.4,-23.4, 3.4, 0.0,-1.2,-0.9},
|
||||
{ 0 , -734.6, 241.9,-158.4, 208.3, 25.1,-16.9,-15.3, 11.0,-0.2, 2.5, 0.5, 0.6},
|
||||
{ 0 , 0 ,-543.4, 199.7,-121.2, 52.8, 2.2, 12.8, 9.8, 3.6,-0.6, 1.4, 1.4},
|
||||
{ 0 , 0 , 0 ,-349.7, 32.3,-64.5, 23.5,-11.7, -5.1, 4.8,-0.4,-1.8,-0.4},
|
||||
{ 0 , 0 , 0 , 0 , 98.9, 8.9, -2.2, 14.9, -6.3,-8.6, 0.6, 0.1,-1.3},
|
||||
{ 0 , 0 , 0 , 0 , 0 , 68.1,-27.2, 3.6, 7.8,-0.1,-0.2, 0.8,-0.1},
|
||||
{ 0 , 0 , 0 , 0 , 0 , 0 , -1.8, -6.9, 0.4,-4.3,-1.7,-0.2, 0.3},
|
||||
{ 0 , 0 , 0 , 0 , 0 , 0 , 0 , 2.8, -1.4,-3.4,-1.6, 0.6,-0.1},
|
||||
{ 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 9.6,-0.1,-3.0, 0.2, 0.5},
|
||||
{ 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 ,-8.8,-2.0,-0.9, 0.5},
|
||||
{ 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 ,-2.6, 0.0,-0.4},
|
||||
{ 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0.5,-0.4},
|
||||
{ 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 ,-0.6}}; // [m][n] in nT
|
||||
{ 0.0, -734.6, 241.9,-158.4, 208.3, 25.1,-16.9,-15.3, 11.0,-0.2, 2.5, 0.5, 0.6},
|
||||
{ 0.0, 0.0,-543.4, 199.7,-121.2, 52.8, 2.2, 12.8, 9.8, 3.6,-0.6, 1.4, 1.4},
|
||||
{ 0.0, 0.0, 0.0,-349.7, 32.3,-64.5, 23.5,-11.7, -5.1, 4.8,-0.4,-1.8,-0.4},
|
||||
{ 0.0, 0.0, 0.0, 0.0, 98.9, 8.9, -2.2, 14.9, -6.3,-8.6, 0.6, 0.1,-1.3},
|
||||
{ 0.0, 0.0, 0.0, 0.0, 0.0, 68.1,-27.2, 3.6, 7.8,-0.1,-0.2, 0.8,-0.1},
|
||||
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.8, -6.9, 0.4,-4.3,-1.7,-0.2, 0.3},
|
||||
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.8, -1.4,-3.4,-1.6, 0.6,-0.1},
|
||||
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 9.6,-0.1,-3.0, 0.2, 0.5},
|
||||
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,-8.8,-2.0,-0.9, 0.5},
|
||||
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,-2.6, 0.0,-0.4},
|
||||
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5,-0.4},
|
||||
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,-0.6}}; // [m][n] in nT
|
||||
|
||||
const double svG[14][13] = {{5.7,-11.0, 2.2,-1.2,-0.3,-0.5,-0.1, 0 , 0, 0, 0, 0 ,0},
|
||||
{7.4, -7.0, -5.9,-1.6, 0.5,-0.3,-0.2, 0.1, 0, 0, 0, 0, 0},
|
||||
{0 , -2.1, 3.1,-5.9,-0.6, 0.4, 0.0,-0.1, 0, 0, 0, 0, 0},
|
||||
{0 , 0 ,-12.0, 5.2, 0.2, 1.3, 0.7, 0.4, 0, 0, 0, 0, 0},
|
||||
{0 , 0 , 0 ,-5.1, 1.3,-1.4, 0.1,-0.1, 0, 0, 0, 0, 0},
|
||||
{0 , 0 , 0 , 0 , 0.9, 0.0,-0.5, 0.4, 0, 0, 0, 0, 0},
|
||||
{0 , 0 , 0 , 0 , 0 , 0.9,-0.8, 0.3, 0, 0, 0, 0, 0},
|
||||
{0 , 0 , 0 , 0 , 0 , 0 , 0.8,-0.1, 0, 0, 0, 0, 0},
|
||||
{0 , 0 , 0 , 0 , 0 , 0 , 0 , 0.4, 0, 0, 0, 0, 0},
|
||||
{0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0, 0, 0, 0, 0},
|
||||
{0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0, 0, 0, 0, 0},
|
||||
{0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0, 0, 0, 0, 0},
|
||||
{0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0, 0, 0, 0, 0},
|
||||
{0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0, 0, 0, 0, 0}}; // [m][n] in nT
|
||||
const double svG[14][13] = {{5.7,-11.0, 2.2,-1.2,-0.3,-0.5,-0.1, 0.0, 0.0, 0.0, 0.0, 0.0 ,0.0},
|
||||
{7.4, -7.0, -5.9,-1.6, 0.5,-0.3,-0.2, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0},
|
||||
{0.0, -2.1, 3.1,-5.9,-0.6, 0.4, 0.0,-0.1, 0.0, 0.0, 0.0, 0.0, 0.0},
|
||||
{0.0, 0.0,-12.0, 5.2, 0.2, 1.3, 0.7, 0.4, 0.0, 0.0, 0.0, 0.0, 0.0},
|
||||
{0.0, 0.0, 0.0,-5.1, 1.3,-1.4, 0.1,-0.1, 0.0, 0.0, 0.0, 0.0, 0.0},
|
||||
{0.0, 0.0, 0.0, 0.0, 0.9, 0.0,-0.5, 0.4, 0.0, 0.0, 0.0, 0.0, 0.0},
|
||||
{0.0, 0.0, 0.0, 0.0, 0.0, 0.9,-0.8, 0.3, 0.0, 0.0, 0.0, 0.0, 0.0},
|
||||
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.8,-0.1, 0.0, 0.0, 0.0, 0.0, 0.0},
|
||||
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.4, 0.0, 0.0, 0.0, 0.0, 0.0},
|
||||
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
|
||||
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
|
||||
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
|
||||
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
|
||||
{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; // [m][n] in nT
|
||||
|
||||
const double svH[14][13] = {{ 0.0, 0.0, 0 , 0.0, 0.0, 0.0, 0.0, 0.0, 0, 0, 0, 0, 0},
|
||||
{-25.9,-30.2, 6.0,-0.1, 0.0, 0.0, 0.6,-0.2, 0, 0, 0, 0, 0},
|
||||
{ 0 ,-22.4,-1.1, 6.5, 2.5,-1.6, 0.6, 0.6, 0, 0, 0, 0, 0},
|
||||
{ 0 , 0 , 0.5, 3.6,-0.6,-1.3,-0.8,-0.2, 0, 0, 0, 0, 0},
|
||||
{ 0 , 0 , 0 ,-5.0, 3.0, 0.8,-0.2, 0.5, 0, 0, 0, 0, 0},
|
||||
{ 0 , 0 , 0 , 0 , 0.3, 0.0,-1.1,-0.3, 0, 0, 0, 0, 0},
|
||||
{ 0 , 0 , 0 , 0 , 0 , 1.0, 0.1,-0.4, 0, 0, 0, 0, 0},
|
||||
{ 0 , 0 , 0 , 0 , 0 , 0 , 0.3, 0.5, 0, 0, 0, 0, 0},
|
||||
{ 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0, 0, 0, 0, 0},
|
||||
{ 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0, 0, 0, 0, 0},
|
||||
{ 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0, 0, 0, 0, 0},
|
||||
{ 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0, 0, 0, 0, 0},
|
||||
{ 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0, 0, 0, 0, 0},
|
||||
{ 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0, 0, 0, 0, 0}}; // [m][n] in nT
|
||||
const double svH[14][13] = {{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
|
||||
{-25.9,-30.2, 6.0,-0.1, 0.0, 0.0, 0.6,-0.2, 0.0, 0.0, 0.0, 0.0, 0.0},
|
||||
{ 0.0,-22.4,-1.1, 6.5, 2.5,-1.6, 0.6, 0.6, 0.0, 0.0, 0.0, 0.0, 0.0},
|
||||
{ 0.0, 0.0, 0.5, 3.6,-0.6,-1.3,-0.8,-0.2, 0.0, 0.0, 0.0, 0.0, 0.0},
|
||||
{ 0.0, 0.0, 0.0,-5.0, 3.0, 0.8,-0.2, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0},
|
||||
{ 0.0, 0.0, 0.0, 0.0, 0.3, 0.0,-1.1,-0.3, 0.0, 0.0, 0.0, 0.0, 0.0},
|
||||
{ 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.1,-0.4, 0.0, 0.0, 0.0, 0.0, 0.0},
|
||||
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0},
|
||||
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
|
||||
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
|
||||
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
|
||||
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
|
||||
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0},
|
||||
{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; // [m][n] in nT
|
||||
|
||||
double updatedG[14][13];
|
||||
double updatedH[14][13];
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user