Tests/StrapdownPreintegrationTests.mo
Download original .moGitHub source
Documentation: Tests.StrapdownPreintegrationTests · Tests.StrapdownPreintegrationTests.run
within Tests;
model StrapdownPreintegrationTests
"Closed-form IMU composition, bias Jacobian, and filter packet regressions"
function run
output Boolean passed;
protected
constant Real rawDt = 0.001;
constant Real packetDt = 0.01;
constant Real finiteDifferenceStep = 1.0e-6;
Real measuredAngularVelocity_rad_s[3];
Real measuredSpecificForce_m_s2[3];
Real deltaPosition_m[3];
Real deltaVelocity_m_s[3];
Real deltaQuaternion[4];
Real rotationGyroscopeBiasJacobian_s[3, 3];
Real velocityGyroscopeBiasJacobian_m[3, 3];
Real velocityAccelerometerBiasJacobian_s[3, 3];
Real positionGyroscopeBiasJacobian_m_s[3, 3];
Real positionAccelerometerBiasJacobian_s2[3, 3];
Real perturbedDeltaPosition_m[3];
Real perturbedDeltaVelocity_m_s[3];
Real perturbedDeltaQuaternion[4];
Real perturbedRotationGyroscopeBiasJacobian_s[3, 3];
Real perturbedVelocityGyroscopeBiasJacobian_m[3, 3];
Real perturbedVelocityAccelerometerBiasJacobian_s[3, 3];
Real perturbedPositionGyroscopeBiasJacobian_m_s[3, 3];
Real perturbedPositionAccelerometerBiasJacobian_s2[3, 3];
Real expectedExtendedPose[10];
Real numericalRotationGyroscopeBiasJacobian_s[3, 3];
Real numericalVelocityGyroscopeBiasJacobian_m[3, 3];
Real numericalVelocityAccelerometerBiasJacobian_s[3, 3];
Real numericalPositionGyroscopeBiasJacobian_m_s[3, 3];
Real numericalPositionAccelerometerBiasJacobian_s2[3, 3];
Real gyroBiasAnchor_rad_s[3];
Real accelerometerBiasAnchor_m_s2[3];
Real relativeQuaternion[4];
Real fohDeltaPosition_m[3];
Real fohDeltaVelocity_m_s[3];
Real fohDeltaQuaternion[4];
Real fohStartAngularVelocity_rad_s[3];
Real fohStartSpecificForce_m_s2[3];
Real fohEndAngularVelocity_rad_s[3];
Real fohEndSpecificForce_m_s2[3];
Real fohSampleAngularVelocity_rad_s[3];
Real fohSampleSpecificForce_m_s2[3];
Real fohPreviousSampleAngularVelocity_rad_s[3];
Real fohPreviousSampleSpecificForce_m_s2[3];
Real fineDeltaPosition_m[3];
Real fineDeltaVelocity_m_s[3];
Real fineDeltaQuaternion[4];
Real interpolationFraction;
Real fohAttitudeError_rad;
Real zohAttitudeError_rad;
Real fohScratchRotationJacobian_s[3, 3];
Real fohScratchVelocityGyroscopeJacobian_m[3, 3];
Real fohScratchVelocityAccelerometerJacobian_s[3, 3];
Real fohScratchPositionGyroscopeJacobian_m_s[3, 3];
Real fohScratchPositionAccelerometerJacobian_s2[3, 3];
Avionics.ImuSample imu;
Estimation.StrapdownINS.InitialVariances initialVariances;
Estimation.StrapdownINS.ProcessNoise processNoise;
Estimation.StrapdownINS.ESKF.State eskfInitial;
Estimation.StrapdownINS.ESKF.State eskfPredicted;
Estimation.StrapdownINS.ESKF.State eskfSequential;
Estimation.StrapdownINS.UKF.State ukfInitial;
Estimation.StrapdownINS.UKF.State ukfPredicted;
Boolean ukfSuccess;
algorithm
measuredAngularVelocity_rad_s := {0.35, -0.22, 0.18};
measuredSpecificForce_m_s2 := {0.7, -0.4, 9.2};
deltaPosition_m := zeros(3);
deltaVelocity_m_s := zeros(3);
deltaQuaternion := {1.0, 0.0, 0.0, 0.0};
rotationGyroscopeBiasJacobian_s := zeros(3, 3);
velocityGyroscopeBiasJacobian_m := zeros(3, 3);
velocityAccelerometerBiasJacobian_s := zeros(3, 3);
positionGyroscopeBiasJacobian_m_s := zeros(3, 3);
positionAccelerometerBiasJacobian_s2 := zeros(3, 3);
for sampleIndex in 1:10 loop
(deltaPosition_m,
deltaVelocity_m_s,
deltaQuaternion,
rotationGyroscopeBiasJacobian_s,
velocityGyroscopeBiasJacobian_m,
velocityAccelerometerBiasJacobian_s,
positionGyroscopeBiasJacobian_m_s,
positionAccelerometerBiasJacobian_s2) :=
Estimation.StrapdownINS.preintegrateImuStep(
deltaPosition_m,
deltaVelocity_m_s,
deltaQuaternion,
rotationGyroscopeBiasJacobian_s,
velocityGyroscopeBiasJacobian_m,
velocityAccelerometerBiasJacobian_s,
positionGyroscopeBiasJacobian_m_s,
positionAccelerometerBiasJacobian_s2,
measuredAngularVelocity_rad_s,
measuredSpecificForce_m_s2,
zeros(3), zeros(3), rawDt);
end for;
expectedExtendedPose := LieGroups.SE23.Quat.exp_mixed(
cat(1, zeros(3), zeros(3), {1.0, 0.0, 0.0, 0.0}),
cat(1, zeros(3), measuredSpecificForce_m_s2 * packetDt,
measuredAngularVelocity_rad_s * packetDt),
zeros(9), [0.0, packetDt; 0.0, 0.0]);
assert(Tests.Assertions.maxAbsVector(
deltaPosition_m - expectedExtendedPose[1:3]) < 2.0e-10 and
Tests.Assertions.maxAbsVector(
deltaVelocity_m_s - expectedExtendedPose[4:6]) < 2.0e-10 and
Tests.Assertions.maxAbsVector(
deltaQuaternion - expectedExtendedPose[7:10]) < 2.0e-10,
"Composed raw IMU intervals differ from one closed-form held-input packet");
numericalRotationGyroscopeBiasJacobian_s := zeros(3, 3);
numericalVelocityGyroscopeBiasJacobian_m := zeros(3, 3);
numericalVelocityAccelerometerBiasJacobian_s := zeros(3, 3);
numericalPositionGyroscopeBiasJacobian_m_s := zeros(3, 3);
numericalPositionAccelerometerBiasJacobian_s2 := zeros(3, 3);
for column in 1:3 loop
gyroBiasAnchor_rad_s := {if row == column then
finiteDifferenceStep else 0.0 for row in 1:3};
accelerometerBiasAnchor_m_s2 := zeros(3);
perturbedDeltaPosition_m := zeros(3);
perturbedDeltaVelocity_m_s := zeros(3);
perturbedDeltaQuaternion := {1.0, 0.0, 0.0, 0.0};
perturbedRotationGyroscopeBiasJacobian_s := zeros(3, 3);
perturbedVelocityGyroscopeBiasJacobian_m := zeros(3, 3);
perturbedVelocityAccelerometerBiasJacobian_s := zeros(3, 3);
perturbedPositionGyroscopeBiasJacobian_m_s := zeros(3, 3);
perturbedPositionAccelerometerBiasJacobian_s2 := zeros(3, 3);
for sampleIndex in 1:10 loop
(perturbedDeltaPosition_m,
perturbedDeltaVelocity_m_s,
perturbedDeltaQuaternion,
perturbedRotationGyroscopeBiasJacobian_s,
perturbedVelocityGyroscopeBiasJacobian_m,
perturbedVelocityAccelerometerBiasJacobian_s,
perturbedPositionGyroscopeBiasJacobian_m_s,
perturbedPositionAccelerometerBiasJacobian_s2) :=
Estimation.StrapdownINS.preintegrateImuStep(
perturbedDeltaPosition_m,
perturbedDeltaVelocity_m_s,
perturbedDeltaQuaternion,
perturbedRotationGyroscopeBiasJacobian_s,
perturbedVelocityGyroscopeBiasJacobian_m,
perturbedVelocityAccelerometerBiasJacobian_s,
perturbedPositionGyroscopeBiasJacobian_m_s,
perturbedPositionAccelerometerBiasJacobian_s2,
measuredAngularVelocity_rad_s,
measuredSpecificForce_m_s2,
gyroBiasAnchor_rad_s, zeros(3), rawDt);
end for;
relativeQuaternion := LieGroups.SO3.Quat.product(
LieGroups.SO3.Quat.inverse(deltaQuaternion),
perturbedDeltaQuaternion);
numericalRotationGyroscopeBiasJacobian_s[:, column] :=
LieGroups.SO3.Quat.log_map(relativeQuaternion)
/ finiteDifferenceStep;
numericalVelocityGyroscopeBiasJacobian_m[:, column] :=
(perturbedDeltaVelocity_m_s - deltaVelocity_m_s)
/ finiteDifferenceStep;
numericalPositionGyroscopeBiasJacobian_m_s[:, column] :=
(perturbedDeltaPosition_m - deltaPosition_m)
/ finiteDifferenceStep;
accelerometerBiasAnchor_m_s2 := {if row == column then
finiteDifferenceStep else 0.0 for row in 1:3};
gyroBiasAnchor_rad_s := zeros(3);
perturbedDeltaPosition_m := zeros(3);
perturbedDeltaVelocity_m_s := zeros(3);
perturbedDeltaQuaternion := {1.0, 0.0, 0.0, 0.0};
perturbedRotationGyroscopeBiasJacobian_s := zeros(3, 3);
perturbedVelocityGyroscopeBiasJacobian_m := zeros(3, 3);
perturbedVelocityAccelerometerBiasJacobian_s := zeros(3, 3);
perturbedPositionGyroscopeBiasJacobian_m_s := zeros(3, 3);
perturbedPositionAccelerometerBiasJacobian_s2 := zeros(3, 3);
for sampleIndex in 1:10 loop
(perturbedDeltaPosition_m,
perturbedDeltaVelocity_m_s,
perturbedDeltaQuaternion,
perturbedRotationGyroscopeBiasJacobian_s,
perturbedVelocityGyroscopeBiasJacobian_m,
perturbedVelocityAccelerometerBiasJacobian_s,
perturbedPositionGyroscopeBiasJacobian_m_s,
perturbedPositionAccelerometerBiasJacobian_s2) :=
Estimation.StrapdownINS.preintegrateImuStep(
perturbedDeltaPosition_m,
perturbedDeltaVelocity_m_s,
perturbedDeltaQuaternion,
perturbedRotationGyroscopeBiasJacobian_s,
perturbedVelocityGyroscopeBiasJacobian_m,
perturbedVelocityAccelerometerBiasJacobian_s,
perturbedPositionGyroscopeBiasJacobian_m_s,
perturbedPositionAccelerometerBiasJacobian_s2,
measuredAngularVelocity_rad_s,
measuredSpecificForce_m_s2,
zeros(3), accelerometerBiasAnchor_m_s2, rawDt);
end for;
numericalVelocityAccelerometerBiasJacobian_s[:, column] :=
(perturbedDeltaVelocity_m_s - deltaVelocity_m_s)
/ finiteDifferenceStep;
numericalPositionAccelerometerBiasJacobian_s2[:, column] :=
(perturbedDeltaPosition_m - deltaPosition_m)
/ finiteDifferenceStep;
end for;
assert(Tests.Assertions.maxAbsMatrix(
rotationGyroscopeBiasJacobian_s
- numericalRotationGyroscopeBiasJacobian_s) < 2.0e-8 and
Tests.Assertions.maxAbsMatrix(
velocityGyroscopeBiasJacobian_m
- numericalVelocityGyroscopeBiasJacobian_m) < 5.0e-5 and
Tests.Assertions.maxAbsMatrix(
velocityAccelerometerBiasJacobian_s
- numericalVelocityAccelerometerBiasJacobian_s) < 5.0e-7 and
Tests.Assertions.maxAbsMatrix(
positionGyroscopeBiasJacobian_m_s
- numericalPositionGyroscopeBiasJacobian_m_s) < 5.0e-7 and
Tests.Assertions.maxAbsMatrix(
positionAccelerometerBiasJacobian_s2
- numericalPositionAccelerometerBiasJacobian_s2) < 5.0e-9,
"Stored packet bias Jacobians disagree with finite differences");
// With the hold flag false the trailing first-order-hold inputs must be
// inert: the explicit call has to reproduce the default call exactly.
(fohDeltaPosition_m,
fohDeltaVelocity_m_s,
fohDeltaQuaternion,
perturbedRotationGyroscopeBiasJacobian_s,
perturbedVelocityGyroscopeBiasJacobian_m,
perturbedVelocityAccelerometerBiasJacobian_s,
perturbedPositionGyroscopeBiasJacobian_m_s,
perturbedPositionAccelerometerBiasJacobian_s2) :=
Estimation.StrapdownINS.preintegrateImuStep(
deltaPosition_m,
deltaVelocity_m_s,
deltaQuaternion,
rotationGyroscopeBiasJacobian_s,
velocityGyroscopeBiasJacobian_m,
velocityAccelerometerBiasJacobian_s,
positionGyroscopeBiasJacobian_m_s,
positionAccelerometerBiasJacobian_s2,
measuredAngularVelocity_rad_s,
measuredSpecificForce_m_s2,
zeros(3), zeros(3), rawDt,
false, {7.0, -3.0, 11.0}, {-5.0, 2.0, 13.0});
(fineDeltaPosition_m,
fineDeltaVelocity_m_s,
fineDeltaQuaternion,
numericalRotationGyroscopeBiasJacobian_s,
numericalVelocityGyroscopeBiasJacobian_m,
numericalVelocityAccelerometerBiasJacobian_s,
numericalPositionGyroscopeBiasJacobian_m_s,
numericalPositionAccelerometerBiasJacobian_s2) :=
Estimation.StrapdownINS.preintegrateImuStep(
deltaPosition_m,
deltaVelocity_m_s,
deltaQuaternion,
rotationGyroscopeBiasJacobian_s,
velocityGyroscopeBiasJacobian_m,
velocityAccelerometerBiasJacobian_s,
positionGyroscopeBiasJacobian_m_s,
positionAccelerometerBiasJacobian_s2,
measuredAngularVelocity_rad_s,
measuredSpecificForce_m_s2,
zeros(3), zeros(3), rawDt);
assert(Tests.Assertions.maxAbsVector(
fohDeltaPosition_m - fineDeltaPosition_m) < 1.0e-30 and
Tests.Assertions.maxAbsVector(
fohDeltaVelocity_m_s - fineDeltaVelocity_m_s) < 1.0e-30 and
Tests.Assertions.maxAbsVector(
fohDeltaQuaternion - fineDeltaQuaternion) < 1.0e-30 and
Tests.Assertions.maxAbsMatrix(
perturbedRotationGyroscopeBiasJacobian_s
- numericalRotationGyroscopeBiasJacobian_s) < 1.0e-30 and
Tests.Assertions.maxAbsMatrix(
perturbedVelocityGyroscopeBiasJacobian_m
- numericalVelocityGyroscopeBiasJacobian_m) < 1.0e-30 and
Tests.Assertions.maxAbsMatrix(
perturbedPositionAccelerometerBiasJacobian_s2
- numericalPositionAccelerometerBiasJacobian_s2) < 1.0e-30,
"Disabled first-order hold does not reproduce the zero-order hold");
// One first-order-hold interval against the same linear input composed
// from two hundred fine sub-intervals: the single Lie bracket has to
// reproduce the fine flow to its T^5 residual, and the wrong bracket
// sign would double the trapezoid-only error and fail the bound.
fohStartAngularVelocity_rad_s := {0.3, -0.2, 0.5};
fohEndAngularVelocity_rad_s := {0.5, -0.1, 0.35};
fohStartSpecificForce_m_s2 := {0.6, -0.3, 9.0};
fohEndSpecificForce_m_s2 := {1.1, -0.7, 9.3};
fineDeltaPosition_m := zeros(3);
fineDeltaVelocity_m_s := zeros(3);
fineDeltaQuaternion := {1.0, 0.0, 0.0, 0.0};
fohPreviousSampleAngularVelocity_rad_s :=
fohStartAngularVelocity_rad_s;
fohPreviousSampleSpecificForce_m_s2 := fohStartSpecificForce_m_s2;
for sampleIndex in 1:200 loop
interpolationFraction := sampleIndex / 200.0;
fohSampleAngularVelocity_rad_s := fohStartAngularVelocity_rad_s
+ interpolationFraction * (fohEndAngularVelocity_rad_s
- fohStartAngularVelocity_rad_s);
fohSampleSpecificForce_m_s2 := fohStartSpecificForce_m_s2
+ interpolationFraction * (fohEndSpecificForce_m_s2
- fohStartSpecificForce_m_s2);
(fineDeltaPosition_m,
fineDeltaVelocity_m_s,
fineDeltaQuaternion,
numericalRotationGyroscopeBiasJacobian_s,
numericalVelocityGyroscopeBiasJacobian_m,
numericalVelocityAccelerometerBiasJacobian_s,
numericalPositionGyroscopeBiasJacobian_m_s,
numericalPositionAccelerometerBiasJacobian_s2) :=
Estimation.StrapdownINS.preintegrateImuStep(
fineDeltaPosition_m,
fineDeltaVelocity_m_s,
fineDeltaQuaternion,
zeros(3, 3), zeros(3, 3), zeros(3, 3), zeros(3, 3),
zeros(3, 3),
fohSampleAngularVelocity_rad_s,
fohSampleSpecificForce_m_s2,
zeros(3), zeros(3), packetDt / 200.0,
true,
fohPreviousSampleAngularVelocity_rad_s,
fohPreviousSampleSpecificForce_m_s2);
fohPreviousSampleAngularVelocity_rad_s :=
fohSampleAngularVelocity_rad_s;
fohPreviousSampleSpecificForce_m_s2 := fohSampleSpecificForce_m_s2;
end for;
(fohDeltaPosition_m,
fohDeltaVelocity_m_s,
fohDeltaQuaternion,
numericalRotationGyroscopeBiasJacobian_s,
numericalVelocityGyroscopeBiasJacobian_m,
numericalVelocityAccelerometerBiasJacobian_s,
numericalPositionGyroscopeBiasJacobian_m_s,
numericalPositionAccelerometerBiasJacobian_s2) :=
Estimation.StrapdownINS.preintegrateImuStep(
zeros(3), zeros(3), {1.0, 0.0, 0.0, 0.0},
zeros(3, 3), zeros(3, 3), zeros(3, 3), zeros(3, 3), zeros(3, 3),
fohEndAngularVelocity_rad_s,
fohEndSpecificForce_m_s2,
zeros(3), zeros(3), packetDt,
true,
fohStartAngularVelocity_rad_s,
fohStartSpecificForce_m_s2);
(perturbedDeltaPosition_m,
perturbedDeltaVelocity_m_s,
perturbedDeltaQuaternion,
numericalRotationGyroscopeBiasJacobian_s,
numericalVelocityGyroscopeBiasJacobian_m,
numericalVelocityAccelerometerBiasJacobian_s,
numericalPositionGyroscopeBiasJacobian_m_s,
numericalPositionAccelerometerBiasJacobian_s2) :=
Estimation.StrapdownINS.preintegrateImuStep(
zeros(3), zeros(3), {1.0, 0.0, 0.0, 0.0},
zeros(3, 3), zeros(3, 3), zeros(3, 3), zeros(3, 3), zeros(3, 3),
fohEndAngularVelocity_rad_s,
fohEndSpecificForce_m_s2,
zeros(3), zeros(3), packetDt);
fohAttitudeError_rad := Tests.Assertions.maxAbsVector(
LieGroups.SO3.Quat.log_map(LieGroups.SO3.Quat.product(
LieGroups.SO3.Quat.inverse(fineDeltaQuaternion),
fohDeltaQuaternion)));
zohAttitudeError_rad := Tests.Assertions.maxAbsVector(
LieGroups.SO3.Quat.log_map(LieGroups.SO3.Quat.product(
LieGroups.SO3.Quat.inverse(fineDeltaQuaternion),
perturbedDeltaQuaternion)));
assert(fohAttitudeError_rad < 1.0e-8 and
zohAttitudeError_rad > 1.0e-4 and
fohAttitudeError_rad < 1.0e-3 * zohAttitudeError_rad and
Tests.Assertions.maxAbsVector(
fohDeltaVelocity_m_s - fineDeltaVelocity_m_s) < 1.0e-8 and
Tests.Assertions.maxAbsVector(
perturbedDeltaVelocity_m_s - fineDeltaVelocity_m_s) > 1.0e-4 and
Tests.Assertions.maxAbsVector(
fohDeltaPosition_m - fineDeltaPosition_m) < 1.0e-9,
"First-order-hold interval disagrees with the fine linear-input flow");
// First-order-hold bias Jacobians over a composed varying-signal window
// against finite differences of the bias anchors, exercising the
// (dt^2/12) cross terms including the velocity-increment sensitivity to
// gyroscope bias that has no zero-order-hold counterpart.
fohDeltaPosition_m := zeros(3);
fohDeltaVelocity_m_s := zeros(3);
fohDeltaQuaternion := {1.0, 0.0, 0.0, 0.0};
perturbedRotationGyroscopeBiasJacobian_s := zeros(3, 3);
perturbedVelocityGyroscopeBiasJacobian_m := zeros(3, 3);
perturbedVelocityAccelerometerBiasJacobian_s := zeros(3, 3);
perturbedPositionGyroscopeBiasJacobian_m_s := zeros(3, 3);
perturbedPositionAccelerometerBiasJacobian_s2 := zeros(3, 3);
fohPreviousSampleAngularVelocity_rad_s :=
{0.35, -0.22 + 0.04, 0.18};
fohPreviousSampleSpecificForce_m_s2 := {0.7, -0.4 + 0.25, 9.2};
for sampleIndex in 1:10 loop
fohSampleAngularVelocity_rad_s := {
0.35 + 0.05 * sin(0.7 * sampleIndex),
-0.22 + 0.04 * cos(0.9 * sampleIndex),
0.18 + 0.03 * sin(1.1 * sampleIndex)};
fohSampleSpecificForce_m_s2 := {
0.7 + 0.3 * sin(0.8 * sampleIndex),
-0.4 + 0.25 * cos(0.6 * sampleIndex),
9.2 + 0.2 * sin(1.3 * sampleIndex)};
(fohDeltaPosition_m,
fohDeltaVelocity_m_s,
fohDeltaQuaternion,
perturbedRotationGyroscopeBiasJacobian_s,
perturbedVelocityGyroscopeBiasJacobian_m,
perturbedVelocityAccelerometerBiasJacobian_s,
perturbedPositionGyroscopeBiasJacobian_m_s,
perturbedPositionAccelerometerBiasJacobian_s2) :=
Estimation.StrapdownINS.preintegrateImuStep(
fohDeltaPosition_m,
fohDeltaVelocity_m_s,
fohDeltaQuaternion,
perturbedRotationGyroscopeBiasJacobian_s,
perturbedVelocityGyroscopeBiasJacobian_m,
perturbedVelocityAccelerometerBiasJacobian_s,
perturbedPositionGyroscopeBiasJacobian_m_s,
perturbedPositionAccelerometerBiasJacobian_s2,
fohSampleAngularVelocity_rad_s,
fohSampleSpecificForce_m_s2,
zeros(3), zeros(3), rawDt,
true,
fohPreviousSampleAngularVelocity_rad_s,
fohPreviousSampleSpecificForce_m_s2);
fohPreviousSampleAngularVelocity_rad_s :=
fohSampleAngularVelocity_rad_s;
fohPreviousSampleSpecificForce_m_s2 := fohSampleSpecificForce_m_s2;
end for;
for column in 1:3 loop
gyroBiasAnchor_rad_s := {if row == column then
finiteDifferenceStep else 0.0 for row in 1:3};
fineDeltaPosition_m := zeros(3);
fineDeltaVelocity_m_s := zeros(3);
fineDeltaQuaternion := {1.0, 0.0, 0.0, 0.0};
fohScratchRotationJacobian_s := zeros(3, 3);
fohScratchVelocityGyroscopeJacobian_m := zeros(3, 3);
fohScratchVelocityAccelerometerJacobian_s := zeros(3, 3);
fohScratchPositionGyroscopeJacobian_m_s := zeros(3, 3);
fohScratchPositionAccelerometerJacobian_s2 := zeros(3, 3);
fohPreviousSampleAngularVelocity_rad_s :=
{0.35, -0.22 + 0.04, 0.18};
fohPreviousSampleSpecificForce_m_s2 := {0.7, -0.4 + 0.25, 9.2};
for sampleIndex in 1:10 loop
fohSampleAngularVelocity_rad_s := {
0.35 + 0.05 * sin(0.7 * sampleIndex),
-0.22 + 0.04 * cos(0.9 * sampleIndex),
0.18 + 0.03 * sin(1.1 * sampleIndex)};
fohSampleSpecificForce_m_s2 := {
0.7 + 0.3 * sin(0.8 * sampleIndex),
-0.4 + 0.25 * cos(0.6 * sampleIndex),
9.2 + 0.2 * sin(1.3 * sampleIndex)};
(fineDeltaPosition_m,
fineDeltaVelocity_m_s,
fineDeltaQuaternion,
fohScratchRotationJacobian_s,
fohScratchVelocityGyroscopeJacobian_m,
fohScratchVelocityAccelerometerJacobian_s,
fohScratchPositionGyroscopeJacobian_m_s,
fohScratchPositionAccelerometerJacobian_s2) :=
Estimation.StrapdownINS.preintegrateImuStep(
fineDeltaPosition_m,
fineDeltaVelocity_m_s,
fineDeltaQuaternion,
fohScratchRotationJacobian_s,
fohScratchVelocityGyroscopeJacobian_m,
fohScratchVelocityAccelerometerJacobian_s,
fohScratchPositionGyroscopeJacobian_m_s,
fohScratchPositionAccelerometerJacobian_s2,
fohSampleAngularVelocity_rad_s,
fohSampleSpecificForce_m_s2,
gyroBiasAnchor_rad_s, zeros(3), rawDt,
true,
fohPreviousSampleAngularVelocity_rad_s,
fohPreviousSampleSpecificForce_m_s2);
fohPreviousSampleAngularVelocity_rad_s :=
fohSampleAngularVelocity_rad_s;
fohPreviousSampleSpecificForce_m_s2 :=
fohSampleSpecificForce_m_s2;
end for;
relativeQuaternion := LieGroups.SO3.Quat.product(
LieGroups.SO3.Quat.inverse(fohDeltaQuaternion),
fineDeltaQuaternion);
numericalRotationGyroscopeBiasJacobian_s[:, column] :=
LieGroups.SO3.Quat.log_map(relativeQuaternion)
/ finiteDifferenceStep;
numericalVelocityGyroscopeBiasJacobian_m[:, column] :=
(fineDeltaVelocity_m_s - fohDeltaVelocity_m_s)
/ finiteDifferenceStep;
numericalPositionGyroscopeBiasJacobian_m_s[:, column] :=
(fineDeltaPosition_m - fohDeltaPosition_m)
/ finiteDifferenceStep;
accelerometerBiasAnchor_m_s2 := {if row == column then
finiteDifferenceStep else 0.0 for row in 1:3};
fineDeltaPosition_m := zeros(3);
fineDeltaVelocity_m_s := zeros(3);
fineDeltaQuaternion := {1.0, 0.0, 0.0, 0.0};
fohScratchRotationJacobian_s := zeros(3, 3);
fohScratchVelocityGyroscopeJacobian_m := zeros(3, 3);
fohScratchVelocityAccelerometerJacobian_s := zeros(3, 3);
fohScratchPositionGyroscopeJacobian_m_s := zeros(3, 3);
fohScratchPositionAccelerometerJacobian_s2 := zeros(3, 3);
fohPreviousSampleAngularVelocity_rad_s :=
{0.35, -0.22 + 0.04, 0.18};
fohPreviousSampleSpecificForce_m_s2 := {0.7, -0.4 + 0.25, 9.2};
for sampleIndex in 1:10 loop
fohSampleAngularVelocity_rad_s := {
0.35 + 0.05 * sin(0.7 * sampleIndex),
-0.22 + 0.04 * cos(0.9 * sampleIndex),
0.18 + 0.03 * sin(1.1 * sampleIndex)};
fohSampleSpecificForce_m_s2 := {
0.7 + 0.3 * sin(0.8 * sampleIndex),
-0.4 + 0.25 * cos(0.6 * sampleIndex),
9.2 + 0.2 * sin(1.3 * sampleIndex)};
(fineDeltaPosition_m,
fineDeltaVelocity_m_s,
fineDeltaQuaternion,
fohScratchRotationJacobian_s,
fohScratchVelocityGyroscopeJacobian_m,
fohScratchVelocityAccelerometerJacobian_s,
fohScratchPositionGyroscopeJacobian_m_s,
fohScratchPositionAccelerometerJacobian_s2) :=
Estimation.StrapdownINS.preintegrateImuStep(
fineDeltaPosition_m,
fineDeltaVelocity_m_s,
fineDeltaQuaternion,
fohScratchRotationJacobian_s,
fohScratchVelocityGyroscopeJacobian_m,
fohScratchVelocityAccelerometerJacobian_s,
fohScratchPositionGyroscopeJacobian_m_s,
fohScratchPositionAccelerometerJacobian_s2,
fohSampleAngularVelocity_rad_s,
fohSampleSpecificForce_m_s2,
zeros(3), accelerometerBiasAnchor_m_s2, rawDt,
true,
fohPreviousSampleAngularVelocity_rad_s,
fohPreviousSampleSpecificForce_m_s2);
fohPreviousSampleAngularVelocity_rad_s :=
fohSampleAngularVelocity_rad_s;
fohPreviousSampleSpecificForce_m_s2 :=
fohSampleSpecificForce_m_s2;
end for;
numericalVelocityAccelerometerBiasJacobian_s[:, column] :=
(fineDeltaVelocity_m_s - fohDeltaVelocity_m_s)
/ finiteDifferenceStep;
numericalPositionAccelerometerBiasJacobian_s2[:, column] :=
(fineDeltaPosition_m - fohDeltaPosition_m)
/ finiteDifferenceStep;
end for;
assert(Tests.Assertions.maxAbsMatrix(
perturbedRotationGyroscopeBiasJacobian_s
- numericalRotationGyroscopeBiasJacobian_s) < 5.0e-8 and
Tests.Assertions.maxAbsMatrix(
perturbedVelocityGyroscopeBiasJacobian_m
- numericalVelocityGyroscopeBiasJacobian_m) < 1.0e-4 and
Tests.Assertions.maxAbsMatrix(
perturbedVelocityAccelerometerBiasJacobian_s
- numericalVelocityAccelerometerBiasJacobian_s) < 1.0e-6 and
Tests.Assertions.maxAbsMatrix(
perturbedPositionGyroscopeBiasJacobian_m_s
- numericalPositionGyroscopeBiasJacobian_m_s) < 1.0e-6 and
Tests.Assertions.maxAbsMatrix(
perturbedPositionAccelerometerBiasJacobian_s2
- numericalPositionAccelerometerBiasJacobian_s2) < 1.0e-8,
"First-order-hold bias Jacobians disagree with finite differences");
imu := Avionics.ImuSample(
valid=true, fresh=true, timestamp_s=packetDt,
angularVelocityBodyFlu_rad_s=measuredAngularVelocity_rad_s,
specificForceBodyFlu_m_s2=measuredSpecificForce_m_s2,
deltaAngleBodyFlu_rad=LieGroups.SO3.Quat.log_map(deltaQuaternion),
deltaVelocityBodyFlu_m_s=deltaVelocity_m_s,
deltaPositionBodyFlu_m=deltaPosition_m,
deltaQuaternionBodyFlu=deltaQuaternion,
integrationTime_s=packetDt,
gyroscopeBiasLinearizationBodyFlu_rad_s=zeros(3),
accelerometerBiasLinearizationBodyFlu_m_s2=zeros(3),
deltaRotationGyroscopeBiasJacobian_s=
rotationGyroscopeBiasJacobian_s,
deltaVelocityGyroscopeBiasJacobian_m=
velocityGyroscopeBiasJacobian_m,
deltaVelocityAccelerometerBiasJacobian_s=
velocityAccelerometerBiasJacobian_s,
deltaPositionGyroscopeBiasJacobian_m_s=
positionGyroscopeBiasJacobian_m_s,
deltaPositionAccelerometerBiasJacobian_s2=
positionAccelerometerBiasJacobian_s2);
initialVariances := Estimation.StrapdownINS.InitialVariances(
position_m2=fill(0.04, 3), velocity_m2_s2=fill(0.01, 3),
attitude_rad2=fill(0.01, 3),
gyroscopeBias_rad2_s2=fill(1.0e-5, 3),
accelerometerBias_m2_s4=fill(1.0e-3, 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);
eskfInitial := Estimation.StrapdownINS.ESKF.initialize(
zeros(3), {1.0, 0.0, 0.0, 0.0}, initialVariances);
eskfPredicted := Estimation.StrapdownINS.ESKF.predictPreintegrated(
eskfInitial, imu, {0.0, 0.0, -9.81}, processNoise);
eskfSequential := eskfInitial;
for sampleIndex in 1:10 loop
eskfSequential := Estimation.StrapdownINS.ESKF.predict(
eskfSequential, measuredAngularVelocity_rad_s,
measuredSpecificForce_m_s2, {0.0, 0.0, -9.81}, rawDt,
processNoise);
end for;
assert(Tests.Assertions.maxAbsVector(
eskfPredicted.positionWorldEnu_m
- eskfSequential.positionWorldEnu_m) < 2.0e-10 and
Tests.Assertions.maxAbsVector(
eskfPredicted.velocityWorldEnu_m_s
- eskfSequential.velocityWorldEnu_m_s) < 2.0e-10 and
Tests.Assertions.maxAbsVector(
eskfPredicted.quaternionWorldBody
- eskfSequential.quaternionWorldBody) < 2.0e-10 and
Tests.Assertions.maxAbsMatrix(
eskfPredicted.covariance - eskfSequential.covariance) < 2.0e-5,
"Packet prediction disagrees with ten sequential held-IMU covariance steps");
ukfInitial := Estimation.StrapdownINS.UKF.initialize(
zeros(3), zeros(3), {1.0, 0.0, 0.0, 0.0},
zeros(3), zeros(3), initialVariances);
(ukfPredicted, ukfSuccess) :=
Estimation.StrapdownINS.UKF.predictPreintegrated(
ukfInitial, imu, {0.0, 0.0, -9.81}, processNoise);
assert(ukfSuccess and
Tests.Assertions.maxAbsVector(
eskfPredicted.positionWorldEnu_m
- ukfPredicted.positionWorldEnu_m) < 2.0e-7 and
Tests.Assertions.maxAbsVector(
eskfPredicted.velocityWorldEnu_m_s
- ukfPredicted.velocityWorldEnu_m_s) < 2.0e-7 and
Tests.Assertions.maxAbsVector(
eskfPredicted.quaternionWorldBody
- ukfPredicted.quaternionWorldBody) < 2.0e-7,
"ESKF and UKF do not apply the same preintegrated nominal motion");
passed := true;
end run;
parameter Boolean passed = run();
equation
assert(passed, "Strapdown preintegration tests did not complete");
end StrapdownPreintegrationTests;