within SLAM.Localization;
model RGBDInertialLocalizationStep
  import AdvanceRGBDLocalization = SLAM.Localization.AdvanceRGBDLocalization;
  import RGBDInertialLocalizationInterface = SLAM.Localization.RGBDInertialLocalizationInterface;

  extends RGBDInertialLocalizationInterface;
  input Real pixels[featureCapacity,2];
  input Real activeCount;
  input Real featureScore[featureCapacity] = zeros(featureCapacity);
algorithm
  (nextPosition,
    nextVelocity,
    nextRotation,
    nextAccelBias,
    nextGyroBias,
    nextCovariance,
    nextCrossCovariance,
    nextReferenceCovariance,
    nextReferencePosition,
    nextReferenceRotation,
    nextReferenceAvailable,
    nextReferenceEpoch,
    nextReferenceUsed,
    nextLastUsedEpoch,
    nextReferenceDescriptor,
    nextReferencePoint,
    nextReferenceEnabled,
    nextReferencePixels,
    nextReferenceCount,
    nextReferenceRgbCalibration,
    nextReferenceDepthCalibration,
    nextReferenceNoiseReferenceFx,
    nextReferenceDisparityNoise,
    nextReferenceBaseline,
    nextReferenceOpticalToBody,
    nextReferenceCameraOriginBody,
    predictionAccepted,
    observationAccepted,
    observationRejected,
    captureAccepted,
    captureRejected,
    imageReuseRejected,
    imagePairEligible,
    frameValid,
    referenceGeometryCompatible,
    visualValid,
    matchCount,
    uncertaintyRejectionReason,
    currentDescriptor,
    currentPoint,
    currentEnabled,
    currentCount,
    currentFromReference,
    currentFromReferenceTranslation,
    relativeCovariance,
    mapCandidatePoint,
    mapCandidateEnabled,
    mapCandidateCount,
    nextQuaternion,
    positionCovariance,
    attitudeCovariance,
    confidence,
    features,
    featureEnabled,
    trackingCurrentPixel,
    trackingReferencePixel,
    trackingEnabled) := AdvanceRGBDLocalization(
    rgb=rgb,
    depth=depth,
    rgbCalibration=rgbCalibration,
    depthCalibration=depthCalibration,
    disparityNoise=disparityNoise,
    noiseReferenceFx=noiseReferenceFx,
    baseline=baseline,
    opticalToBody=opticalToBody,
    cameraOriginBody=cameraOriginBody,
    frameEnabled=frameEnabled,
    imageCaptureRequested=imageCaptureRequested,
    position=position,
    velocity=velocity,
    rotation=rotation,
    accelBias=accelBias,
    gyroBias=gyroBias,
    covariance=covariance,
    crossCovariance=crossCovariance,
    referenceCovariance=referenceCovariance,
    referencePosition=referencePosition,
    referenceRotation=referenceRotation,
    referenceAvailable=referenceAvailable,
    referenceEpoch=referenceEpoch,
    currentEpoch=currentEpoch,
    referenceUsed=referenceUsed,
    lastUsedEpoch=lastUsedEpoch,
    referenceDescriptor=referenceDescriptor,
    referencePoint=referencePoint,
    referenceEnabled=referenceEnabled,
    referencePixels=referencePixels,
    referenceCount=referenceCount,
    referenceRgbCalibration=referenceRgbCalibration,
    referenceDepthCalibration=referenceDepthCalibration,
    referenceNoiseReferenceFx=referenceNoiseReferenceFx,
    referenceDisparityNoise=referenceDisparityNoise,
    referenceBaseline=referenceBaseline,
    referenceOpticalToBody=referenceOpticalToBody,
    referenceCameraOriginBody=referenceCameraOriginBody,
    accel=accel,
    gyro=gyro,
    gravity=gravity,
    h=h,
    density=density,
    pixels=pixels,
    activeCount=activeCount,
    featureScore=featureScore,depthUnits=depthUnits);
end RGBDInertialLocalizationStep;