Program Listing for File surface_rover_nav_app.h¶
↰ Return to documentation for file (applications/rover/surface_rover_nav_app.h)
#pragma once
#include <random>
#include <string>
#include <vector>
#include "lupnt/applications/application.h"
#include "lupnt/core/config.h"
#include "lupnt/core/definitions.h"
#include "lupnt/measurements/surface_measurements.h"
namespace lupnt {
class LunarNavConstellation; // relay-satellite provider agent (queried for truth states)
struct SurfaceRoverNavAppParams {
// ---- IMU (Kalibr) noise densities ----------------------------------------
double accel_noise_density = 3.4e-4;
double accel_bias_rw = 1.0e-4;
double gyro_noise_density = 3.4e-6;
double gyro_bias_rw = 1.0e-6;
// ---- Aiding measurement noise --------------------------------------------
double pseudorange_sigma_m = 1.0;
double sise_m = 3.0;
double dem_sigma_m = 5.0;
// ---- Clock process noise --------------------------------------------------
double clock_bias_process_sigma = 1.0e-11;
double clock_drift_process_sigma = 1.0e-13;
};
struct SurfaceNavConfig {
int seed = 42;
std::string start_epoch_utc = "2027-03-01T00:00:00";
double duration_s = 1800.0;
double dt_s = 1.0;
// ---- Site / DEM (site/half_width used only by the legacy free function; the agent-based
// app takes the DEM from the shared `World`. `dem_max_res_m` is also the terrain-slope
// finite-difference step and must match the World's `dem.max_res_m`). ------------------
double site_lat_deg = -89.45;
double site_lon_deg = 222.8;
double dem_half_width_m = 4000.0;
double dem_max_res_m = 20.0;
// ---- Rover truth path (local ENU tangent plane) ---------------------------
double rover_start_east_m = 0.0;
double rover_start_north_m = -250.0;
double rover_speed_mps = 2.0;
double rover_heading_deg = 0.0;
double rover_turn_rate_dps = 0.6;
double rover_weave_amplitude_deg = 0.0;
double rover_weave_period_s = 300.0;
double rover_clock_bias_s = 1.0e-6;
double rover_clock_drift_sps = 1.0e-11;
// ---- IMU (Kalibr noise model) --------------------------------------------
double accel_noise_density = 3.4e-4;
double accel_bias_rw = 1.0e-4;
double gyro_noise_density = 3.4e-6;
double gyro_bias_rw = 1.0e-6;
double accel_bias0 = 1.0e-2;
double gyro_bias0 = 5.0e-4;
// ---- Navigation constellation ---------------------------------------------
std::string constellation_name;
std::vector<LcrnsSatConfig> satellites;
double elevation_mask_deg = 5.0;
double pseudorange_sigma_m = 1.0;
double sise_m = 3.0;
// ---- Filter initialization & tuning --------------------------------------
double init_pos_sigma_m = 50.0;
double init_vel_sigma_mps = 0.1;
double init_att_sigma_deg = 2.0;
double init_accel_bias_sigma = 2.0e-2;
double init_gyro_bias_sigma = 1.0e-3;
double init_clock_bias_sigma_s = 1.0e-6;
double init_clock_drift_sigma_sps = 1.0e-9;
double dem_sigma_m = 5.0;
double filter_bias_rw_scale = 3.0;
bool enable_dem_constraint = true;
};
struct SurfaceNavResults {
std::string site_id;
std::string site_name;
double site_lat_deg = 0.0;
double site_lon_deg = 0.0;
// Terrain grid (native DEM projected meters), for plotting.
MatXd dem_x;
MatXd dem_y;
MatXd dem_elevation;
double dem_center_x = 0.0;
double dem_center_y = 0.0;
// Time series (length N).
VecXd time_s;
MatXd pos_err_enu;
MatXd pos_sigma_enu;
VecXd pos_err_norm;
VecXd clock_bias_err;
VecXd clock_bias_sigma;
VecXi n_visible;
// IMU estimation (body frame), length N.
MatXd accel_bias_err;
MatXd accel_bias_sigma;
MatXd gyro_bias_err;
MatXd gyro_bias_sigma;
MatXd att_err_deg;
MatXd att_sigma_deg;
MatXd rover_track_enu_truth;
MatXd rover_track_enu_est;
VecXd rover_alt_truth;
std::vector<std::string> satellite_names;
};
class SurfaceRoverNavApp : public Application {
public:
SurfaceRoverNavApp() = default;
explicit SurfaceRoverNavApp(const SurfaceRoverNavAppParams& params) : params_(params) {}
explicit SurfaceRoverNavApp(Config& config);
void Setup() override;
void Step(Real t) override;
void Log(Real t) override;
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);
void Predict(const SurfaceImuMeasurement& imu, double dt);
void UpdateLans(const std::vector<SurfaceLansMeasurement>& meas);
void UpdateScalar(const VecXd& H, double z_pred, double z_meas, double variance);
double time() const { return t_; }
const Vec3d& position() const { return r_; }
const Vec3d& velocity() const { return v_; }
const Mat3d& attitude() const { return R_b2n_; }
const Vec3d& accel_bias() const { return ba_; }
const Vec3d& gyro_bias() const { return bg_; }
double clock_bias() const { return cb_; }
double clock_drift() const { return cd_; }
const MatXd& covariance() const { return P_; }
const SurfaceRoverNavAppParams& params() const { return params_; }
// ---- Result series (valid after the Simulation has run) -------------------
const SurfaceNavConfig& config() const { return cfg_; }
const SurfaceNavResults& results() const { return res_; }
const std::string& site_id() const { return res_.site_id; }
const std::string& site_name() const { return res_.site_name; }
const MatXd& dem_x() const { return res_.dem_x; }
const MatXd& dem_y() const { return res_.dem_y; }
const MatXd& dem_elevation() const { return res_.dem_elevation; }
const VecXd& time_series() const { return res_.time_s; }
const MatXd& pos_err_enu() const { return res_.pos_err_enu; }
const MatXd& pos_sigma_enu() const { return res_.pos_sigma_enu; }
const VecXd& pos_err_norm() const { return res_.pos_err_norm; }
const VecXd& clock_bias_err() const { return res_.clock_bias_err; }
const VecXd& clock_bias_sigma() const { return res_.clock_bias_sigma; }
const VecXi& n_visible() const { return res_.n_visible; }
const MatXd& accel_bias_err() const { return res_.accel_bias_err; }
const MatXd& accel_bias_sigma() const { return res_.accel_bias_sigma; }
const MatXd& gyro_bias_err() const { return res_.gyro_bias_err; }
const MatXd& gyro_bias_sigma() const { return res_.gyro_bias_sigma; }
const MatXd& att_err_deg() const { return res_.att_err_deg; }
const MatXd& att_sigma_deg() const { return res_.att_sigma_deg; }
const MatXd& rover_track_enu_truth() const { return res_.rover_track_enu_truth; }
const MatXd& rover_track_enu_est() const { return res_.rover_track_enu_est; }
const VecXd& rover_alt_truth() const { return res_.rover_alt_truth; }
const std::vector<std::string>& satellite_names() const { return res_.satellite_names; }
private:
MatXd ProcessNoise(double dt) const;
void InjectErrorState(const VecXd& dx);
void InitScenario();
void LogEpoch(int k);
SurfaceRoverNavAppParams params_;
double t_ = 0.0;
// Nominal state.
Vec3d r_ = Vec3d::Zero();
Vec3d v_ = Vec3d::Zero();
Mat3d R_b2n_ = Mat3d::Identity();
Vec3d ba_ = Vec3d::Zero();
Vec3d bg_ = Vec3d::Zero();
double cb_ = 0.0;
double cd_ = 0.0;
MatXd P_; // error-state covariance
// ---- Self-driving (config-constructed) scenario state ---------------------
bool self_driving_ = false;
bool initialized_ = false;
SurfaceNavConfig cfg_;
SurfaceNavResults res_;
// RNG shared across Setup and Step so the draw order matches the legacy monolith exactly
// (a single mt19937 + single normal_distribution, whose Box-Muller cache must persist).
std::mt19937 rng_;
std::normal_distribution<double> nd_{0.0, 1.0};
double Gauss(double sigma) { return sigma * nd_(rng_); }
Vec3d Gauss3(double sigma) { return Vec3d(Gauss(sigma), Gauss(sigma), Gauss(sigma)); }
// Local ENU tangent frame (from the World), Moon-fixed geometry.
int N_ = 0;
int n_sat_ = 0;
LunarNavConstellation* nav_ = nullptr; // relay provider (resolved in InitScenario), or null
double dt_ = 1.0;
double ds_ = 1.0; // DEM finite-difference step [m]
Mat3d R_enu2pa_ = Mat3d::Identity();
Mat3d R_pa2enu_ = Mat3d::Identity();
Vec3d r_center_pa_ = Vec3d::Zero();
Vec3d up_hat_pa_ = Vec3d::UnitZ();
// Precomputed truth series (length N_).
std::vector<double> Ee_, Nn_, Uu_;
std::vector<Vec3d> r_truth_, v_truth_;
std::vector<Mat3d> R_truth_;
std::vector<Vec3d> f_body_truth_, w_body_truth_;
std::vector<Vec3d> ba_truth_k_, bg_truth_k_;
std::vector<std::vector<Vec3d>> sat_pa_; // [k][j] relay position, Moon-fixed [m]
std::vector<double> sise_bias_;
double ClockBiasTruth(int k) const {
return cfg_.rover_clock_bias_s + cfg_.rover_clock_drift_sps * (k * dt_);
}
};
} // namespace lupnt