model

SLAM.Inertial.ES15NominalPrediction

Public declarations

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

Role	Name	Type	Dimensions / attributes / default
input	`position`	`Real`	`[3] = {0.0,0.0,0.0}`
input	`velocity`	`Real`	`[3] = {0.0,0.0,0.0}`
input	`accel`	`Real`	`[3] = {0.0,0.0,9.81}`
input	`gyro`	`Real`	`[3] = {0.0,0.0,0.0}`
input	`accel_bias`	`Real`	`[3] = {0.0,0.0,0.0}`
input	`gyro_bias`	`Real`	`[3] = {0.0,0.0,0.0}`
input	`gravity`	`Real`	`[3] = {0.0,0.0,-9.81}`
input	`h`	`Real`	`= 1.0/90.0`
output	`force`	`Real`	`[3]`
output	`omega`	`Real`	`[3]`
output	`middle_rotation`	`Real`	`[3,3]`
output	`next_rotation`	`Real`	`[3,3]`
output	`next_position`	`Real`	`[3]`
output	`next_velocity`	`Real`	`[3]`
output	`valid`	`Real`

Members

Name	Kind	Description
Predict	function

Modelica declarations and implementation

Show parameters, interfaces and source

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

model ES15NominalPrediction
  pure function Predict
    input Real rotation[3,3] = identity(3);
    input Real position[3] = {0.0,0.0,0.0};
    input Real velocity[3] = {0.0,0.0,0.0};
    input Real accel[3] = {0.0,0.0,9.81};
    input Real gyro[3] = {0.0,0.0,0.0};
    input Real accel_bias[3] = {0.0,0.0,0.0};
    input Real gyro_bias[3] = {0.0,0.0,0.0};
    input Real gravity[3] = {0.0,0.0,-9.81};
    input Real h = 1.0/90.0;
    output Real force[3]; output Real omega[3];
    output Real middle_rotation[3,3]; output Real next_rotation[3,3];
    output Real next_position[3]; output Real next_velocity[3];
    output Real valid;
  protected
    Real angle; Real a; Real b; Real middle_a; Real middle_b;
    Real W[3,3]; Real W2[3,3];
    Real increment[3,3]; Real middle_increment[3,3]; Real proposed_rotation[3,3];
    Real acceleration[3];
  algorithm
    force := accel-accel_bias;
    omega := gyro-gyro_bias;
    angle := sqrt(omega[1]^2+omega[2]^2+omega[3]^2)*h;
    valid := if noEvent(h > 0.0 and h <= 0.02 and angle <= 0.1) then 1.0 else 0.0;
    // Protected denominators also keep unselected small-angle branches finite.
    a := if noEvent(angle < 1e-7) then 1.0 else sin(angle)/max(angle,1e-7);
    b := if noEvent(angle < 1e-7) then 0.5 else (1.0-cos(angle))/max(angle^2,1e-14);
    middle_a := if noEvent(angle*0.5 < 1e-7) then 1.0 else sin(angle*0.5)/max(angle*0.5,1e-7);
    middle_b := if noEvent(angle*0.5 < 1e-7) then 0.5 else (1.0-cos(angle*0.5))/max(angle^2*0.25,1e-14);
    W := [0.0,-omega[3]*h,omega[2]*h;
         omega[3]*h,0.0,-omega[1]*h;
         -omega[2]*h,omega[1]*h,0.0];
    W2 := W*W;
    increment := identity(3)+a*W+b*W2;
    middle_increment := identity(3)+middle_a*0.5*W+middle_b*0.25*W2;
    middle_rotation := rotation*middle_increment;
    proposed_rotation := rotation*increment;
    acceleration := middle_rotation*force+gravity;
    for i in 1:3 loop
      next_position[i] := if noEvent(valid > 0.5) then position[i]+velocity[i]*h+0.5*acceleration[i]*h^2 else position[i];
      next_velocity[i] := if noEvent(valid > 0.5) then velocity[i]+acceleration[i]*h else velocity[i];
      for j in 1:3 loop
        next_rotation[i,j] := if noEvent(valid > 0.5) then proposed_rotation[i,j] else rotation[i,j];
      end for;
    end for;
  end Predict;

  input Real rotation[3,3] = identity(3);
  input Real position[3] = {0.0,0.0,0.0};
  input Real velocity[3] = {0.0,0.0,0.0};
  input Real accel[3] = {0.0,0.0,9.81};
  input Real gyro[3] = {0.0,0.0,0.0};
  input Real accel_bias[3] = {0.0,0.0,0.0};
  input Real gyro_bias[3] = {0.0,0.0,0.0};
  input Real gravity[3] = {0.0,0.0,-9.81};
  input Real h = 1.0/90.0;
  output Real force[3]; output Real omega[3];
  output Real middle_rotation[3,3]; output Real next_rotation[3,3];
  output Real next_position[3]; output Real next_velocity[3];
  output Real valid;
algorithm
  (force,omega,middle_rotation,next_rotation,next_position,next_velocity,valid) :=
    Predict(rotation,position,velocity,accel,gyro,accel_bias,gyro_bias,gravity,h);
end ES15NominalPrediction;