Vehicles.Cubs2.OuterLoopComponents.StateEstimator

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

block StateEstimator
  parameter Real dt(unit="s", min=1.0e-9) = 0.02;
  parameter Real filterCutoffHz(unit="Hz") = 10.0;
  parameter Real velocityFilterCutoffHz(unit="Hz") = 1.5;
  parameter Real accelFilterCutoffHz(unit="Hz") = 0.4;
  parameter Real freshnessDistance(unit="m", min=1.0e-12) = 1.0e-5
    "Pose-equivalent motion that gives a packet full correction weight";
  parameter Real attitudeFreshnessLength(unit="m") = 0.1
    "Length scale converting attitude motion into pose-equivalent motion";
  parameter Real positionCorrectionSpeedLimit(unit="m/s") = 10.0;
  parameter Real attitudeCorrectionRateLimit(unit="rad/s") = 4.0;
  constant Real pi = 3.141592653589793;

  input Real position_m[3];
  input Real euler_rad[3];
  input Real velocity_m_s[3];
  input Real eulerRate_rad_s[3];
  FlightStateOutput estimate(
    position_m(each start=0.0),
    euler_rad(each start=0.0),
    velocity_m_s(each start=0.0),
    speed(start=0.0),
    flightPathAngle(start=0.0),
    acceleration_m_s2(start=0.0),
    eulerRate_rad_s(each start=0.0));
  discrete output Real measurementWeight(start=0.0)
    "Numeric correction weight; zero for a repeated mocap packet";

protected
  discrete Real previousMeasurementPosition_m[3](each start=0.0);
  discrete Real previousMeasurementEuler_rad[3](each start=0.0);
  discrete Real previousMeasurementVelocity_m_s[3](each start=0.0);
  discrete Real previousMeasurementEulerRate_rad_s[3](each start=0.0);
  discrete Real measurementPositionDelta_m[3];
  discrete Real measurementEulerDelta_rad[3];
  discrete Real measurementVelocityDelta_m_s[3];
  discrete Real measurementEulerRateDelta_rad_s[3];
  discrete Real predictedPosition_m[3];
  discrete Real predictedEuler_rad[3];
  discrete Real positionInnovation_m[3];
  discrete Real attitudeInnovation_rad[3];
  discrete Real sampleMotion_m;
  discrete Real positionInnovationWeight;
  discrete Real attitudeInnovationWeight;
  discrete Real measuredSpeed;
  discrete Real measuredFlightPathAngle;
  discrete Real filterSampleWeight;
  discrete Real velocitySampleWeight;
  discrete Real accelSampleWeight;
  discrete Real rawAcceleration_m_s2;

algorithm
  when sample(0.0, dt) then
    filterSampleWeight := 1.0 - exp(-2.0 * pi * filterCutoffHz * dt);
    velocitySampleWeight := 1.0 - exp(-2.0 * pi * velocityFilterCutoffHz * dt);
    accelSampleWeight := 1.0 - exp(-2.0 * pi * accelFilterCutoffHz * dt);

    measurementPositionDelta_m :=
      position_m - pre(previousMeasurementPosition_m);
    measurementVelocityDelta_m_s :=
      velocity_m_s - pre(previousMeasurementVelocity_m_s);
    for axis in 1:3 loop
      measurementEulerDelta_rad[axis] := MathUtilities.wrapAngle(
        euler_rad[axis] - pre(previousMeasurementEuler_rad[axis]));
      measurementEulerRateDelta_rad_s[axis] :=
        eulerRate_rad_s[axis] - pre(previousMeasurementEulerRate_rad_s[axis]);
    end for;
    sampleMotion_m :=
      MathUtilities.norm3(measurementPositionDelta_m)
      + attitudeFreshnessLength * MathUtilities.norm3(measurementEulerDelta_rad)
      + dt * MathUtilities.norm3(measurementVelocityDelta_m_s)
      + dt * attitudeFreshnessLength
        * MathUtilities.norm3(measurementEulerRateDelta_rad_s);
    measurementWeight :=
      MathUtilities.clip(sampleMotion_m / freshnessDistance, 0.0, 1.0);

    predictedPosition_m :=
      pre(estimate.position_m) + dt * pre(estimate.velocity_m_s);
    for axis in 1:3 loop
      predictedEuler_rad[axis] := MathUtilities.wrapAngle(
        pre(estimate.euler_rad[axis])
          + dt * pre(estimate.eulerRate_rad_s[axis]));
      attitudeInnovation_rad[axis] := MathUtilities.wrapAngle(
        euler_rad[axis] - predictedEuler_rad[axis]);
    end for;
    positionInnovation_m := position_m - predictedPosition_m;
    positionInnovationWeight := MathUtilities.clip(
      positionCorrectionSpeedLimit * dt
        / max(MathUtilities.norm3(positionInnovation_m), 1.0e-12),
      0.0,
      1.0);
    attitudeInnovationWeight := MathUtilities.clip(
      attitudeCorrectionRateLimit * dt
        / max(MathUtilities.norm3(attitudeInnovation_rad), 1.0e-12),
      0.0,
      1.0);

    estimate.position_m := predictedPosition_m
      + filterSampleWeight * measurementWeight
        * positionInnovationWeight * positionInnovation_m;
    for axis in 1:3 loop
      estimate.euler_rad[axis] := MathUtilities.wrapAngle(
        predictedEuler_rad[axis]
          + filterSampleWeight * measurementWeight
            * attitudeInnovationWeight * attitudeInnovation_rad[axis]);
    end for;
    estimate.velocity_m_s := MathUtilities.lowPass3(
      velocity_m_s,
      pre(estimate.velocity_m_s),
      velocitySampleWeight * measurementWeight);
    estimate.eulerRate_rad_s := MathUtilities.lowPass3(
      eulerRate_rad_s,
      pre(estimate.eulerRate_rad_s),
      filterSampleWeight * measurementWeight);

    measuredSpeed := MathUtilities.norm3(estimate.velocity_m_s);
    measuredFlightPathAngle := asin(MathUtilities.clip(
      estimate.velocity_m_s[3] / max(measuredSpeed, 1.0e-5), -1.0, 1.0));
    estimate.speed := measuredSpeed;
    estimate.flightPathAngle := measuredFlightPathAngle;
    rawAcceleration_m_s2 := (estimate.speed - pre(estimate.speed)) / dt;
    estimate.acceleration_m_s2 := MathUtilities.lowPass(
      rawAcceleration_m_s2,
      pre(estimate.acceleration_m_s2),
      accelSampleWeight);

    previousMeasurementPosition_m := position_m;
    previousMeasurementEuler_rad := euler_rad;
    previousMeasurementVelocity_m_s := velocity_m_s;
    previousMeasurementEulerRate_rad_s := eulerRate_rad_s;
  end when;
end StateEstimator;