Edit on GitHub

Dead Reckon Tether

Towards real-rime location prediction and model based target tracking. See brief description in this presentation.

Towards Real-Time Non-Gaussian SLAM from Dehann on Vimeo.

Functions to Use

See the related functions while this documentation is being expanded:

  • addVariable!(fg, :drt_0, ..., solvable=0)
  • drec1 = MutablePose2Pose2Gaussian(...)
  • addFactor!(dfg, [:x0; :drt_0], drec1, solvable=0, graphinit=false)
  • accumulateDiscreteLocalFrame!
  • accumulateFactorMeans
  • duplicateToStandardFactorVariable
RoME.duplicateToStandardFactorVariable — Function
duplicateToStandardFactorVariable(, mpp, dfg, prevsym, newsym; solvable, graphinit, cov)

Helper function to duplicate values from a special factor variable into standard factor and variable. Returns the name of the new factor.

Notes:

  • Developed for accumulating odometry in a MutablePosePose and then cloning out a standard PosePose and new variable.
  • Does not change the original MutablePosePose source factor or variable in any way.
  • Assumes timestampe from mpp object.

Related

addVariable!, addFactor!

RoME.accumulateDiscreteLocalFrame! — Function
accumulateDiscreteLocalFrame!(mpp, DX, Qc)
accumulateDiscreteLocalFrame!(mpp, DX, Qc, dt; Fk, Gk, Phik)

Advance an odometry factor as though integrating an ODE – i.e. $X_2 = X_1 ⊕ ΔX$. Accepts continuous domain process noise density Qc which is internally integrated to discrete process noise Qd. $DX$ is assumed to already be incrementally integrated before this function. See related accumulateContinuousLocalFrame! for fully continuous system propagation.

Notes

  • This update stays in the same reference frame but updates the local vector as though accumulating measurement values over time.
  • Kalman filter would have used for noise propagation: $Pk1 = F*Pk*F' + Qdk$
  • From Chirikjian, Vol.II, 2012, p.35: Jacobian SE(2), Jr = [cθ sθ 0; -sθ cθ 0; 0 0 1] – i.e. dSE2/dX' = SE2([0;0;-θ])
  • DX = dX/dt*Dt
  • assumed process noise for {}^b Qc = {}^b [x;y;yaw] = [fwd; sideways; rotation.rate]

Dev Notes

  • TODO many operations here can be done in-place.

Related

accumulateContinuousLocalFrame!, accumulateDiscreteReferenceFrame!, accumulateFactorMeans

IncrementalInference.accumulateFactorMeans — Function
accumulateFactorMeans(dfg, fctsyms)

Accumulate chains of binary factors–-potentially starting from a prior–-as a parameteric mean value only.

Notes

  • Not used during tree inference.
  • Expected uses are for user analysis of factors and estimates.
  • real-time dead reckoning chain prediction.

DevNotes

  • TODO consolidate with similar approxConvChain

Related:

approxConv, solveBinaryFactorParameteric, RoME.MutablePose2Pose2Gaussian

RoME.MutablePose2Pose2Gaussian — Type
mutable struct MutablePose2Pose2Gaussian <: AbstractRelativeRoots

Specialized Pose2Pose2 factor type (Gaussian), which allows for rapid accumulation of odometry information as a branch on the factor graph.