Tests/StrapdownESKFTests.mo
Download original .moGitHub source
Documentation: Tests.StrapdownESKFTests · Tests.StrapdownESKFTests.run
within Tests;
model StrapdownESKFTests
"Aided strapdown INS ESKF prediction, covariance, correction, and interfaces"
function run
output Boolean passed;
protected
constant Real tolerance = 1.0e-8;
Estimation.StrapdownINS.InitialVariances initialVariances;
Estimation.StrapdownINS.ProcessNoise processNoise;
Estimation.StrapdownINS.ESKF.State initialized;
Estimation.StrapdownINS.ESKF.State hoverPrediction;
Estimation.StrapdownINS.ESKF.State hoverRootPrediction;
Estimation.StrapdownINS.ESKF.State gpsPositionCorrection;
Estimation.StrapdownINS.ESKF.State gpsVelocityCorrection;
Estimation.StrapdownINS.ESKF.State mocapCorrection;
Estimation.StrapdownINS.ESKF.State flowPrior;
Estimation.StrapdownINS.ESKF.State flowCorrection;
Estimation.StrapdownINS.ESKF.NominalState transitionNominal;
Estimation.StrapdownINS.ESKF.NominalState transitionPerturbed;
Estimation.StrapdownINS.ESKF.NominalState transitionPredicted;
Estimation.StrapdownINS.ESKF.NominalState perturbedPredicted;
Avionics.GpsSample gps;
Avionics.MocapSample mocap;
Avionics.OpticalFlowSample opticalFlow;
Avionics.ImuSample imu;
Avionics.NavigationEstimate navigation;
Real A[15, 15];
Real transition[15, 15];
Real hoverTransition[15, 15];
Real numericalTransition[15, 15];
Real perturbation[15];
Real nominalExtendedPose[10];
Real perturbedExtendedPose[10];
Real localGroupError[10];
Real localErrorTangent[9];
Real measuredAngularVelocity[3];
Real measuredSpecificForce[3];
Real noiseInput[15, 12];
Real expectedNoiseInput[15, 12];
Real continuousNoise[12, 12];
Real discreteNoise[15, 15];
Real expectedDiscreteNoise[15, 15];
Real resetCorrection[9];
Real resetPerturbation[9];
Real resetGroup[10];
Real perturbedResetGroup[10];
Real resetErrorGroup[10];
Real resetJacobian[9, 9];
Real numericalResetJacobian[9, 9];
Real denseResetJacobian[15, 15];
Real resetPriorCovariance[15, 15];
Real denseResetCovariance[15, 15];
Real blockResetCovariance[15, 15];
Estimation.StrapdownINS.ESKF.State trustPrior;
Estimation.StrapdownINS.ESKF.State trustCorrected;
Estimation.StrapdownINS.ESKF.NominalState trustExpectedNominal;
Real trustResidual[3];
Real trustH[3, 15];
Real trustR[3, 3];
Real trustGain[15, 3];
Real trustCorrection[15];
Real trustJosephFactor[15, 15];
Real trustPosteriorCovariance[15, 15];
Real trustExpectedCovariance[15, 15];
Real trustScale;
Boolean trustAccepted;
Integer trustReason;
Real trustNis;
constant Real finiteDifferenceStep = 1.0e-6;
constant Real transitionDt = 1.0e-3;
Boolean gpsPositionAccepted;
Boolean gpsVelocityAccepted;
Boolean mocapAccepted;
Boolean flowAccepted;
Integer gpsPositionReason;
Integer gpsVelocityReason;
Integer mocapReason;
Integer flowReason;
Real nis;
algorithm
initialVariances := Estimation.StrapdownINS.InitialVariances(
position_m2=fill(1.0, 3),
velocity_m2_s2=fill(0.5, 3),
attitude_rad2=fill(0.1, 3),
gyroscopeBias_rad2_s2=fill(0.01, 3),
accelerometerBias_m2_s4=fill(0.02, 3));
processNoise := Estimation.StrapdownINS.ProcessNoise(
gyroscope_rad2_s=identity(3) * 1.0e-5,
accelerometer_m2_s3=identity(3) * 1.0e-3,
gyroscopeBias_rad2_s3=identity(3) * 1.0e-8,
accelerometerBias_m2_s5=identity(3) * 1.0e-6);
initialized := Estimation.StrapdownINS.ESKF.initialize(
zeros(3), {1.0, 0.0, 0.0, 0.0}, initialVariances);
hoverPrediction := Estimation.StrapdownINS.ESKF.predict(
initialized,
zeros(3),
{0.0, 0.0, 9.81},
{0.0, 0.0, -9.81},
0.01,
processNoise);
hoverRootPrediction := Estimation.StrapdownINS.ESKF.predict(
Estimation.StrapdownINS.ESKF.initialize(
zeros(3), {1.0, 0.0, 0.0, 0.0}, initialVariances,
useSquareRootCovariance=true),
zeros(3), {0.0, 0.0, 9.81}, {0.0, 0.0, -9.81}, 0.01, processNoise);
assert(min({hoverPrediction.covariance[axis, axis]
- initialized.covariance[axis, axis] for axis in 1:9}) >= 0.0
and Tests.Assertions.maxAbsMatrix(hoverRootPrediction.covariance
- hoverPrediction.covariance) < tolerance,
"Raw prediction lost prior uncertainty or disagrees between covariance representations");
A := Estimation.StrapdownINS.ESKF.continuousTransition(
zeros(3), {0.0, 0.0, 9.81});
hoverTransition :=
Estimation.StrapdownINS.ESKF.discreteTransition(A, 0.01);
// Barfoot SER (2nd ed.), inertial-navigation Eq. (9.208): process noise
// is ordered gyro, accelerometer, gyro-bias walk, accelerometer-bias
// walk. A local/right pose perturbation receives -accelerometer noise
// in velocity, -gyro noise in attitude, and positive bias walks.
noiseInput := Estimation.StrapdownINS.ESKF.noiseInputMatrix();
expectedNoiseInput := cat(1,
zeros(3, 12),
cat(2, zeros(3, 3), -identity(3), zeros(3, 6)),
cat(2, -identity(3), zeros(3, 9)),
cat(2, zeros(3, 6), identity(3), zeros(3, 3)),
cat(2, zeros(3, 9), identity(3)));
continuousNoise :=
Estimation.StrapdownINS.ESKF.processNoiseMatrix(processNoise);
discreteNoise := Estimation.StrapdownINS.ESKF.discreteProcessCovariance(
zeros(15, 15), noiseInput, continuousNoise, 0.01);
expectedDiscreteNoise := 0.01 * noiseInput * continuousNoise
* transpose(noiseInput);
// After right injection x+ = x Exp(delta), the new local error is
// Log(Exp(-delta) Exp(delta + e)) = Jr(delta)e + O(e^2). Numerically
// differentiating that identity catches a left/right Jacobian swap or
// a sign error in the covariance reset.
resetCorrection := {0.08, -0.04, 0.03, -0.02, 0.05, 0.01,
0.12, -0.07, 0.09};
resetGroup := LieGroups.SE23.Quat.exp_map(resetCorrection);
resetJacobian := LieGroups.SE23.Quat.right_jacobian(resetCorrection);
numericalResetJacobian := zeros(9, 9);
for column in 1:9 loop
for row in 1:9 loop
resetPerturbation[row] := if row == column then
finiteDifferenceStep else 0.0;
end for;
perturbedResetGroup := LieGroups.SE23.Quat.exp_map(
resetCorrection + resetPerturbation);
resetErrorGroup := LieGroups.SE23.Quat.product(
LieGroups.SE23.Quat.inverse(resetGroup), perturbedResetGroup);
numericalResetJacobian[:, column] :=
LieGroups.SE23.Quat.log_map(resetErrorGroup) / finiteDifferenceStep;
end for;
denseResetJacobian := cat(1,
cat(2, resetJacobian, zeros(9, 6)),
cat(2, zeros(6, 9), identity(6)));
resetPriorCovariance := initialized.covariance;
resetPriorCovariance[1, 10] := 0.004;
resetPriorCovariance[10, 1] := 0.004;
denseResetCovariance := denseResetJacobian * resetPriorCovariance
* transpose(denseResetJacobian);
blockResetCovariance := Estimation.StrapdownINS.ESKF.conjugateReset(
resetJacobian, resetPriorCovariance);
// Exercise the nonlinear trust-region branch. The effective gain that
// creates the limited correction must also be the gain in the Joseph
// covariance update; otherwise the state and covariance describe two
// different measurement updates.
trustPrior := initialized;
trustResidual := {0.0, 0.0, 1.0};
trustH := cat(2, zeros(3, 6), identity(3), zeros(3, 6));
trustR := 0.01 * identity(3);
trustGain := trustPrior.covariance * transpose(trustH)
* (1.0 / 0.11);
trustCorrection := trustGain * trustResidual;
trustScale := Estimation.StrapdownINS.ESKF.MaxAttitudeCorrection_rad
/ sqrt(trustCorrection[7] * trustCorrection[7]
+ trustCorrection[8] * trustCorrection[8]
+ trustCorrection[9] * trustCorrection[9]);
trustGain := cat(1, trustGain[1:6, :],
trustScale * trustGain[7:9, :], trustGain[10:15, :]);
trustCorrection := trustGain * trustResidual;
trustExpectedNominal := Estimation.StrapdownINS.ESKF.inject(
Estimation.StrapdownINS.ESKF.NominalState(
positionWorldEnu_m=trustPrior.positionWorldEnu_m,
velocityWorldEnu_m_s=trustPrior.velocityWorldEnu_m_s,
quaternionWorldBody=trustPrior.quaternionWorldBody,
gyroscopeBiasBodyFlu_rad_s=trustPrior.gyroscopeBiasBodyFlu_rad_s,
accelerometerBiasBodyFlu_m_s2=
trustPrior.accelerometerBiasBodyFlu_m_s2),
trustCorrection);
trustJosephFactor := identity(15) - trustGain * trustH;
trustPosteriorCovariance := LinearAlgebra.symmetrize(
LinearAlgebra.josephUpdate(
trustJosephFactor, trustPrior.covariance, trustGain, trustR));
trustExpectedCovariance := LinearAlgebra.symmetrize(
Estimation.StrapdownINS.ESKF.conjugateReset(
LieGroups.SE23.Quat.right_jacobian(trustCorrection[1:9]),
trustPosteriorCovariance));
(trustCorrected, trustAccepted, trustReason, trustNis) :=
Estimation.StrapdownINS.ESKF.correctLinear(
trustPrior, trustResidual, trustH, trustR);
transitionNominal := Estimation.StrapdownINS.ESKF.NominalState(
positionWorldEnu_m={1.0, -2.0, 0.5},
velocityWorldEnu_m_s={0.4, -0.2, 0.1},
quaternionWorldBody=LieGroups.SO3.Quat.exp_map({0.2, -0.1, 0.3}),
gyroscopeBiasBodyFlu_rad_s={0.01, -0.02, 0.03},
accelerometerBiasBodyFlu_m_s2={0.1, -0.05, 0.02});
measuredAngularVelocity := {0.5, -0.2, 0.4};
measuredSpecificForce := {1.0, 0.3, 9.0};
transitionPredicted := Estimation.StrapdownINS.ESKF.predictNominal(
transitionNominal,
measuredAngularVelocity,
measuredSpecificForce,
{0.0, 0.0, -9.81},
transitionDt);
A := Estimation.StrapdownINS.ESKF.continuousTransition(
measuredAngularVelocity
- transitionNominal.gyroscopeBiasBodyFlu_rad_s,
measuredSpecificForce
- transitionNominal.accelerometerBiasBodyFlu_m_s2);
transition := Estimation.StrapdownINS.ESKF.discreteTransition(
A, transitionDt);
numericalTransition := zeros(15, 15);
for column in 1:15 loop
for row in 1:15 loop
perturbation[row] := if row == column then
finiteDifferenceStep else 0.0;
end for;
transitionPerturbed := Estimation.StrapdownINS.ESKF.inject(
transitionNominal, perturbation);
perturbedPredicted := Estimation.StrapdownINS.ESKF.predictNominal(
transitionPerturbed,
measuredAngularVelocity,
measuredSpecificForce,
{0.0, 0.0, -9.81},
transitionDt);
nominalExtendedPose := cat(1,
transitionPredicted.positionWorldEnu_m,
transitionPredicted.velocityWorldEnu_m_s,
transitionPredicted.quaternionWorldBody);
perturbedExtendedPose := cat(1,
perturbedPredicted.positionWorldEnu_m,
perturbedPredicted.velocityWorldEnu_m_s,
perturbedPredicted.quaternionWorldBody);
localGroupError := LieGroups.SE23.Quat.product(
LieGroups.SE23.Quat.inverse(nominalExtendedPose),
perturbedExtendedPose);
localErrorTangent := LieGroups.SE23.Quat.log_map(localGroupError);
numericalTransition[1:9, column] :=
localErrorTangent / finiteDifferenceStep;
numericalTransition[10:12, column] :=
(perturbedPredicted.gyroscopeBiasBodyFlu_rad_s
- transitionPredicted.gyroscopeBiasBodyFlu_rad_s)
/ finiteDifferenceStep;
numericalTransition[13:15, column] :=
(perturbedPredicted.accelerometerBiasBodyFlu_m_s2
- transitionPredicted.accelerometerBiasBodyFlu_m_s2)
/ finiteDifferenceStep;
end for;
gps := Avionics.GpsSample(
valid=true,
fresh=true,
positionValid=true,
velocityValid=true,
timestamp_s=0.01,
geodetic_deg_m=zeros(3),
positionWorldEnu_m={1.0, 0.0, 0.0},
velocityWorldEnu_m_s={0.5, 0.0, 0.0},
positionCovarianceWorld_m2=identity(3) * 0.01,
velocityCovarianceWorld_m2_s2=identity(3) * 0.02);
(gpsPositionCorrection, gpsPositionAccepted, gpsPositionReason, nis) :=
Estimation.StrapdownINS.ESKF.correctGpsPosition(
hoverPrediction, gps);
(gpsVelocityCorrection, gpsVelocityAccepted, gpsVelocityReason, nis) :=
Estimation.StrapdownINS.ESKF.correctGpsVelocity(
gpsPositionCorrection, gps);
mocap := Avionics.MocapSample(
valid=true,
fresh=true,
timestamp_s=0.01,
positionWorldEnu_m=gpsVelocityCorrection.positionWorldEnu_m,
quaternionWorldBody=LieGroups.SO3.Quat.exp_map({0.0, 0.0, 0.1}),
positionCovarianceWorld_m2=identity(3) * 0.001,
attitudeCovarianceBody_rad2=identity(3) * 0.001);
(mocapCorrection, mocapAccepted, mocapReason, nis) :=
Estimation.StrapdownINS.ESKF.correctMocap(
gpsVelocityCorrection, mocap);
flowPrior := initialized;
flowPrior.velocityWorldEnu_m_s := {1.0, 0.0, 0.0};
opticalFlow := Avionics.OpticalFlowSample(
valid=true,
fresh=true,
timestamp_s=0.01,
integratedLineOfSight_rad={0.0, 0.005},
integratedLineOfSightCovariance_rad2=identity(2) * 1.0e-6,
integratedGyroscopeBodyFlu_rad=zeros(3),
integratedGyroscopeCovariance_rad2=identity(3) * 1.0e-10,
integrationTime_s=0.01,
groundDistance_m=1.0,
groundDistanceVariance_m2=0.01,
quality=1.0);
(flowCorrection, flowAccepted, flowReason, nis) :=
Estimation.StrapdownINS.ESKF.correctOpticalFlow(
flowPrior, opticalFlow);
imu := Avionics.ImuSample(
valid=true,
fresh=true,
timestamp_s=0.01,
angularVelocityBodyFlu_rad_s=zeros(3),
specificForceBodyFlu_m_s2={0.0, 0.0, 9.81},
deltaAngleBodyFlu_rad=zeros(3),
deltaVelocityBodyFlu_m_s={0.0, 0.0, 0.0981},
deltaPositionBodyFlu_m={0.0, 0.0, 0.0004905},
deltaQuaternionBodyFlu={1.0, 0.0, 0.0, 0.0},
gyroscopeBiasLinearizationBodyFlu_rad_s=zeros(3),
accelerometerBiasLinearizationBodyFlu_m_s2=zeros(3),
deltaRotationGyroscopeBiasJacobian_s=-identity(3) * 0.01,
deltaVelocityGyroscopeBiasJacobian_m=zeros(3, 3),
deltaVelocityAccelerometerBiasJacobian_s=-identity(3) * 0.01,
deltaPositionGyroscopeBiasJacobian_m_s=zeros(3, 3),
deltaPositionAccelerometerBiasJacobian_s2=-0.5 * identity(3) * 0.0001,
integrationTime_s=0.01);
(navigation.valid,
navigation.timestamp_s,
navigation.positionWorldEnu_m,
navigation.velocityWorldEnu_m_s,
navigation.accelerationWorldEnu_m_s2,
navigation.quaternionWorldBody,
navigation.rotationWorldBody,
navigation.eulerRpy_rad,
navigation.angularVelocityBodyFlu_rad_s,
navigation.angularVelocityWorldEnu_rad_s) :=
Estimation.StrapdownINS.ESKF.navigationEstimate(
mocapCorrection, imu, {0.0, 0.0, -9.81}, true);
assert(Tests.Assertions.maxAbsVector(
hoverPrediction.positionWorldEnu_m) < tolerance and
Tests.Assertions.maxAbsVector(
hoverPrediction.velocityWorldEnu_m_s) < tolerance,
"Mixed SE_2(3) hover prediction did not cancel gravity");
assert(abs(hoverTransition[1, 4] - 0.01) < tolerance and
abs(hoverTransition[4, 8] - 0.0981) < 1.0e-6,
"Geometric error-state covariance transition omitted position or force coupling");
assert(Tests.Assertions.maxAbsMatrix(
transition - numericalTransition) < 2.0e-5,
"Geometric error-state covariance transition does not linearize mixed prediction");
assert(Tests.Assertions.maxAbsMatrix(noiseInput - expectedNoiseInput)
< tolerance,
"Invariant process-noise input matrix disagrees with the local-error convention");
assert(Tests.Assertions.maxAbsMatrix(
discreteNoise - expectedDiscreteNoise) < tolerance,
"Zero-dynamics process covariance is not the exact Qd = dt G Q G' integral");
assert(Tests.Assertions.maxAbsMatrix(
resetJacobian - numericalResetJacobian) < 2.0e-6,
"Covariance reset is not the differential of right correction injection");
assert(Tests.Assertions.maxAbsMatrix(
blockResetCovariance - denseResetCovariance) < tolerance,
"Block covariance reset differs from dense Jr P Jr' conjugation");
assert(trustAccepted and trustReason
== Estimation.StrapdownINS.CorrectionAccepted and
Tests.Assertions.maxAbsVector(
trustCorrected.quaternionWorldBody
- trustExpectedNominal.quaternionWorldBody) < tolerance and
Tests.Assertions.maxAbsMatrix(
trustCorrected.covariance - trustExpectedCovariance) < tolerance,
"Trust-limited correction and Joseph covariance used different effective gains");
assert(Tests.Assertions.maxAbsMatrix(
hoverPrediction.covariance
- transpose(hoverPrediction.covariance)) < tolerance,
"Geometric error-state covariance prediction was not symmetric");
assert(gpsPositionAccepted and gpsVelocityAccepted and mocapAccepted
and flowAccepted,
"A positive-definite multisensor correction was rejected");
assert(gpsPositionReason
== Estimation.StrapdownINS.CorrectionAccepted
and gpsVelocityReason
== Estimation.StrapdownINS.CorrectionAccepted
and mocapReason
== Estimation.StrapdownINS.CorrectionAccepted
and flowReason
== Estimation.StrapdownINS.CorrectionAccepted,
"A disabled innovation gate reported a rejection");
assert(gpsPositionCorrection.positionWorldEnu_m[1] > 0.0 and
gpsPositionCorrection.positionWorldEnu_m[1] < 1.0,
"GPS position correction did not move the estimate toward the sample");
assert(flowCorrection.velocityWorldEnu_m_s[1] < 1.0 and
flowCorrection.velocityWorldEnu_m_s[1] > 0.5,
"Optical-flow correction did not reduce planar velocity error");
assert(Tests.Assertions.maxAbsMatrix(
navigation.rotationWorldBody
- LieGroups.SO3.Quat.to_DCM(
navigation.quaternionWorldBody)) < tolerance and
abs(navigation.eulerRpy_rad[3] - 0.1) < 1.0e-2,
"Canonical navigation attitude representations are inconsistent");
// Every field of the published record is checked against the source it
// must carry; the named boundary prevents positional output mixups.
assert(navigation.valid and
abs(navigation.timestamp_s - 0.01) < tolerance and
Tests.Assertions.maxAbsVector(
navigation.positionWorldEnu_m
- mocapCorrection.positionWorldEnu_m) < tolerance and
Tests.Assertions.maxAbsVector(
navigation.velocityWorldEnu_m_s
- mocapCorrection.velocityWorldEnu_m_s) < tolerance and
Tests.Assertions.maxAbsVector(
navigation.quaternionWorldBody
- mocapCorrection.quaternionWorldBody) < tolerance and
Tests.Assertions.maxAbsVector(
navigation.angularVelocityBodyFlu_rad_s
- (imu.angularVelocityBodyFlu_rad_s
- mocapCorrection.gyroscopeBiasBodyFlu_rad_s)) < tolerance and
Tests.Assertions.maxAbsVector(
navigation.angularVelocityWorldEnu_rad_s
- navigation.rotationWorldBody
* navigation.angularVelocityBodyFlu_rad_s) < tolerance and
Tests.Assertions.maxAbsVector(
navigation.accelerationWorldEnu_m_s2
- (navigation.rotationWorldBody
* (imu.specificForceBodyFlu_m_s2
- mocapCorrection.accelerometerBiasBodyFlu_m_s2)
+ {0.0, 0.0, -9.81})) < tolerance,
"Published navigation outputs do not match their declared sources");
passed := true;
end run;
parameter Boolean passed = run();
equation
assert(passed, "Geometric error-state estimator tests did not complete");
end StrapdownESKFTests;