function

SLAM.Inertial.ES15CovarianceStep

Public declarations

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

Role	Name	Type	Dimensions / attributes / default
input	`F`	`Real`	`[15,15]`
input	`G`	`Real`	`[15,12]`
input	`P`	`Real`	`[15,15]`
input	`dt`	`Real`
input	`density`	`Real`	`[12]`
output	`Phi`	`Real`	`[15,15]`
output	`Q`	`Real`	`[15,15]`
output	`predicted`	`Real`	`[15,15]`

Modelica declarations and implementation

Show parameters, interfaces and source

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

function ES15CovarianceStep
  import ES15TransitionNoise = SLAM.Inertial.ES15TransitionNoise;

  input Real F[15,15];
  input Real G[15,12];
  input Real P[15,15];
  input Real dt;
  input Real density[12];
  output Real Phi[15,15];
  output Real Q[15,15];
  output Real predicted[15,15];
protected
  Real propagated[15,15]; Real raw[15,15];
algorithm
  (Phi,Q) := ES15TransitionNoise(F,G,dt,density);
  propagated := Phi*P*transpose(Phi);
  raw := propagated+Q;
  predicted := 0.5*(raw+transpose(raw));
end ES15CovarianceStep;