Function lupnt::InitialCovariancePosVelClock¶

Defined in File filter_utils.cc

Function Documentation¶

MatXd lupnt::InitialCovariancePosVelClock(double sigma_r, double sigma_v, double sigma_b, double sigma_d)¶

Build a block-diagonal initial covariance for a position/velocity/clock-bias/ clock-drift state: diag(sigma_r^2 * I3, sigma_v^2 * I3, sigma_b^2, sigma_d^2).

Used by application setup code to initialize a filter’s P0 (via Filter::SetCovariance) for the common 8-element [r(3), v(3), clock_bias, / clock_drift] state.

  @param sigma_r  Initial position 1-sigma uncertainty [m]
  @param sigma_v  Initial velocity 1-sigma uncertainty [m/s]
  @param sigma_b  Initial clock-bias 1-sigma uncertainty [s]
  @param sigma_d  Initial clock-drift 1-sigma uncertainty [s/s]
  @return         Initial covariance matrix, size [8 x 8]