function

SLAM.Inertial.SchmidtGraphPoseCorrection.RightJacobian

Public declarations

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

Role	Name	Type	Dimensions / attributes / default	Description
input	`vector`	`Real`	`[3]`
output	`jacobian`	`Real`	`[3,3]`

Modelica declarations and implementation

Show parameters, interfaces and source

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

function RightJacobian
    input Real vector[3]; output Real jacobian[3,3];
  protected
    Real square; Real angle; Real a; Real b; Real skew[3,3];
  algorithm
    square := sum(vector.^2); angle := sqrt(square);
    a := if square < 1e-8 then 0.5-square/24+square^2/720 else (1-cos(angle))/square;
    b := if square < 1e-8 then 1.0/6-square/120+square^2/5040 else (angle-sin(angle))/(square*angle);
    skew := RGBDUncertaintySkew(vector);
    jacobian := identity(3)-a*skew+b*(skew*skew);
  end RightJacobian;