block

Tests.VisualNavigationReplay

Sequential fixed-map visual and GPS aiding comparison

Public declarations

Explicit source declarations; dimensions, modifications and defaults are shown as written, without evaluation.

Role	Name	Type	Dimensions / attributes / default
input	`tightCoupling`	`Boolean`	`= false`
input	`previousNominal`	`Real`	`[16]`
input	`previousCovariance`	`Real`	`[15, 15]`
input	`angularVelocityBody_rad_s`	`Real`	`[3]`
input	`specificForceBody_m_s2`	`Real`	`[3]`
input	`interval_s`	`Real`
input	`imuNoiseDensity`	`Real`	`[2]`
input	`gpsAvailable`	`Boolean`
input	`cameraAvailable`	`Boolean`
input	`gpsPositionWorld_m`	`Real`	`[3]`
input	`gpsVelocityWorld_m_s`	`Real`	`[3]`
input	`gpsStandardDeviation`	`Real`	`[2]`
input	`landmarksWorld_m`	`Real`	`[4, 3]`
input	`observations`	`Real`	`[4, 3]`
input	`measurementCovariance`	`Real`	`[12, 12]`
input	`rotationBodyCamera`	`Real`	`[3, 3]`
input	`cameraPositionBody_m`	`Real`	`[3]`
input	`intrinsics`	`Real`	`[4]`
output	`nominal`	`Real`	`[16]`
output	`covariance`	`Real`	`[15, 15]`
output	`gpsAccepted`	`Boolean`
output	`cameraAccepted`	`Boolean`
output	`gpsNis`	`Real`
output	`cameraNis`	`Real`

Modelica declarations and implementation

Show parameters, interfaces and source

Parameters, inputs, outputs, defaults and equations from the source file.

block VisualNavigationReplay "Sequential fixed-map visual and GPS aiding comparison"
  input Boolean tightCoupling = false;
  input Real previousNominal[16];
  input Real previousCovariance[15, 15];
  input Real angularVelocityBody_rad_s[3];
  input Real specificForceBody_m_s2[3];
  input Real interval_s;
  input Real imuNoiseDensity[2];
  input Boolean gpsAvailable;
  input Boolean cameraAvailable;
  input Real gpsPositionWorld_m[3];
  input Real gpsVelocityWorld_m_s[3];
  input Real gpsStandardDeviation[2];
  input Real landmarksWorld_m[4, 3];
  input Real observations[4, 3];
  input Real measurementCovariance[12, 12];
  input Real rotationBodyCamera[3, 3];
  input Real cameraPositionBody_m[3];
  input Real intrinsics[4];
  output Real nominal[16];
  output Real covariance[15, 15];
  output Boolean gpsAccepted;
  output Boolean cameraAccepted;
  output Real gpsNis;
  output Real cameraNis;
protected
  Estimation.StrapdownINS.ESKF.State previous(
    positionWorldEnu_m=previousNominal[1:3],
    velocityWorldEnu_m_s=previousNominal[4:6],
    quaternionWorldBody=previousNominal[7:10],
    gyroscopeBiasBodyFlu_rad_s=previousNominal[11:13],
    accelerometerBiasBodyFlu_m_s2=previousNominal[14:16],
    covariance=previousCovariance, useSquareRootCovariance=false,
    covarianceRoot=zeros(15, 15), barometerBiasCrossCovariance=zeros(15),
    barometerBias_m=0.0, barometerBiasVariance_m2=0.0, useJointBarometerBias=false);
  Estimation.StrapdownINS.ProcessNoise processNoise(
    gyroscope_rad2_s=imuNoiseDensity[1]^2 * identity(3),
    accelerometer_m2_s3=imuNoiseDensity[2]^2 * identity(3),
    gyroscopeBias_rad2_s3=zeros(3, 3), accelerometerBias_m2_s5=zeros(3, 3));
  Avionics.GpsSample gps(
    valid=gpsAvailable, fresh=gpsAvailable, positionValid=gpsAvailable,
    velocityValid=gpsAvailable, timestamp_s=0.0, geodetic_deg_m=zeros(3),
    positionWorldEnu_m=gpsPositionWorld_m,
    velocityWorldEnu_m_s=gpsVelocityWorld_m_s,
    positionCovarianceWorld_m2=gpsStandardDeviation[1]^2 * identity(3),
    velocityCovarianceWorld_m2_s2=gpsStandardDeviation[2]^2 * identity(3));
  Estimation.StrapdownINS.ESKF.State predicted;
  Estimation.StrapdownINS.ESKF.State gpsCorrected;
  Estimation.StrapdownINS.ESKF.State cameraCorrected;
  Integer rejectionReason;
algorithm
  when sample(0.0, 0.01) then
    predicted := Estimation.StrapdownINS.ESKF.predict(previous,
      angularVelocityBody_rad_s, specificForceBody_m_s2,
      {0.0, 0.0, -9.81}, interval_s, processNoise);
    if gpsAvailable then
      (gpsCorrected, gpsAccepted, rejectionReason, gpsNis) :=
        Estimation.StrapdownINS.ESKF.correctGps(predicted, gps);
    else
      gpsCorrected := Estimation.StrapdownINS.ESKF.copyState(predicted);
      gpsAccepted := false;
      gpsNis := 0.0;
      rejectionReason := 0;
    end if;
    if cameraAvailable then
      if tightCoupling then
        (cameraCorrected, cameraAccepted, cameraNis) := SLAM.Fusion.correctMappedLandmarksTight(
          gpsCorrected, landmarksWorld_m, observations, measurementCovariance,
          rotationBodyCamera, cameraPositionBody_m, intrinsics);
      else
        (cameraCorrected, cameraAccepted, cameraNis) := SLAM.Fusion.correctMappedLandmarksLoose(
          gpsCorrected, landmarksWorld_m, observations, measurementCovariance,
          rotationBodyCamera, cameraPositionBody_m, intrinsics);
      end if;
    else
      cameraCorrected := Estimation.StrapdownINS.ESKF.copyState(gpsCorrected);
      cameraAccepted := false;
      cameraNis := 0.0;
    end if;
    nominal := cat(1, cameraCorrected.positionWorldEnu_m, cameraCorrected.velocityWorldEnu_m_s,
      cameraCorrected.quaternionWorldBody, cameraCorrected.gyroscopeBiasBodyFlu_rad_s,
      cameraCorrected.accelerometerBiasBodyFlu_m_s2);
    covariance := cameraCorrected.covariance;
  end when;
end VisualNavigationReplay;