Class SurfaceRoverNavApp

• Defined in File surface_rover_nav_app.h

Inheritance Relationships

Base Type

• public lupnt::Application (Class Application)

Class Documentation

class SurfaceRoverNavApp : public lupnt::Application

Strapdown inertial-navigation error-state EKF that fuses a full IMU (accelerometer + gyroscope), LCRNS (LANS) pseudoranges, and a DEM altitude constraint to position and orient a surface rover, estimating the IMU biases online.

The nominal state is [r(3), v(3), R_b2n(SO3), b_a(3), b_g(3), clock_bias, clock_drift] in a Moon-fixed frame; the 17-element error state [dr, dv, dtheta, db_a, db_g, d(cb), d(cd)] (see kSurfaceNavErrorStateSize) carries the covariance. Each step it Predicts with an IMU sample (bias-corrected strapdown mechanization + Kalibr process noise), then applies scalar updates: one per visible LANS satellite (UpdateLans) and, optionally, a linearized DEM altitude constraint (UpdateScalar).

Hosted on a agent as its (attach via Agent::SetApplication). When constructed from a YAML application: block, the app is self-driving: Setup() (called by the Simulation once the agent’s World is available) precomputes the truth trajectory, IMU truth, relay orbits, and the perturbed initial estimate, seeds the filter, and schedules Step(t) at the IMU cadence; each Step synthesizes that epoch’s IMU and pseudorange measurements from the shared World (terrain, gravity, ENU frame) and runs the predict/update cycle, recording the truth/estimate/covariance series accessible from the result accessors below. The lower-level Configure/Predict/Update* core can also be driven directly (e.g. from a custom driver) via the params-struct constructor.

Public Functions

SurfaceRoverNavApp() = default

inline explicit SurfaceRoverNavApp(const SurfaceRoverNavAppParams &params)

explicit SurfaceRoverNavApp(Config &config)

Construct a self-driving app from the application: block of a Rover agent. Reads the scenario (SurfaceNavConfig) and IMU/measurement tuning; the site / DEM is taken from the shared World in Setup().

virtual void Setup() override

Precompute the truth trajectory, IMU truth, relay orbits, and perturbed initial estimate (in the same RNG draw order as the legacy RunSurfaceNav), seed the filter, log epoch 0, and schedule periodic Steps at the IMU cadence. Only active when the app was constructed from a YAML config; the source of terrain / gravity / ENU is GetWorld().

virtual void Step(Real t) override

One self-driving epoch: set the host agent’s truth state, synthesize + Predict the IMU sample, apply LANS pseudorange and DEM-constraint updates (same order as the legacy loop body), and record the result series. No-op for the params-struct driver.

virtual void Log(Real t) override

Log this application’s current state/diagnostics to the DataLogger.

Base implementation only emits a debug message. Called once after Setup() and thereafter from Agent::Log(t) (which forwards to application_->Log(time) each time the owning agent logs). Derived classes (e.g. LanderNavApp::Log) override this to additionally log their Filter’s state/covariance and any error metrics.

Parameters:

t – Current simulation time [s, since simulation epoch]

void Configure(double t0, const Vec3d &r0, const Vec3d &v0, const Mat3d &R0, const Vec3d &ba0, const Vec3d &bg0, double cb0, double cd0, const MatXd &P0)

Seed the filter nominal state and covariance.

void Predict(const SurfaceImuMeasurement &imu, double dt)

Predict to t = t_ + dt using a full IMU sample.

void UpdateLans(const std::vector<SurfaceLansMeasurement> &meas)

Apply a sequential scalar EKF update for each LANS pseudorange.

void UpdateScalar(const VecXd &H, double z_pred, double z_meas, double variance)

Generic scalar EKF update in the error state, immediately injecting the resulting correction into the nominal state (error-state reset).

inline double time() const

current filter epoch [s].

inline const Vec3d &position() const

estimated position [m].

inline const Vec3d &velocity() const

estimated velocity [m/s].

inline const Mat3d &attitude() const

estimated body-to-nav rotation.

inline const Vec3d &accel_bias() const

estimated accel bias [m/s^2].

inline const Vec3d &gyro_bias() const

estimated gyro bias [rad/s].

inline double clock_bias() const

estimated clock bias [s].

inline double clock_drift() const

estimated clock drift [s/s].

inline const MatXd &covariance() const

error-state covariance.

inline const SurfaceRoverNavAppParams &params() const

inline const SurfaceNavConfig &config() const

inline const SurfaceNavResults &results() const

inline const std::string &site_id() const

inline const std::string &site_name() const

inline const MatXd &dem_x() const

inline const MatXd &dem_y() const

inline const MatXd &dem_elevation() const

inline const VecXd &time_series() const

inline const MatXd &pos_err_enu() const

inline const MatXd &pos_sigma_enu() const

inline const VecXd &pos_err_norm() const

inline const VecXd &clock_bias_err() const

inline const VecXd &clock_bias_sigma() const

inline const VecXi &n_visible() const

inline const MatXd &accel_bias_err() const

inline const MatXd &accel_bias_sigma() const

inline const MatXd &gyro_bias_err() const

inline const MatXd &gyro_bias_sigma() const

inline const MatXd &att_err_deg() const

inline const MatXd &att_sigma_deg() const

inline const MatXd &rover_track_enu_truth() const

inline const MatXd &rover_track_enu_est() const

inline const VecXd &rover_alt_truth() const

inline const std::vector<std::string> &satellite_names() const