Program Listing for File lander_nav_app.h¶
↰ Return to documentation for file (applications/lander/lander_nav_app.h)
#pragma once
#include <Eigen/Geometry>
#include <random>
#include <string>
#include <vector>
#include "lupnt/applications/application.h"
#include "lupnt/core/config.h"
#include "lupnt/core/definitions.h"
#include "lupnt/measurements/lander_measurements.h"
#include "lupnt/measurements/surface_measurements.h"
namespace lupnt {
class LanderGncApp; // co-hosted guidance app that owns the descent truth trajectory
struct LanderNavAppParams {
// ---- 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 altimeter_sigma_m = 2.0;
double crater_sigma_rad = 1.0e-3;
// ---- Clock process noise --------------------------------------------------
double clock_bias_process_sigma = 1.0e-11;
double clock_drift_process_sigma = 1.0e-13;
};
struct LanderNavConfig {
int seed = 42;
std::string start_epoch_utc = "2027-03-01T00:00:00";
double dt_s = 0.5;
// 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 FD step).
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;
// ---- Lander clock truth (estimated online by the filter) ------------------
// The descent truth trajectory itself is owned by the co-hosted `LanderGncApp`.
double lander_clock_bias_s = 1.0e-6;
double lander_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;
// ---- Radar altimeter ------------------------------------------------------
bool enable_altimeter = true;
double altimeter_sigma_m = 2.0;
double altimeter_max_range_m = 3000.0;
// ---- Crater-landmark camera (terrain-relative navigation) -----------------
bool enable_craters = true;
int n_craters = 60;
double crater_field_radius_m = 3000.0;
double camera_fov_deg = 30.0;
int max_craters_per_epoch = 6;
double crater_sigma_arcsec = 120.0;
// ---- LunaNet (LCRNS) constellation ----------------------------------------
bool enable_lunanet = true;
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 = 100.0;
double init_vel_sigma_mps = 1.0;
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 filter_bias_rw_scale = 3.0;
};
struct LanderNavResults {
std::string site_id;
std::string site_name;
double site_lat_deg = 0.0;
double site_lon_deg = 0.0;
MatXd dem_x;
MatXd dem_y;
MatXd dem_elevation;
double dem_center_x = 0.0;
double dem_center_y = 0.0;
MatXd crater_enu;
VecXd time_s;
MatXd pos_err_enu;
MatXd pos_sigma_enu;
VecXd pos_err_norm;
VecXd vel_err_norm;
VecXd clock_bias_err;
VecXd clock_bias_sigma;
VecXi n_visible_sat;
VecXi n_craters;
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 att_quat_est;
MatXd traj_enu_truth;
MatXd traj_enu_est;
VecXd alt_truth;
VecXd alt_est;
std::vector<std::string> satellite_names;
};
class LanderNavApp : public Application {
public:
LanderNavApp() = default;
explicit LanderNavApp(const LanderNavAppParams& params) : params_(params) {}
explicit LanderNavApp(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 UpdateAltimeter(const LanderAltimeterMeasurement& meas);
void UpdateCraters(const std::vector<LanderCraterMeasurement>& meas);
void UpdateCrater(const LanderCraterMeasurement& 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_; }
Mat3d attitude() const { return q_b2n_.toRotationMatrix(); }
Vec4d quaternion() const { return Vec4d(q_b2n_.w(), q_b2n_.x(), q_b2n_.y(), q_b2n_.z()); }
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 LanderNavAppParams& params() const { return params_; }
// ---- Result series (valid after the Simulation has run) -------------------
const LanderNavConfig& config() const { return cfg_; }
const LanderNavResults& 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 MatXd& crater_enu() const { return res_.crater_enu; }
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& vel_err_norm() const { return res_.vel_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_sat() const { return res_.n_visible_sat; }
const VecXi& n_craters() const { return res_.n_craters; }
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& att_quat_est() const { return res_.att_quat_est; }
const MatXd& traj_enu_truth() const { return res_.traj_enu_truth; }
const MatXd& traj_enu_est() const { return res_.traj_enu_est; }
const VecXd& alt_truth() const { return res_.alt_truth; }
const VecXd& alt_est() const { return res_.alt_est; }
const std::vector<std::string>& satellite_names() const { return res_.satellite_names; }
private:
MatXd ProcessNoise(double dt) const;
void InjectErrorState(const VecXd& dx);
void UpdateVector(const MatXd& H, const VecXd& y, const MatXd& R);
void InitScenario();
void LogEpoch(int k);
LanderNavAppParams params_;
double t_ = 0.0;
// Nominal state (attitude as a unit quaternion — the MEKF global orientation).
Vec3d r_ = Vec3d::Zero();
Vec3d v_ = Vec3d::Zero();
Eigen::Quaterniond q_b2n_ = Eigen::Quaterniond::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;
LanderNavConfig cfg_;
LanderNavResults res_;
std::mt19937 rng_;
std::normal_distribution<double> nd_{0.0, 1.0};
std::uniform_real_distribution<double> ud_{0.0, 1.0};
double Gauss(double sigma) { return sigma * nd_(rng_); }
Vec3d Gauss3(double sigma) { return Vec3d(Gauss(sigma), Gauss(sigma), Gauss(sigma)); }
int N_ = 0;
int n_sat_ = 0;
int n_crat_ = 0;
double dt_ = 0.5;
double ds_ = 1.0;
double crater_sigma_rad_ = 1.0e-3;
Mat3d R_enu2pa_ = Mat3d::Identity();
Mat3d R_pa2enu_ = Mat3d::Identity();
Vec3d r_center_pa_ = Vec3d::Zero();
Vec3d up_hat_pa_ = Vec3d::UnitZ();
// The descent truth trajectory is owned by the co-hosted guidance app; read through this
// handle (resolved in InitScenario). The nav app owns only the sensor/filter state below.
LanderGncApp* gnc_ = nullptr;
std::vector<Vec3d> ba_truth_k_, bg_truth_k_; // truth IMU biases (this app's sensor model)
std::vector<std::vector<Vec3d>> sat_pa_;
std::vector<double> sise_bias_;
std::vector<Vec3d> crater_pa_;
double ClockBiasTruth(int k) const {
return cfg_.lander_clock_bias_s + cfg_.lander_clock_drift_sps * (k * dt_);
}
};
} // namespace lupnt