From cd266aa15ce401fb9e5900ee8a331f89070da55f Mon Sep 17 00:00:00 2001 From: jtran34-ctrl Date: Wed, 25 Mar 2026 20:24:21 -0700 Subject: [PATCH 1/3] Integrated WMM2025 model and updated magnetic field pipeline --- CMakeLists.txt | 28 +-- README_WMM_UPDATE.txt | 28 +++ include/frames.hpp | 119 +++++++++++ include/math.hpp | 157 +++++++++++++++ include/modules.hpp | 24 +++ include/params.hpp | 20 +- include/quaternion.hpp | 103 ++++++++++ include/rng.hpp | 20 ++ include/sim_context.hpp | 80 ++++++++ include/wmm.hpp | 20 ++ plot_all.py | 89 +++++++++ reference/README-WMM-COEFS.txt | 70 +++++++ reference/WMM.COF | 93 +++++++++ reference/WMM2025.COF | 93 +++++++++ reference/WMM2025_TestValues.txt | 118 +++++++++++ src/control.cpp | 26 +++ src/disturbance.cpp | 44 +++++ src/main.cpp | 197 +++++++++++++------ src/navigation.cpp | 36 ++++ src/satellite.cpp | 274 ++++++++++++++++++-------- src/sensor.cpp | 45 +++++ src/wmm.cpp | 326 +++++++++++++++++++++++++++++++ 22 files changed, 1838 insertions(+), 172 deletions(-) create mode 100644 README_WMM_UPDATE.txt create mode 100644 include/frames.hpp create mode 100644 include/math.hpp create mode 100644 include/modules.hpp create mode 100644 include/quaternion.hpp create mode 100644 include/rng.hpp create mode 100644 include/sim_context.hpp create mode 100644 include/wmm.hpp create mode 100644 plot_all.py create mode 100644 reference/README-WMM-COEFS.txt create mode 100644 reference/WMM.COF create mode 100644 reference/WMM2025.COF create mode 100644 reference/WMM2025_TestValues.txt create mode 100644 src/control.cpp create mode 100644 src/disturbance.cpp create mode 100644 src/navigation.cpp create mode 100644 src/sensor.cpp create mode 100644 src/wmm.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index b67005a..8635ee7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,16 +1,22 @@ -cmake_minimum_required(VERSION 3.10) -project(MyProject LANGUAGES CXX) +cmake_minimum_required(VERSION 3.16) +# Small C++ project for the ADCS simulation. +project(adcs_faithful LANGUAGES CXX) + +# Keep the compiler settings simple and explicit. set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) -find_package(GeographicLib REQUIRED) # required for magentic field model - -# Source files -file(GLOB SOURCES "src/*.cpp") - -add_executable(${PROJECT_NAME} ${SOURCES}) - -target_include_directories(${PROJECT_NAME} PRIVATE include) +# Build one executable from the core simulation modules. +add_executable(adcs + src/main.cpp + src/satellite.cpp + src/control.cpp + src/sensor.cpp + src/navigation.cpp + src/disturbance.cpp + src/wmm.cpp +) -target_link_libraries(${PROJECT_NAME} PRIVATE GeographicLib::GeographicLib) # required for magentic field model \ No newline at end of file +# All headers live in the local include folder. +target_include_directories(adcs PRIVATE include) diff --git a/README_WMM_UPDATE.txt b/README_WMM_UPDATE.txt new file mode 100644 index 0000000..e18af4d --- /dev/null +++ b/README_WMM_UPDATE.txt @@ -0,0 +1,28 @@ +This package updates the earlier magnetorquer-only ADCS simulation to use WMM2025. + +What changed +- Replaced the old placeholder magnetic model with a real WMM2025 spherical-harmonic model. +- Added ECI <-> ECEF rotation so the field is evaluated in an Earth-fixed frame. +- Added ECEF -> geodetic latitude / longitude / altitude conversion on WGS84. +- Kept the rest of the closed-loop flow the same: sensors -> nav filter -> controller -> magnetorquer torque. +- Kept comments plain and direct so the code is easier to follow. + +Field pipeline +1. Orbit state is propagated in an inertial frame (ECI-like). +2. Position is rotated into ECEF using Earth rotation. +3. ECEF position is converted to geodetic lat/lon/alt. +4. WMM2025 returns local North/East/Down magnetic field in nanoTesla. +5. That local field is turned into an ECEF vector, then rotated back to ECI, then into the body frame. +6. The body-frame field is stored in Tesla for torque calculations. + +Build +- Open a terminal in the folder that contains CMakeLists.txt. +- Run: cmake -S . -B build +- Run: cmake --build build +- Run: .\build\adcs.exe (Windows) +- Then run: python .\plot_all.py + +Notes +- This is a much better magnetic environment model than the old placeholder, but it is still not a full flight-quality simulator. +- The disturbance and sensor models are still simple. +- The navigation block is still just a smoothing filter, not a real estimator. \ No newline at end of file diff --git a/include/frames.hpp b/include/frames.hpp new file mode 100644 index 0000000..96c9488 --- /dev/null +++ b/include/frames.hpp @@ -0,0 +1,119 @@ +#pragma once +#include "math.hpp" +#include + +// 3-2-1 Euler rotation matrix. +// This maps a vector from body coordinates to inertial coordinates. +inline Mat3 TIB(double phi, double theta, double psi) { + const double ct = std::cos(theta); + const double st = std::sin(theta); + const double sp = std::sin(phi); + const double cp = std::cos(phi); + const double ss = std::sin(psi); + const double cs = std::cos(psi); + + Mat3 out{}; + out[0][0] = ct * cs; + out[0][1] = sp * st * cs - cp * ss; + out[0][2] = cp * st * cs + sp * ss; + + out[1][0] = ct * ss; + out[1][1] = sp * st * ss + cp * cs; + out[1][2] = cp * st * ss - sp * cs; + + out[2][0] = -st; + out[2][1] = sp * ct; + out[2][2] = cp * ct; + return out; +} + +// Small helper used by some of the old code. +inline Mat3 Rscrew(const Vec3& nhat) { + const double x = nhat.x; + const double y = nhat.y; + const double z = nhat.z; + const double psi = std::atan2(y, x); + const double theta = std::atan2(z, std::sqrt(x * x + y * y)); + const double phi = 0.0; + return transpose(TIB(phi, theta, psi)); +} + +// Rotate a vector about the Earth Z axis. +inline Mat3 Rz(double theta) { + const double c = std::cos(theta); + const double s = std::sin(theta); + Mat3 R{}; + R[0][0] = c; R[0][1] = -s; R[0][2] = 0.0; + R[1][0] = s; R[1][1] = c; R[1][2] = 0.0; + R[2][0] = 0.0;R[2][1] = 0.0;R[2][2] = 1.0; + return R; +} + +// Convert an inertial vector into Earth-fixed coordinates. +inline Vec3 eci_to_ecef(const Vec3& r_eci, double earth_angle_rad) { + return mul(Rz(earth_angle_rad), r_eci); +} + +// Convert an Earth-fixed vector back into inertial coordinates. +inline Vec3 ecef_to_eci(const Vec3& v_ecef, double earth_angle_rad) { + return mul(Rz(-earth_angle_rad), v_ecef); +} + +// Simple geodetic container. +struct GeodeticLLA { + double lat_rad{0.0}; + double lon_rad{0.0}; + double alt_m{0.0}; +}; + +// Convert ECEF position to geodetic latitude, longitude, and height above WGS84. +inline GeodeticLLA ecef_to_geodetic_wgs84(const Vec3& r_ecef) { + const double a = 6378137.0; + const double f = 1.0 / 298.257223563; + const double e2 = f * (2.0 - f); + + const double x = r_ecef.x; + const double y = r_ecef.y; + const double z = r_ecef.z; + + const double lon = std::atan2(y, x); + const double p = std::sqrt(x * x + y * y); + + if (p < 1e-9) { + const double lat = (z >= 0.0) ? (M_PI / 2.0) : (-M_PI / 2.0); + const double alt = std::abs(z) - a * std::sqrt(1.0 - e2); + return {lat, lon, alt}; + } + + double lat = std::atan2(z, p * (1.0 - e2)); + double N = 0.0; + double alt = 0.0; + + for (int k = 0; k < 6; ++k) { + const double s = std::sin(lat); + N = a / std::sqrt(1.0 - e2 * s * s); + alt = p / std::cos(lat) - N; + lat = std::atan2(z, p * (1.0 - e2 * (N / (N + alt)))); + } + + return {lat, lon, alt}; +} + +// Local NED basis written in ECEF components. +// Each column of the matrix is one unit vector: [North East Down]. +inline Mat3 ned_basis_ecef(double lat_rad, double lon_rad) { + const double sphi = std::sin(lat_rad); + const double cphi = std::cos(lat_rad); + const double slam = std::sin(lon_rad); + const double clam = std::cos(lon_rad); + + const Vec3 north{-sphi * clam, -sphi * slam, cphi}; + const Vec3 east {-slam, clam, 0.0}; + const Vec3 down {-cphi * clam, -cphi * slam, -sphi}; + + Mat3 A{}; + A[0][0] = north.x; A[1][0] = north.y; A[2][0] = north.z; + A[0][1] = east.x; A[1][1] = east.y; A[2][1] = east.z; + A[0][2] = down.x; A[1][2] = down.y; A[2][2] = down.z; + return A; +} diff --git a/include/math.hpp b/include/math.hpp new file mode 100644 index 0000000..2452f4e --- /dev/null +++ b/include/math.hpp @@ -0,0 +1,157 @@ +#pragma once +#include +#include +#include + +// Simple 3D vector type used everywhere in the sim. +// This is intentionally tiny so the math stays easy to follow. +struct Vec3 { + double x{0.0}; + double y{0.0}; + double z{0.0}; + + Vec3() = default; + Vec3(double x_, double y_, double z_) : x(x_), y(y_), z(z_) {} + + // Allow v[0], v[1], v[2] access for loops. + double& operator[](size_t i) { return (i == 0) ? x : (i == 1) ? y : z; } + double operator[](size_t i) const { return (i == 0) ? x : (i == 1) ? y : z; } +}; + +inline Vec3 operator+(const Vec3& a, const Vec3& b) { return {a.x + b.x, a.y + b.y, a.z + b.z}; } +inline Vec3 operator-(const Vec3& a, const Vec3& b) { return {a.x - b.x, a.y - b.y, a.z - b.z}; } +inline Vec3 operator*(double s, const Vec3& v) { return {s * v.x, s * v.y, s * v.z}; } +inline Vec3 operator*(const Vec3& v, double s) { return s * v; } +inline Vec3 operator/(const Vec3& v, double s) { return {v.x / s, v.y / s, v.z / s}; } + +inline double dot(const Vec3& a, const Vec3& b) { + return a.x * b.x + a.y * b.y + a.z * b.z; +} + +inline Vec3 cross(const Vec3& a, const Vec3& b) { + return { + a.y * b.z - a.z * b.y, + a.z * b.x - a.x * b.z, + a.x * b.y - a.y * b.x + }; +} + +inline double norm(const Vec3& v) { + return std::sqrt(dot(v, v)); +} + +inline Vec3 normalize(const Vec3& v) { + const double n = norm(v); + if (n == 0.0) { + return {0.0, 0.0, 0.0}; + } + return v / n; +} + +// 3x3 matrix stored row-by-row. +struct Mat3 { + double m[3][3]{}; + double* operator[](size_t r) { return m[r]; } + const double* operator[](size_t r) const { return m[r]; } +}; + +inline Mat3 eye3() { + Mat3 I{}; + I[0][0] = 1.0; + I[1][1] = 1.0; + I[2][2] = 1.0; + return I; +} + +inline Mat3 transpose(const Mat3& A) { + Mat3 T{}; + for (int r = 0; r < 3; ++r) { + for (int c = 0; c < 3; ++c) { + T[r][c] = A[c][r]; + } + } + return T; +} + +inline Vec3 mul(const Mat3& A, const Vec3& v) { + return { + A[0][0] * v.x + A[0][1] * v.y + A[0][2] * v.z, + A[1][0] * v.x + A[1][1] * v.y + A[1][2] * v.z, + A[2][0] * v.x + A[2][1] * v.y + A[2][2] * v.z + }; +} + +inline Mat3 mul(const Mat3& A, const Mat3& B) { + Mat3 C{}; + for (int r = 0; r < 3; ++r) { + for (int c = 0; c < 3; ++c) { + double s = 0.0; + for (int k = 0; k < 3; ++k) { + s += A[r][k] * B[k][c]; + } + C[r][c] = s; + } + } + return C; +} + +inline Mat3 add(const Mat3& A, const Mat3& B) { + Mat3 C{}; + for (int r = 0; r < 3; ++r) { + for (int c = 0; c < 3; ++c) { + C[r][c] = A[r][c] + B[r][c]; + } + } + return C; +} + +inline Mat3 scale(const Mat3& A, double s) { + Mat3 C{}; + for (int r = 0; r < 3; ++r) { + for (int c = 0; c < 3; ++c) { + C[r][c] = A[r][c] * s; + } + } + return C; +} + +// Skew-symmetric matrix used for cross-product style formulas. +inline Mat3 skew(const Vec3& v) { + Mat3 S{}; + S[0][1] = -v.z; + S[0][2] = v.y; + S[1][0] = v.z; + S[1][2] = -v.x; + S[2][0] = -v.y; + S[2][1] = v.x; + return S; +} + +// Plain 3x3 inverse. This assumes the matrix really is invertible. +inline Mat3 inv3(const Mat3& A) { + const double a = A[0][0], b = A[0][1], c = A[0][2]; + const double d = A[1][0], e = A[1][1], f = A[1][2]; + const double g = A[2][0], h = A[2][1], i = A[2][2]; + + const double A11 = (e * i - f * h); + const double A12 = -(d * i - f * g); + const double A13 = (d * h - e * g); + const double A21 = -(b * i - c * h); + const double A22 = (a * i - c * g); + const double A23 = -(a * h - b * g); + const double A31 = (b * f - c * e); + const double A32 = -(a * f - c * d); + const double A33 = (a * e - b * d); + + const double det = a * A11 + b * A12 + c * A13; + if (std::abs(det) < 1e-30) { + throw std::runtime_error("inv3: singular matrix"); + } + + const double invdet = 1.0 / det; + Mat3 inv{}; + inv[0][0] = A11 * invdet; inv[0][1] = A21 * invdet; inv[0][2] = A31 * invdet; + inv[1][0] = A12 * invdet; inv[1][1] = A22 * invdet; inv[1][2] = A32 * invdet; + inv[2][0] = A13 * invdet; inv[2][1] = A23 * invdet; inv[2][2] = A33 * invdet; + return inv; +} diff --git a/include/modules.hpp b/include/modules.hpp new file mode 100644 index 0000000..d765a56 --- /dev/null +++ b/include/modules.hpp @@ -0,0 +1,24 @@ +#pragma once +#include "sim_context.hpp" +#include + +// State vector layout used everywhere in the sim. +// [x y z xdot ydot zdot q0 q1 q2 q3 p q r] +using State13 = std::array; + +// Core rigid-body/orbit dynamics. +State13 satellite_derivatives(double t, const State13& state, SimContext& ctx); + +// Sensor and navigation pieces. +void sensor_update(Vec3& BB, Vec3& pqr, Vec3& ptp, SimContext& ctx); +void navigation_update(const Vec3& BfieldMeasured, const Vec3& pqrMeasured, const Vec3& ptpMeasured, SimContext& ctx); + +// Magnetorquer controller. +void control_compute(const Vec3& BfieldNav, const Vec3& pqrNav, const Vec3& ptpNav, + SimContext& ctx, Vec3& current_out); + +// Disturbance model. +void disturbance(double altitude_m, double Amax, double lmax, const Vec3& vel, double CD, const Vec3& BI_Tesla, + Vec3& XYZD_out, Vec3& LMND_out); + +double density_kg_m3(double altitude_m); diff --git a/include/params.hpp b/include/params.hpp index aff1b8b..ab9366e 100644 --- a/include/params.hpp +++ b/include/params.hpp @@ -1,18 +1,6 @@ #pragma once -namespace params{ - // general parameters - constexpr double pi = 3.141592653589793; - constexpr double timeStep = 0.5; - - // satellite/orbit parameters - constexpr int mass = 3; // kg - constexpr int altitude = 500000; // meters - constexpr double inclination = 0.0 * (pi)/ 180; // radians - - // planet parameters - constexpr int R = 6371000; // meters - constexpr double M = 5.972e24; // kg - constexpr double G = 6.67e-11; //some SI units - constexpr double mu = M * G; -} \ No newline at end of file +namespace SimParams { + // Integration step used by the RK4 solver. + constexpr double timestep = 0.1; +} diff --git a/include/quaternion.hpp b/include/quaternion.hpp new file mode 100644 index 0000000..cc96853 --- /dev/null +++ b/include/quaternion.hpp @@ -0,0 +1,103 @@ +#pragma once +#include "math.hpp" +#include + +// Scalar-first quaternion. +// q0 is the scalar term, q1/q2/q3 are the vector terms. +struct Quat { + double q0{1.0}; + double q1{0.0}; + double q2{0.0}; + double q3{0.0}; +}; + +// Keep a quaternion on the unit sphere. +inline Quat normalize(const Quat& q) { + const double n = std::sqrt(q.q0 * q.q0 + q.q1 * q.q1 + q.q2 * q.q2 + q.q3 * q.q3); + if (n == 0.0) { + return {1.0, 0.0, 0.0, 0.0}; + } + return {q.q0 / n, q.q1 / n, q.q2 / n, q.q3 / n}; +} + +// Rotation matrix that maps a vector from body coordinates to inertial coordinates. +inline Mat3 TIBquat(const Quat& q_) { + const Quat q = q_; + const double q0 = q.q0; + const double q1 = q.q1; + const double q2 = q.q2; + const double q3 = q.q3; + + const double q0s = q0 * q0; + const double q1s = q1 * q1; + const double q2s = q2 * q2; + const double q3s = q3 * q3; + + Mat3 R{}; + R[0][0] = q0s + q1s - q2s - q3s; + R[0][1] = 2.0 * (q1 * q2 - q0 * q3); + R[0][2] = 2.0 * (q0 * q2 + q1 * q3); + + R[1][0] = 2.0 * (q1 * q2 + q0 * q3); + R[1][1] = q0s - q1s + q2s - q3s; + R[1][2] = 2.0 * (q2 * q3 - q0 * q1); + + R[2][0] = 2.0 * (q1 * q3 - q0 * q2); + R[2][1] = 2.0 * (q0 * q1 + q2 * q3); + R[2][2] = q0s - q1s - q2s + q3s; + return R; +} + +// Convert 3-2-1 Euler angles to a quaternion. +inline Quat euler321_to_quat(const Vec3& ptp) { + const double phi = ptp.x; + const double theta = ptp.y; + const double psi = ptp.z; + + const double q0 = std::cos(phi / 2.0) * std::cos(theta / 2.0) * std::cos(psi / 2.0) + + std::sin(phi / 2.0) * std::sin(theta / 2.0) * std::sin(psi / 2.0); + const double q1 = std::sin(phi / 2.0) * std::cos(theta / 2.0) * std::cos(psi / 2.0) + - std::cos(phi / 2.0) * std::sin(theta / 2.0) * std::sin(psi / 2.0); + const double q2 = std::cos(phi / 2.0) * std::sin(theta / 2.0) * std::cos(psi / 2.0) + + std::sin(phi / 2.0) * std::cos(theta / 2.0) * std::sin(psi / 2.0); + const double q3 = std::cos(phi / 2.0) * std::cos(theta / 2.0) * std::sin(psi / 2.0) + - std::sin(phi / 2.0) * std::sin(theta / 2.0) * std::cos(psi / 2.0); + + return {q0, q1, q2, q3}; +} + +// Convert a quaternion back to 3-2-1 Euler angles. +inline Vec3 quat_to_euler321(const Quat& q) { + const double q0 = q.q0; + const double q1 = q.q1; + const double q2 = q.q2; + const double q3 = q.q3; + + const double phi = std::atan2(2.0 * (q0 * q1 + q2 * q3), 1.0 - 2.0 * (q1 * q1 + q2 * q2)); + + double arg = 2.0 * (q0 * q2 - q3 * q1); + if (arg > 1.0) { + arg = 1.0; + } + if (arg < -1.0) { + arg = -1.0; + } + + const double theta = std::asin(arg); + const double psi = std::atan2(2.0 * (q0 * q3 + q1 * q2), 1.0 - 2.0 * (q2 * q2 + q3 * q3)); + return {phi, theta, psi}; +} + +// Standard rigid-body quaternion kinematics for body rates [p, q, r]. +inline Quat quat_derivative(const Quat& q, const Vec3& omega_body) { + const double p = omega_body.x; + const double qv = omega_body.y; + const double r = omega_body.z; + + Quat qdot{}; + qdot.q0 = -0.5 * ( p * q.q1 + qv * q.q2 + r * q.q3 ); + qdot.q1 = 0.5 * ( p * q.q0 + r * q.q2 - qv * q.q3 ); + qdot.q2 = 0.5 * ( qv * q.q0 - r * q.q1 + p * q.q3 ); + qdot.q3 = 0.5 * ( r * q.q0 + qv * q.q1 - p * q.q2 ); + return qdot; +} diff --git a/include/rng.hpp b/include/rng.hpp new file mode 100644 index 0000000..faf1f88 --- /dev/null +++ b/include/rng.hpp @@ -0,0 +1,20 @@ +#pragma once +#include + +// Small wrapper around the C++ random generator. +// The default fixed seed makes runs repeatable, which is handy for debugging. +struct Rng { + std::mt19937_64 gen; + std::uniform_real_distribution unif; + + explicit Rng(uint64_t seed = 1) : gen(seed), unif(0.0, 1.0) {} + + double rand01() { + return unif(gen); + } + + // Match the common MATLAB pattern (2*rand()-1). + double rand_m11() { + return 2.0 * rand01() - 1.0; + } +}; diff --git a/include/sim_context.hpp b/include/sim_context.hpp new file mode 100644 index 0000000..ee31fea --- /dev/null +++ b/include/sim_context.hpp @@ -0,0 +1,80 @@ +#pragma once +#include "math.hpp" +#include "quaternion.hpp" +#include "rng.hpp" + +// All long-lived simulation state that needs to be shared between modules lives here. +// It is not the prettiest pattern, but it keeps this small codebase easy to move around. +struct SimContext { + // Earth constants used by the orbit model. + double R{6.371e6}; + double M{5.972e24}; + double G{6.67e-11}; + double mu{0.0}; + + // Earth rotation used for the ECI <-> ECEF conversion. + double omegaE{7.2921150e-5}; + double greenwichAngle0{0.0}; + + // Start date for the geomagnetic model. + // One orbit is only a few hours, so the decimal year barely changes, but it is still handled correctly. + double decimalYear0{2026.0}; + + // Spacecraft geometry and mass. + double ms{2.6}; + double lx{0.10}; + double ly{0.10}; + double lz{0.20}; + double Amax{0.0}; + double lmax{0.0}; + double CD{1.0}; + double m{0.0}; + + // Spacecraft inertia. + Mat3 Is{}; + Mat3 I{}; + Mat3 invI{}; + + // Magnetorquer settings. + double n_turns{84.0}; + double A_turn{0.02}; + double maxCurrent_mA{120.0}; + Vec3 current{0.0, 0.0, 0.0}; + + // Update timing for field, sensor, and nav logic. + double nextMagUpdate{1.0}; + double lastMagUpdate{0.0}; + double nextSensorUpdate{1.0}; + double lastSensorUpdate{0.0}; + + // Sensor model settings and one-time bias values. + bool sensorModelInitialized{false}; + double fsensor{1.0}; + double MagFieldBias{0.0}; + double AngFieldBias{0.0}; + double EulerBias{0.0}; + double MagFieldNoise{0.0}; + double AngFieldNoise{0.0}; + double EulerNoise{0.0}; + + // Navigation filter state. + bool navInitialized{false}; + Vec3 BfieldNav{0.0, 0.0, 0.0}; + Vec3 BfieldNavPrev{0.0, 0.0, 0.0}; + Vec3 pqrNav{0.0, 0.0, 0.0}; + Vec3 pqrNavPrev{0.0, 0.0, 0.0}; + Vec3 ptpNav{0.0, 0.0, 0.0}; + Vec3 ptpNavPrev{0.0, 0.0, 0.0}; + Vec3 Bdot{0.0, 0.0, 0.0}; + + // Truth and measured signals. + Vec3 BB_truth{0.0, 0.0, 0.0}; + Vec3 BfieldMeasured{0.0, 0.0, 0.0}; + Vec3 pqrMeasured{0.0, 0.0, 0.0}; + Vec3 ptpMeasured{0.0, 0.0, 0.0}; + + // Random number generator used by the noise model. + Rng rng; + + explicit SimContext(uint64_t seed = 1) : rng(seed) {} +}; diff --git a/include/wmm.hpp b/include/wmm.hpp new file mode 100644 index 0000000..0c6e449 --- /dev/null +++ b/include/wmm.hpp @@ -0,0 +1,20 @@ +#pragma once +#include "math.hpp" + +// Compute the WMM2025 magnetic field at a geodetic point. +// Inputs: +// latitude_deg - geodetic latitude in degrees +// longitude_deg - geodetic longitude in degrees +// altitude_km - altitude above the WGS84 ellipsoid in kilometers +// decimal_year - decimal year, valid from 2025.0 through 2030.0 +// Outputs: +// X_nT - north component in nanoTesla +// Y_nT - east component in nanoTesla +// Z_nT - down component in nanoTesla +void wmm2025_geodetic_ned_nT(double latitude_deg, + double longitude_deg, + double altitude_km, + double decimal_year, + double& X_nT, + double& Y_nT, + double& Z_nT); diff --git a/plot_all.py b/plot_all.py new file mode 100644 index 0000000..ad5aebc --- /dev/null +++ b/plot_all.py @@ -0,0 +1,89 @@ +import numpy as np +import pandas as pd +import matplotlib.pyplot as plt + +# Read the CSV written by the simulator. +df = pd.read_csv("adcs_output.csv") + +# Helper for turning vector components into one magnitude trace. +def mag3(x, y, z): + x = np.asarray(x) + y = np.asarray(y) + z = np.asarray(z) + return np.sqrt(x * x + y * y + z * z) + +# Time axis for every plot. +t = df["t"].to_numpy() + +# Quick sanity check on the output file. +numeric = df.select_dtypes(include=[np.number]) +all_finite = np.isfinite(numeric.to_numpy()).all() +print("Rows:", len(df), "Cols:", len(df.columns)) +print("Any NaN/Inf in numeric data?:", not bool(all_finite)) + +# Plot the body rates. +wmag = mag3(df["p"], df["q"], df["r"]) +print("Initial |w| [rad/s]:", float(wmag[0])) +print("Final |w| [rad/s]:", float(wmag[-1])) +print("Min/Max |w| [rad/s]:", float(wmag.min()), float(wmag.max())) + +plt.figure(figsize=(9, 4)) +plt.plot(t, df["p"], label="p") +plt.plot(t, df["q"], label="q") +plt.plot(t, df["r"], label="r") +plt.xlabel("Time [s]") +plt.ylabel("Rate [rad/s]") +plt.title("Body Rates") +plt.legend() +plt.grid(True) +plt.tight_layout() +plt.show() + +# Plot the navigation Euler angles. +phi = df.get("ptpN_phi", None) +theta = df.get("ptpN_theta", None) +psi = df.get("ptpN_psi", None) + +if phi is not None and theta is not None and psi is not None: + plt.figure(figsize=(9, 4)) + plt.plot(t, phi, label="phi") + plt.plot(t, theta, label="theta") + plt.plot(t, psi, label="psi") + plt.xlabel("Time [s]") + plt.ylabel("Angle [rad]") + plt.title("Euler Angles (Navigation)") + plt.legend() + plt.grid(True) + plt.tight_layout() + plt.show() + +# Plot magnetic field magnitudes. +B_truth = mag3(df["BBx"], df["BBy"], df["BBz"]) +B_meas = mag3(df["Bmx"], df["Bmy"], df["Bmz"]) +B_nav = mag3(df["BNx"], df["BNy"], df["BNz"]) + +plt.figure(figsize=(9, 4)) +plt.plot(t, B_truth, label="|B| truth") +plt.plot(t, B_meas, label="|B| meas", alpha=0.6) +plt.plot(t, B_nav, label="|B| nav", linewidth=2) +plt.xlabel("Time [s]") +plt.ylabel("|B| [Tesla]") +plt.title("Magnetic Field Magnitude") +plt.legend() +plt.grid(True) +plt.tight_layout() +plt.show() + +# Plot the magnetorquer current commands. +if all(c in df.columns for c in ["ix", "iy", "iz"]): + plt.figure(figsize=(9, 4)) + plt.plot(t, df["ix"], label="ix") + plt.plot(t, df["iy"], label="iy") + plt.plot(t, df["iz"], label="iz") + plt.xlabel("Time [s]") + plt.ylabel("Magnetorquer current [A]") + plt.title("Magnetorquer Currents") + plt.legend() + plt.grid(True) + plt.tight_layout() + plt.show() diff --git a/reference/README-WMM-COEFS.txt b/reference/README-WMM-COEFS.txt new file mode 100644 index 0000000..6ff7769 --- /dev/null +++ b/reference/README-WMM-COEFS.txt @@ -0,0 +1,70 @@ +World Magnetic Model WMM2025 +======================================================== +Date December 17, 2024 + +WMM.COF WMM2025 Coefficients file + (Replace old WMM.COF (WMM2020 + or WMM2015) file with this) + +1. Installation Instructions +========================== + +WMM2025 GUI +----------- + +Go to installed directory, find WMM.COF and remove it. +Replace it with the new WMM.COF. + + +WMM_Linux and WMM_Windows C software +------------------------------------ + +For the version <= WMM2020, replace the WMM.COF file in the "bin" directory with the +new provided WMM.COF file + + +Your own software +----------------- +Depending on your installation, find the coefficient +file, WMM.COF (unless renamed). Replace it with the new +WMM.COF file (renaming it appropriately if necessary). + +If the coefficients are embedded in your software, you +may need to embed the new coefficients. + +2. Installation Verification +============================ + +To confirm you are using the correct WMM.COF file open +it and verify that the header is: + + 2025.0 WMM-2025 11/13/2024 + +To assist in confirming that the installation of the new +coefficient file is correct we provide a set of test +values in this package. Here are a few as an example: + +Date HAE Lat Long Decl Incl H X Y Z F Ddot Idot Hdot Xdot Ydot Zdot Fdot +2020.0 66 14 143 0.12 13.08 34916.9 34916.8 70.2 8114.9 35847.5 -0.1 -0.1 29.6 29.6 -41.4 -46.4 18.3 +2020.0 18 0 21 1.05 -26.46 29316.1 29311.2 536.0 -14589.0 32745.6 0.1 0.1 0.0 -0.9 51.2 54.5 -24.3 +2020.5 6 -36 -137 20.16 -52.21 25511.4 23948.6 8791.9 -32897.6 41630.3 0.0 0.0 -21.6 -21.6 -3.8 65.4 -64.9 +2020.5 63 26 81 0.43 40.84 34738.7 34737.7 259.2 30023.4 45914.9 0.0 0.1 5.9 5.9 2.4 128.2 88.3 + +Where HAE is height above WGS-84 ellipsoid. + + + + +Model Software Support +====================== + +* National Centers for Environmental Information (NCEI) +* E/NE42 325 Broadway +* Boulder, CO 80305 USA +* Attn: Manoj Nair or Arnaud Chulliat +* Phone: (303) 497-4642 or -6522 +* Email: geomag.models@noaa.gov +For more details about the World Magnetic Model visit +http://www.ngdc.noaa.gov/geomag/WMM/DoDWMM.shtml + + diff --git a/reference/WMM.COF b/reference/WMM.COF new file mode 100644 index 0000000..098c579 --- /dev/null +++ b/reference/WMM.COF @@ -0,0 +1,93 @@ + 2025.0 WMM-2025 11/13/2024 + 1 0 -29351.8 0.0 12.0 0.0 + 1 1 -1410.8 4545.4 9.7 -21.5 + 2 0 -2556.6 0.0 -11.6 0.0 + 2 1 2951.1 -3133.6 -5.2 -27.7 + 2 2 1649.3 -815.1 -8.0 -12.1 + 3 0 1361.0 0.0 -1.3 0.0 + 3 1 -2404.1 -56.6 -4.2 4.0 + 3 2 1243.8 237.5 0.4 -0.3 + 3 3 453.6 -549.5 -15.6 -4.1 + 4 0 895.0 0.0 -1.6 0.0 + 4 1 799.5 278.6 -2.4 -1.1 + 4 2 55.7 -133.9 -6.0 4.1 + 4 3 -281.1 212.0 5.6 1.6 + 4 4 12.1 -375.6 -7.0 -4.4 + 5 0 -233.2 0.0 0.6 0.0 + 5 1 368.9 45.4 1.4 -0.5 + 5 2 187.2 220.2 0.0 2.2 + 5 3 -138.7 -122.9 0.6 0.4 + 5 4 -142.0 43.0 2.2 1.7 + 5 5 20.9 106.1 0.9 1.9 + 6 0 64.4 0.0 -0.2 0.0 + 6 1 63.8 -18.4 -0.4 0.3 + 6 2 76.9 16.8 0.9 -1.6 + 6 3 -115.7 48.8 1.2 -0.4 + 6 4 -40.9 -59.8 -0.9 0.9 + 6 5 14.9 10.9 0.3 0.7 + 6 6 -60.7 72.7 0.9 0.9 + 7 0 79.5 0.0 -0.0 0.0 + 7 1 -77.0 -48.9 -0.1 0.6 + 7 2 -8.8 -14.4 -0.1 0.5 + 7 3 59.3 -1.0 0.5 -0.8 + 7 4 15.8 23.4 -0.1 0.0 + 7 5 2.5 -7.4 -0.8 -1.0 + 7 6 -11.1 -25.1 -0.8 0.6 + 7 7 14.2 -2.3 0.8 -0.2 + 8 0 23.2 0.0 -0.1 0.0 + 8 1 10.8 7.1 0.2 -0.2 + 8 2 -17.5 -12.6 0.0 0.5 + 8 3 2.0 11.4 0.5 -0.4 + 8 4 -21.7 -9.7 -0.1 0.4 + 8 5 16.9 12.7 0.3 -0.5 + 8 6 15.0 0.7 0.2 -0.6 + 8 7 -16.8 -5.2 -0.0 0.3 + 8 8 0.9 3.9 0.2 0.2 + 9 0 4.6 0.0 -0.0 0.0 + 9 1 7.8 -24.8 -0.1 -0.3 + 9 2 3.0 12.2 0.1 0.3 + 9 3 -0.2 8.3 0.3 -0.3 + 9 4 -2.5 -3.3 -0.3 0.3 + 9 5 -13.1 -5.2 0.0 0.2 + 9 6 2.4 7.2 0.3 -0.1 + 9 7 8.6 -0.6 -0.1 -0.2 + 9 8 -8.7 0.8 0.1 0.4 + 9 9 -12.9 10.0 -0.1 0.1 + 10 0 -1.3 0.0 0.1 0.0 + 10 1 -6.4 3.3 0.0 0.0 + 10 2 0.2 0.0 0.1 -0.0 + 10 3 2.0 2.4 0.1 -0.2 + 10 4 -1.0 5.3 -0.0 0.1 + 10 5 -0.6 -9.1 -0.3 -0.1 + 10 6 -0.9 0.4 0.0 0.1 + 10 7 1.5 -4.2 -0.1 0.0 + 10 8 0.9 -3.8 -0.1 -0.1 + 10 9 -2.7 0.9 -0.0 0.2 + 10 10 -3.9 -9.1 -0.0 -0.0 + 11 0 2.9 0.0 0.0 0.0 + 11 1 -1.5 0.0 -0.0 -0.0 + 11 2 -2.5 2.9 0.0 0.1 + 11 3 2.4 -0.6 0.0 -0.0 + 11 4 -0.6 0.2 0.0 0.1 + 11 5 -0.1 0.5 -0.1 -0.0 + 11 6 -0.6 -0.3 0.0 -0.0 + 11 7 -0.1 -1.2 -0.0 0.1 + 11 8 1.1 -1.7 -0.1 -0.0 + 11 9 -1.0 -2.9 -0.1 0.0 + 11 10 -0.2 -1.8 -0.1 0.0 + 11 11 2.6 -2.3 -0.1 0.0 + 12 0 -2.0 0.0 0.0 0.0 + 12 1 -0.2 -1.3 0.0 -0.0 + 12 2 0.3 0.7 -0.0 0.0 + 12 3 1.2 1.0 -0.0 -0.1 + 12 4 -1.3 -1.4 -0.0 0.1 + 12 5 0.6 -0.0 -0.0 -0.0 + 12 6 0.6 0.6 0.1 -0.0 + 12 7 0.5 -0.1 -0.0 -0.0 + 12 8 -0.1 0.8 0.0 0.0 + 12 9 -0.4 0.1 0.0 -0.0 + 12 10 -0.2 -1.0 -0.1 -0.0 + 12 11 -1.3 0.1 -0.0 0.0 + 12 12 -0.7 0.2 -0.1 -0.1 +999999999999999999999999999999999999999999999999 +999999999999999999999999999999999999999999999999 diff --git a/reference/WMM2025.COF b/reference/WMM2025.COF new file mode 100644 index 0000000..098c579 --- /dev/null +++ b/reference/WMM2025.COF @@ -0,0 +1,93 @@ + 2025.0 WMM-2025 11/13/2024 + 1 0 -29351.8 0.0 12.0 0.0 + 1 1 -1410.8 4545.4 9.7 -21.5 + 2 0 -2556.6 0.0 -11.6 0.0 + 2 1 2951.1 -3133.6 -5.2 -27.7 + 2 2 1649.3 -815.1 -8.0 -12.1 + 3 0 1361.0 0.0 -1.3 0.0 + 3 1 -2404.1 -56.6 -4.2 4.0 + 3 2 1243.8 237.5 0.4 -0.3 + 3 3 453.6 -549.5 -15.6 -4.1 + 4 0 895.0 0.0 -1.6 0.0 + 4 1 799.5 278.6 -2.4 -1.1 + 4 2 55.7 -133.9 -6.0 4.1 + 4 3 -281.1 212.0 5.6 1.6 + 4 4 12.1 -375.6 -7.0 -4.4 + 5 0 -233.2 0.0 0.6 0.0 + 5 1 368.9 45.4 1.4 -0.5 + 5 2 187.2 220.2 0.0 2.2 + 5 3 -138.7 -122.9 0.6 0.4 + 5 4 -142.0 43.0 2.2 1.7 + 5 5 20.9 106.1 0.9 1.9 + 6 0 64.4 0.0 -0.2 0.0 + 6 1 63.8 -18.4 -0.4 0.3 + 6 2 76.9 16.8 0.9 -1.6 + 6 3 -115.7 48.8 1.2 -0.4 + 6 4 -40.9 -59.8 -0.9 0.9 + 6 5 14.9 10.9 0.3 0.7 + 6 6 -60.7 72.7 0.9 0.9 + 7 0 79.5 0.0 -0.0 0.0 + 7 1 -77.0 -48.9 -0.1 0.6 + 7 2 -8.8 -14.4 -0.1 0.5 + 7 3 59.3 -1.0 0.5 -0.8 + 7 4 15.8 23.4 -0.1 0.0 + 7 5 2.5 -7.4 -0.8 -1.0 + 7 6 -11.1 -25.1 -0.8 0.6 + 7 7 14.2 -2.3 0.8 -0.2 + 8 0 23.2 0.0 -0.1 0.0 + 8 1 10.8 7.1 0.2 -0.2 + 8 2 -17.5 -12.6 0.0 0.5 + 8 3 2.0 11.4 0.5 -0.4 + 8 4 -21.7 -9.7 -0.1 0.4 + 8 5 16.9 12.7 0.3 -0.5 + 8 6 15.0 0.7 0.2 -0.6 + 8 7 -16.8 -5.2 -0.0 0.3 + 8 8 0.9 3.9 0.2 0.2 + 9 0 4.6 0.0 -0.0 0.0 + 9 1 7.8 -24.8 -0.1 -0.3 + 9 2 3.0 12.2 0.1 0.3 + 9 3 -0.2 8.3 0.3 -0.3 + 9 4 -2.5 -3.3 -0.3 0.3 + 9 5 -13.1 -5.2 0.0 0.2 + 9 6 2.4 7.2 0.3 -0.1 + 9 7 8.6 -0.6 -0.1 -0.2 + 9 8 -8.7 0.8 0.1 0.4 + 9 9 -12.9 10.0 -0.1 0.1 + 10 0 -1.3 0.0 0.1 0.0 + 10 1 -6.4 3.3 0.0 0.0 + 10 2 0.2 0.0 0.1 -0.0 + 10 3 2.0 2.4 0.1 -0.2 + 10 4 -1.0 5.3 -0.0 0.1 + 10 5 -0.6 -9.1 -0.3 -0.1 + 10 6 -0.9 0.4 0.0 0.1 + 10 7 1.5 -4.2 -0.1 0.0 + 10 8 0.9 -3.8 -0.1 -0.1 + 10 9 -2.7 0.9 -0.0 0.2 + 10 10 -3.9 -9.1 -0.0 -0.0 + 11 0 2.9 0.0 0.0 0.0 + 11 1 -1.5 0.0 -0.0 -0.0 + 11 2 -2.5 2.9 0.0 0.1 + 11 3 2.4 -0.6 0.0 -0.0 + 11 4 -0.6 0.2 0.0 0.1 + 11 5 -0.1 0.5 -0.1 -0.0 + 11 6 -0.6 -0.3 0.0 -0.0 + 11 7 -0.1 -1.2 -0.0 0.1 + 11 8 1.1 -1.7 -0.1 -0.0 + 11 9 -1.0 -2.9 -0.1 0.0 + 11 10 -0.2 -1.8 -0.1 0.0 + 11 11 2.6 -2.3 -0.1 0.0 + 12 0 -2.0 0.0 0.0 0.0 + 12 1 -0.2 -1.3 0.0 -0.0 + 12 2 0.3 0.7 -0.0 0.0 + 12 3 1.2 1.0 -0.0 -0.1 + 12 4 -1.3 -1.4 -0.0 0.1 + 12 5 0.6 -0.0 -0.0 -0.0 + 12 6 0.6 0.6 0.1 -0.0 + 12 7 0.5 -0.1 -0.0 -0.0 + 12 8 -0.1 0.8 0.0 0.0 + 12 9 -0.4 0.1 0.0 -0.0 + 12 10 -0.2 -1.0 -0.1 -0.0 + 12 11 -1.3 0.1 -0.0 0.0 + 12 12 -0.7 0.2 -0.1 -0.1 +999999999999999999999999999999999999999999999999 +999999999999999999999999999999999999999999999999 diff --git a/reference/WMM2025_TestValues.txt b/reference/WMM2025_TestValues.txt new file mode 100644 index 0000000..64b2801 --- /dev/null +++ b/reference/WMM2025_TestValues.txt @@ -0,0 +1,118 @@ +# Field 1: Decimal year +# Field 2: Altitude (km) +# Field 3: geodetic latitude (deg) +# Field 4: geodetic longitude (deg) +# Field 5: declination (deg) +# Field 6: inclination (deg) +# Field 7: H (nT) +# Field 8: X (nT) +# Field 9: Y (nT) +# Field 10: Z (nT) +# Field 11: F (nT) +# Field 12: dD/dt (deg/year) +# Field 13: dI/dt (deg/year) +# Field 14: dH/dt (nT/year) +# Field 15: dX/dt (nT/year) +# Field 16: dY/dt (nT/year) +# Field 17: dZ/dt (nT/year) +# Field 18: dF/dt (nT/year) +2025.000000 28 89 -121 -99.77 88.47 1504.298146 -255.388723 -1482.460628 56194.288771 56214.419888 2.491706 -0.009987 10.285640 62.723738 -21.242793 18.075146 18.343917 +2025.000000 48 80 -96 -29.91 87.77 2164.285547 1875.982280 -1079.269389 55623.044051 55665.134163 1.248417 -0.057537 55.469239 71.596398 13.214774 -12.148507 -9.982652 +2025.000000 54 82 87 54.89 87.68 2302.427342 1324.336929 1883.428620 56740.772059 56787.466800 0.884574 0.036130 -34.169939 -48.732002 -7.505574 41.134755 39.715524 +2025.000000 65 43 93 0.50 64.10 24300.764692 24299.852822 210.517066 50037.923998 55626.621348 -0.019813 0.030822 -3.938347 -3.865401 -8.437166 60.388819 52.601187 +2025.000000 51 -33 109 -5.49 -67.50 21838.046477 21737.778822 -2090.274098 -52710.003920 57054.752538 0.069861 0.026375 29.809763 32.221578 23.651711 -3.334040 14.490015 +2025.000000 39 -59 -8 -15.75 -58.55 14918.115796 14358.095523 -4049.107540 -24389.086374 28589.818346 0.010988 0.055916 -0.905656 -0.095123 2.999401 54.951779 -47.350226 +2025.000000 3 -50 -103 27.96 -54.89 22106.041373 19526.532799 10362.990980 -31437.562789 38431.724126 -0.032104 0.024888 -42.140973 -31.417096 -30.696074 88.952383 -97.003616 +2025.000000 94 -29 -110 15.74 -38.25 24181.990098 23275.471080 6559.046509 -19063.605287 30792.688931 -0.011062 0.027107 -42.453758 -39.595926 -16.008811 52.018933 -65.544285 +2025.000000 66 14 143 -0.19 12.82 35003.635649 35003.441364 -116.624818 7966.315182 35898.700342 -0.055086 -0.003859 10.848172 10.735986 -33.689325 -0.010939 10.575266 +2025.000000 18 0 21 1.29 -26.06 29282.246275 29274.811882 659.800118 -14316.722540 32594.761714 0.030544 0.079429 -8.761425 -9.110937 15.408838 54.581250 -31.844958 +2025.500000 6 -36 -137 20.28 -52.11 25353.230658 23781.930678 8786.698927 -32577.518648 41280.516301 0.025348 0.014224 -34.463126 -36.214518 -1.422651 60.968936 -69.281309 +2025.500000 63 26 81 0.51 41.07 34803.980117 34802.613433 308.431881 30332.056989 46166.554053 0.018525 0.021888 20.342054 20.241534 11.432574 41.122831 42.353703 +2025.500000 69 38 -144 12.93 56.97 23096.337032 22510.804524 5167.636206 35525.990264 42373.774537 -0.105300 -0.007297 -33.951051 -23.593092 -48.967345 -62.123761 -70.589724 +2025.500000 50 -70 -133 57.21 -71.94 16656.709403 9021.847823 14001.865232 -51084.838301 53731.803175 -0.036998 0.045634 8.590794 13.694592 1.395803 111.703074 -103.537175 +2025.500000 8 -52 -75 14.91 -49.63 20005.506364 19331.857134 5147.774727 -23532.664273 30886.996822 -0.098427 -0.028885 -58.502212 -47.688987 -48.263512 44.775999 -72.006513 +2025.500000 8 -66 17 -33.14 -59.55 18154.870136 15201.438652 -9925.501123 -30881.123197 35822.382382 -0.112991 0.041932 10.080437 -11.133189 -35.489343 34.582697 -24.703647 +2025.500000 22 -37 140 9.28 -68.62 21688.847590 21404.761773 3498.897430 -55397.587760 59492.006517 0.028818 0.004094 0.393984 -1.371020 10.829532 10.653875 -9.777010 +2025.500000 40 -12 -129 10.76 -15.46 29105.866326 28594.451238 5432.201489 -8052.525092 30199.248583 -0.009568 0.030404 -39.188467 -37.592705 -12.089297 27.469404 -45.094246 +2025.500000 44 33 -118 11.10 57.89 23678.743422 23235.835850 4558.379362 37727.715752 44542.826874 -0.075717 -0.021971 -41.700619 -34.896682 -38.734089 -98.573110 -105.659134 +2025.500000 50 -81 -67 28.13 -67.61 18292.842720 16132.682326 8623.494405 -44412.283868 48032.062762 -0.099291 0.019879 -12.853806 3.608124 -34.016532 74.964128 -74.210029 +2026.000000 74 -57 3 -22.51 -58.65 14362.206593 13268.119649 -5498.179626 -23576.062921 27606.226129 -0.037845 0.082339 12.945610 8.327751 -13.719801 55.005355 -40.240277 +2026.000000 46 -24 -122 14.01 -34.17 26638.500090 25846.118652 6448.863284 -18080.399376 32194.883578 0.000271 0.019932 -40.514451 -39.339842 -9.685735 41.034418 -56.566841 +2026.000000 69 23 63 1.17 35.92 34565.858527 34558.605297 708.078839 25043.405885 42684.549360 0.013234 0.010624 23.857027 23.688475 8.470762 27.058242 35.194683 +2026.000000 33 -3 -147 9.71 -2.12 30957.666082 30514.086409 5221.840663 -1146.978999 30978.906535 -0.006185 0.036855 -36.361404 -35.276692 -9.427361 21.287924 -37.124648 +2026.000000 47 -72 -22 -6.32 -61.16 18392.516222 18280.800055 -2024.105320 -33397.486160 38127.112857 -0.048225 0.017330 -15.939764 -17.546607 -13.632509 52.849287 -53.982732 +2026.000000 62 -14 99 -1.43 -44.70 33448.275663 33437.828630 -835.919473 -33100.922253 47058.030120 0.061400 0.072480 38.755947 39.639639 34.864408 45.397052 -4.385324 +2026.000000 83 86 -46 -30.61 86.84 3000.255301 2582.099403 -1527.839829 54279.308437 54362.163830 1.221388 -0.002547 3.280093 35.392266 53.372894 15.551936 15.709262 +2026.000000 82 -64 87 -81.74 -75.40 13974.248411 2007.074870 -13829.362571 -53663.514288 55453.154865 -0.187260 -0.029553 -23.144309 -48.522801 16.344602 -24.623137 17.996086 +2026.000000 34 -19 43 -14.98 -52.33 20212.448819 19525.694050 -5224.017527 -26182.862330 33076.961273 -0.135700 0.023383 51.024088 36.917850 -59.432251 -44.004821 66.012531 +2026.000000 56 -81 40 -59.77 -68.45 17877.818542 9000.536024 -15446.900890 -45267.890393 48670.301997 -0.146798 0.017031 -1.330470 -40.246526 -21.910829 42.754262 -40.254140 +2026.500000 14 0 80 -3.10 -17.15 39489.089663 39431.415992 -2133.456168 -12188.838466 41327.424134 0.056431 0.021103 33.618195 35.670346 37.019904 5.553327 30.484922 +2026.500000 12 -82 -68 29.79 -68.07 18497.480257 16052.819558 9190.416754 -45940.146795 49524.275496 -0.104864 0.021236 -11.118018 7.171828 -34.904116 76.756556 -75.354213 +2026.500000 44 -46 -42 -11.36 -54.39 14140.316272 13863.176808 -2785.834358 -19744.304022 24285.511845 0.039043 -0.119143 -63.277988 -60.139435 21.913412 1.623579 -38.163791 +2026.500000 43 17 52 1.19 23.95 36002.676553 35994.907968 747.876551 15990.895993 39394.180708 -0.010785 0.016571 20.351040 20.487426 -6.352799 21.505886 27.328663 +2026.500000 64 10 78 -1.53 7.53 39441.702532 39427.724480 -1049.971869 5214.811025 39784.948821 0.031231 0.021184 27.794943 28.357414 20.751458 18.512565 29.981674 +2026.500000 12 33 -145 11.96 52.51 24672.289649 24136.839639 5112.225422 32162.934508 40536.110231 -0.086620 0.000340 -40.973255 -32.355349 -44.980044 -53.017362 -67.004405 +2026.500000 12 -79 115 -137.58 -77.37 13023.480845 -9613.650418 -8785.714482 -58104.306533 59545.961164 -0.241722 0.014808 7.471685 -42.581077 35.518140 37.027107 -34.496496 +2026.500000 14 -33 -114 18.12 -44.10 24613.183584 23393.133584 7653.110952 -23854.293436 34275.882505 -0.000322 0.022971 -42.803882 -40.639067 -13.440897 60.620545 -72.925914 +2026.500000 19 29 66 2.24 46.04 32749.959268 32725.000691 1278.343380 33958.454002 47177.711160 0.018224 0.011101 23.712541 23.287876 11.334193 37.754815 43.636705 +2026.500000 86 -11 167 10.24 -31.60 33105.255278 32578.377044 5882.794932 -20368.224196 38869.300019 0.013426 -0.025948 -16.653141 -17.766581 4.674631 -10.422224 -8.722161 +2027.000000 37 -66 -5 -17.22 -59.04 17159.836500 16390.771268 -5079.626554 -28608.243575 33360.029814 -0.043630 0.039964 -2.074991 -5.850093 -11.867216 48.695530 -42.826703 +2027.000000 67 72 -115 13.73 84.84 5026.868699 4883.287833 1192.857435 55689.615237 55916.032175 -0.315901 -0.072636 66.581032 71.256140 -11.124661 -50.889259 -44.697541 +2027.000000 44 22 174 6.46 31.89 28867.487799 28683.972510 3249.857362 17961.198575 33999.066253 -0.024375 -0.000168 2.815221 4.179894 -11.885941 1.634357 3.253718 +2027.000000 54 54 178 0.63 65.46 20617.099795 20615.871559 225.041793 45149.631876 49634.202547 -0.170729 0.012944 -3.909066 -3.238256 -61.473597 18.434524 15.145168 +2027.000000 57 -43 50 -48.27 -63.13 16833.417225 11203.748204 -12563.437494 -33221.366617 37242.759503 -0.165742 -0.033376 12.323002 -28.141121 -41.606787 -72.317297 70.078525 +2027.000000 44 -43 -111 24.31 -52.57 22462.470699 20471.522067 9245.505620 -29347.556906 36957.295441 -0.006299 0.024728 -40.058633 -35.491603 -18.738696 78.579889 -86.747247 +2027.000000 12 -63 178 57.87 -79.14 11720.691727 6233.774935 9925.455386 -61075.730989 62190.188377 0.151793 0.036783 27.353301 -11.747306 39.678751 69.308326 -62.911163 +2027.000000 38 27 -169 8.48 42.66 26106.758229 25821.061601 3851.701313 24058.365347 35501.658671 -0.058765 0.013094 -19.135886 -14.976028 -29.306273 -6.601488 -18.545527 +2027.000000 61 59 -77 -16.48 78.68 10884.812802 10437.775689 -3087.391846 54397.713552 55476.034370 0.230418 -0.074136 59.669126 69.634646 25.051419 -67.643997 -54.621632 +2027.000000 67 -47 -32 -13.52 -57.98 12805.786103 12450.832543 -2994.148743 -20475.298079 24150.072239 0.115550 -0.077078 -52.470761 -44.977997 37.378183 22.627270 -47.007290 +2027.500000 8 62 53 19.39 76.67 12997.751893 12260.425578 4315.497527 54849.810218 56368.814385 0.099601 0.030088 -12.884268 -19.655269 17.035218 74.003425 69.038304 +2027.500000 77 -68 -7 -16.19 -59.82 17262.817301 16578.099495 -4813.676173 -29680.383821 34335.550744 -0.048399 0.032649 -5.108702 -8.972297 -12.579376 47.699056 -43.800575 +2027.500000 98 -5 159 7.79 -23.22 33857.496691 33544.960996 4589.735713 -14525.780052 36841.937629 -0.005671 -0.036102 -11.751062 -11.188278 -4.913401 -20.218524 -2.827531 +2027.500000 34 -29 -107 15.64 -37.45 24446.053109 23540.396213 6592.363667 -18723.097084 30792.269761 -0.017007 0.030517 -44.335923 -40.736596 -18.943541 54.615130 -68.406867 +2027.500000 60 27 65 1.85 42.83 33079.422542 33062.159421 1068.555172 30667.425573 45108.083389 0.016699 0.011093 23.388908 23.065275 10.391374 33.592654 39.990433 +2027.500000 73 -72 95 -102.64 -76.49 13306.747292 -2912.418045 -12984.118939 -55399.217725 56974.931751 -0.235287 -0.006801 -8.442550 -51.471801 20.197772 6.191978 -7.992526 +2027.500000 96 -46 -85 17.93 -47.37 19914.457641 18947.658009 6129.590452 -21631.434316 29402.458634 -0.091418 -0.005439 -53.229644 -40.865392 -46.615842 53.698344 -75.558704 +2027.500000 0 -13 -59 -17.49 -15.26 22401.493010 21365.849306 -6732.560620 -6112.313520 23220.406233 -0.173690 -0.385680 -68.873971 -86.099325 -44.070217 -143.226783 -28.743372 +2027.500000 16 66 -178 0.37 75.67 13821.614337 13821.326215 89.244328 54092.646409 55830.559897 -0.327448 0.003026 -1.280867 -0.770805 -78.997758 6.899408 6.367545 +2027.500000 72 -87 38 -65.44 -70.97 16661.696834 6926.051596 -15153.941754 -48295.635435 51088.947370 -0.148310 0.021340 0.540463 -39.001360 -18.419660 56.777921 -53.497300 +2028.000000 49 20 167 5.10 26.82 30251.262453 30131.436134 2689.876669 15295.611788 33898.298187 -0.020383 -0.011359 9.497816 10.417136 -9.874921 -2.728173 7.244961 +2028.000000 71 5 -13 -6.47 -17.66 28323.200567 28142.647915 -3192.970202 -9017.928693 29724.177504 0.166481 -0.073692 -18.125869 -8.732692 83.815865 -34.350117 -6.850172 +2028.000000 95 14 65 -0.51 17.44 36933.940251 36932.466767 -329.910612 11601.180703 38713.089985 0.011924 0.011881 29.013291 29.080794 7.427118 17.527263 32.932326 +2028.000000 86 -85 -79 41.09 -70.25 16867.459172 12713.115116 11085.480728 -46988.258092 49924.018042 -0.121705 0.024621 -3.420695 20.969078 -29.252744 73.025430 -69.886927 +2028.000000 30 -36 -64 -4.65 -40.08 17398.651081 17341.333537 -1411.102609 -14639.967305 22738.551012 -0.162162 -0.152944 -69.721104 -73.485198 -43.425730 -20.660569 -40.045784 +2028.000000 75 79 125 -18.59 87.42 2582.332595 2447.595900 -823.235047 57308.751756 57366.902212 -0.910648 0.022931 -21.221891 -33.198946 -32.136170 39.082660 38.087754 +2028.000000 21 6 -32 -14.34 -8.70 28453.070088 27567.094260 -7045.034525 -4352.046687 28783.980055 0.177960 -0.311470 -29.041854 -6.255704 92.814073 -153.852374 -5.445988 +2028.000000 1 -76 -75 29.87 -65.23 19597.635210 16993.742946 9761.147808 -42479.847250 46782.525885 -0.086171 0.016127 -22.854241 -5.137167 -36.941289 80.971555 -83.098300 +2028.000000 45 -46 -41 -11.68 -54.96 13897.562372 13609.973828 -2812.623735 -19816.196211 24203.798713 0.048411 -0.117246 -62.721602 -59.047199 24.193263 3.174357 -38.612991 +2028.000000 11 -22 -21 -23.24 -57.67 13528.270036 12430.606447 -5337.987778 -21373.837958 25295.356080 0.176027 -0.196560 -92.571749 -68.660974 74.716931 -16.002042 -35.987260 +2028.500000 28 54 -120 15.43 73.74 15286.094495 14735.145798 4066.959949 52393.678137 54578.037650 -0.147623 -0.053732 20.801434 30.530225 -32.430768 -111.448817 -101.162318 +2028.500000 68 -58 156 41.57 -81.52 9282.943032 6944.959164 6159.591995 -62264.415929 62952.605366 0.218259 0.014957 11.274695 -15.028950 33.936954 35.824068 -33.769886 +2028.500000 39 -65 -88 29.45 -60.20 20609.270068 17946.905687 10131.662696 -35982.872979 41466.964689 -0.076076 0.014457 -36.693634 -18.500934 -41.868201 85.117374 -92.097328 +2028.500000 27 -23 81 -13.27 -58.58 25625.329284 24940.672257 -5883.907569 -41948.733657 49156.421313 0.129297 -0.007968 18.629651 31.409843 52.004836 -43.610533 46.927695 +2028.500000 11 34 0 1.57 46.77 29089.194286 29078.234597 798.434048 30945.577813 42471.284539 0.111314 0.010464 15.380882 13.823891 56.915296 27.686933 30.707939 +2028.500000 72 -62 65 -67.87 -68.50 17434.847799 6567.891205 -16150.440331 -44267.327811 47576.992646 -0.176819 -0.022421 -7.894025 -52.815238 -12.956527 -30.762466 25.729685 +2028.500000 55 86 70 67.64 87.57 2370.361097 901.741754 2192.139033 55926.154052 55976.363930 1.341225 0.014915 -13.210618 -56.340956 8.891342 32.415740 31.827250 +2028.500000 59 32 163 0.15 43.10 28217.204381 28217.104175 75.200106 26405.862611 38645.571587 -0.043407 0.005164 13.000120 13.057045 -21.342658 16.936400 21.064439 +2028.500000 65 48 148 -9.55 61.79 23693.546804 23365.167010 -3931.047029 44177.553125 50130.233993 -0.040524 0.019409 4.495479 1.652822 -17.271572 44.310648 41.173752 +2028.500000 95 30 28 4.56 44.27 29786.406936 29692.133298 2367.965026 29039.808439 41599.765773 0.034245 0.035803 8.349023 6.907310 18.410171 44.444119 37.003480 +2029.000000 95 -60 -59 8.58 -55.17 18095.598879 17893.018948 2700.105868 -26011.845842 31687.013474 -0.053323 -0.035373 -49.689333 -46.620155 -24.066802 37.170306 -58.889314 +2029.000000 95 -70 42 -55.06 -64.54 18202.660480 10426.249753 -14920.796381 -38237.047006 42348.655378 -0.157751 0.012766 3.604264 -39.016467 -31.660684 14.381378 -11.435882 +2029.000000 50 87 -154 -73.48 89.07 906.920459 257.877420 -869.484879 55992.308183 55999.652503 1.562405 -0.064156 62.930173 41.603892 -53.300475 13.406039 14.423442 +2029.000000 58 32 19 4.11 46.03 29434.989012 29359.395781 2108.188210 30508.465542 42393.219362 0.056569 0.028397 10.593100 8.484435 29.745872 41.240139 37.033780 +2029.000000 57 34 -13 -1.89 45.74 28257.277809 28241.863242 -933.225495 28997.458189 40488.595068 0.154151 -0.026819 21.195169 23.694393 75.283010 -5.404499 10.921620 +2029.000000 38 -76 49 -64.28 -67.36 18412.640681 7991.121323 -16588.167977 -44151.303941 47836.837024 -0.160186 0.012467 -0.041101 -46.394534 -22.304306 27.140173 -25.065010 +2029.000000 49 -50 -179 32.11 -71.33 18080.593790 15315.042731 9610.272521 -53522.651338 56494.088877 0.110906 0.012602 -5.111774 -22.932228 26.927867 53.956270 -52.754307 +2029.000000 90 -55 -171 38.65 -72.79 16416.538468 12821.507915 10252.398258 -52995.143972 55479.618058 0.117199 0.024083 2.622098 -18.923406 27.863957 70.344114 -66.418097 +2029.000000 41 42 -19 -4.13 56.44 24503.475397 24439.977735 -1762.893879 36929.897501 44319.720622 0.180959 -0.034375 24.712482 30.216258 75.411786 -10.849192 4.622821 +2029.000000 19 46 -22 -5.65 60.89 22632.016516 22522.142232 -2227.393290 40651.688191 46527.066578 0.196585 -0.034725 25.397648 32.916674 74.775309 -12.351198 1.562595 +2029.500000 31 13 -132 9.04 31.41 28145.022897 27795.620707 4421.061344 17187.983590 32978.312476 -0.039071 0.025976 -57.604250 -53.874337 -28.002848 -17.659477 -58.365744 +2029.500000 93 -2 158 7.09 -17.84 34067.301020 33806.520595 4207.156292 -10964.647547 35788.329028 -0.010065 -0.038403 -8.149803 -7.348347 -6.945244 -22.576489 -0.841015 +2029.500000 51 -76 40 -56.34 -66.22 18517.441118 10263.650094 -15412.758102 -42018.541264 45917.898858 -0.148779 0.015688 0.140771 -39.943896 -26.768531 30.856345 -28.179249 +2029.500000 64 22 -132 10.23 43.76 26014.780783 25601.266218 4619.955329 24914.378368 36020.758857 -0.057237 0.010172 -55.619345 -50.120044 -35.452393 -44.412257 -70.887703 +2029.500000 26 -65 55 -63.48 -65.71 18695.741849 8347.198524 -16728.868464 -41431.528035 45454.397791 -0.179396 -0.005003 1.416835 -51.746426 -27.403316 -12.788863 12.239760 +2029.500000 66 -21 32 -14.63 -56.68 16100.322907 15578.335534 -4066.430831 -24494.877642 29312.444941 -0.198629 0.076846 37.723336 22.403071 -63.533684 14.184486 8.866905 +2029.500000 18 9 -172 9.24 15.85 30922.514410 30520.999126 4966.941695 8779.472279 32144.689001 -0.000053 0.016657 -17.137150 -16.910013 -2.781046 4.848633 -15.161302 +2029.500000 63 88 26 36.52 87.37 2539.900024 2041.140972 1511.567287 55286.620082 55344.931586 1.375737 0.002257 -1.029565 -37.121853 48.397411 25.084712 25.011033 +2029.500000 33 17 5 0.89 13.77 34026.126581 34021.976056 531.446418 8341.137769 35033.582023 0.078642 0.026998 3.099752 2.369935 46.745426 17.756374 7.238224 +2029.500000 77 -18 138 4.45 -47.55 31847.600506 31751.497580 2472.257962 -34817.395113 47186.021876 -0.036580 0.012491 0.895474 2.471158 -20.201885 14.262673 -9.919684 diff --git a/src/control.cpp b/src/control.cpp new file mode 100644 index 0000000..570d85c --- /dev/null +++ b/src/control.cpp @@ -0,0 +1,26 @@ +#include "modules.hpp" +#include + +// Keep the actuator constants in one place. +static void magtorquer_params(SimContext& ctx) { + ctx.n_turns = 84.0; + ctx.A_turn = 0.02; + ctx.maxCurrent_mA = 120.0; +} + +// Simple magnetic detumble control law. +// This is not a full flight-quality controller, but it is a common first closed-loop test. +void control_compute(const Vec3& BfieldNav, + const Vec3& pqrNav, + const Vec3& ptpNav, + SimContext& ctx, + Vec3& current_out) { + (void)ptpNav; + + const double k = 67200.0; + magtorquer_params(ctx); + + // Command current from the rate-field cross product. + const Vec3 c = cross(pqrNav, BfieldNav); + current_out = (k / (ctx.n_turns * ctx.A_turn)) * c; +} diff --git a/src/disturbance.cpp b/src/disturbance.cpp new file mode 100644 index 0000000..e23ec53 --- /dev/null +++ b/src/disturbance.cpp @@ -0,0 +1,44 @@ +#include "modules.hpp" +#include + +// Very rough exponential atmosphere. +// This is still a placeholder, but it gives the sim some drag without needing a full atmosphere model. +double density_kg_m3(double altitude_m) { + const double rho0 = 1.225; + const double sigma = 0.1354; + return rho0 * std::exp(-sigma * altitude_m / 1000.0); +} + +// Simple disturbance force and torque model. +// This part is still approximate and should not be treated as a flight-quality environment model. +void disturbance(double altitude_m, + double Amax, + double lmax, + const Vec3& vel, + double CD, + const Vec3& BI_Tesla, + Vec3& XYZD_out, + Vec3& LMND_out) { + const double V = norm(vel); + const double rho = density_kg_m3(altitude_m); + + const double Fdrag = 0.5 * rho * V * V * Amax * CD; + const Vec3 vhat = (V > 0.0) ? (vel / V) : Vec3{0.0, 0.0, 0.0}; + const Vec3 F_aero = -Fdrag * vhat; + const double M_aero = Fdrag * lmax / 2.0; + const Vec3 T_aero{M_aero, M_aero, M_aero}; + + const double solar_pressure = 4.5e-6; + const double F_srp_mag = solar_pressure * Amax; + const Vec3 shat = vhat; + const Vec3 F_srp = -F_srp_mag * shat; + const double M_srp = F_srp_mag * lmax / 2.0; + const Vec3 T_srp{M_srp, M_srp, M_srp}; + + // Placeholder residual magnetic dipole torque term. + const double dconstant = 2.64e-3; + const Vec3 T_residual = dconstant * BI_Tesla; + + XYZD_out = F_srp + F_aero; + LMND_out = T_srp + T_aero + T_residual; +} diff --git a/src/main.cpp b/src/main.cpp index 1dae934..68bed88 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,69 +1,146 @@ -#pragma once -#include -#include -#include +#include "modules.hpp" #include "params.hpp" -#include -#include "satellite.hpp" +#include +#include +#include +#include +// Add two state vectors component-by-component. +static State13 add(const State13& a, const State13& b) { + State13 c{}; + for (int i = 0; i < 13; ++i) { + c[i] = a[i] + b[i]; + } + return c; +} + +// Multiply a state vector by a scalar. +static State13 scale(const State13& a, double s) { + State13 c{}; + for (int i = 0; i < 13; ++i) { + c[i] = a[i] * s; + } + return c; +} + +int main() { + // Fixed seed makes debugging repeatable. + SimContext ctx(/*seed=*/1); + + // Start in a 600 km orbit. + const double altitude_m = 600.0 * 1000.0; + + // Set the Earth constants here so we can build the starting orbit right away. + // We do not call the full dynamics function on a zero state because that would + // divide by zero in the orbit math and pollute the nav state before the run starts. + ctx.R = 6.371e6; + ctx.M = 5.972e24; + ctx.G = 6.67e-11; + ctx.mu = ctx.G * ctx.M; + + // Circular orbit initial condition in the inertial frame. + const double x0 = ctx.R + altitude_m; + const double y0 = 0.0; + const double z0 = 0.0; + const double xdot0 = 0.0; + + const double inclination = 56.0 * M_PI / 180.0; + const double semi_major = std::sqrt(x0 * x0 + y0 * y0 + z0 * z0); + const double vcircular = std::sqrt(ctx.mu / semi_major); + const double ydot0 = vcircular * std::cos(inclination); + const double zdot0 = vcircular * std::sin(inclination); + + // Start with level attitude but some body-rate tumble. + const Vec3 ptp0{0.0, 0.0, 0.0}; + const Quat q0 = euler321_to_quat(ptp0); + const double p0 = 0.8; + const double q00 = -0.2; + const double r0 = 0.3; + + State13 state{}; + state[0] = x0; state[1] = y0; state[2] = z0; + state[3] = xdot0; state[4] = ydot0; state[5] = zdot0; + state[6] = q0.q0; state[7] = q0.q1; state[8] = q0.q2; state[9] = q0.q3; + state[10] = p0; state[11] = q00; state[12] = r0; + + // Run for one orbit. + const double period = 2.0 * M_PI / std::sqrt(ctx.mu) * std::pow(semi_major, 1.5); + const double tfinal = period; + const double dt = SimParams::timestep; + + // Initial update settings. + ctx.lastSensorUpdate = 0.0; + ctx.lastMagUpdate = 0.0; + ctx.nextMagUpdate = 1.0; + ctx.nextSensorUpdate = 1.0; -int main(){ - std::ofstream file("../output/test.txt"); - if (!file) { // Check if file opened - std::cerr << "Error opening file!" << std::endl; - return 1; + // Prime the truth/sensor/nav states before the first log line. + satellite_derivatives(0.0, state, ctx); + + // Controller update period. + double lastControl = -dt; + const double nextControl = 0.1; + + std::ofstream csv("adcs_output.csv"); + csv << std::setprecision(17); + + // CSV header. + csv << "t," + << "x,y,z,xdot,ydot,zdot,q0,q1,q2,q3,p,q,r," + << "BBx,BBy,BBz,Bmx,Bmy,Bmz,BNx,BNy,BNz," + << "pqrm_x,pqrm_y,pqrm_z,pqrN_x,pqrN_y,pqrN_z," + << "ptpm_phi,ptpm_theta,ptpm_psi,ptpN_phi,ptpN_theta,ptpN_psi," + << "ix,iy,iz\n"; + + for (double t = 0.0; t <= tfinal + 1e-12; t += dt) { + // Log the state and the main internal signals. + csv << t << ","; + for (int i = 0; i < 13; ++i) { + csv << state[i] << ","; } - - std::ofstream bfile("../output/bfield.txt"); - if (!bfile) { // magnetic field output - std::cerr << "Error opening bfield.txt!" << std::endl; - return 1; + csv << ctx.BB_truth.x << "," << ctx.BB_truth.y << "," << ctx.BB_truth.z << "," + << ctx.BfieldMeasured.x << "," << ctx.BfieldMeasured.y << "," << ctx.BfieldMeasured.z << "," + << ctx.BfieldNav.x << "," << ctx.BfieldNav.y << "," << ctx.BfieldNav.z << "," + << ctx.pqrMeasured.x << "," << ctx.pqrMeasured.y << "," << ctx.pqrMeasured.z << "," + << ctx.pqrNav.x << "," << ctx.pqrNav.y << "," << ctx.pqrNav.z << "," + << ctx.ptpMeasured.x << "," << ctx.ptpMeasured.y << "," << ctx.ptpMeasured.z << "," + << ctx.ptpNav.x << "," << ctx.ptpNav.y << "," << ctx.ptpNav.z << "," + << ctx.current.x << "," << ctx.current.y << "," << ctx.current.z + << "\n"; + + // Update the controller on its own schedule. + if (t > lastControl) { + Vec3 current{}; + control_compute(ctx.BfieldNav, ctx.pqrNav, ctx.ptpNav, ctx, current); + ctx.current = current; + lastControl += nextControl; } - std::cout << "Simulation Started." << '\n'; - - Satellite sat = Satellite(params::mass); - int numberOfOrbits = 1; - - - imu::Vector<6> state = imu::Vector<6>(); - state[0] = params::R + params::altitude; - state[1] = 0.0; - state[2] = 0.0; - - double semimajor = std::sqrt(state[0]*state[0] + state[1]*state[1] + state[2]*state[2]); - double vCircular = std::sqrt(params::mu / semimajor); - std::cout << "" << vCircular << '\n'; - state[3] = 0.0; - state[4] = vCircular * std::cos(params::inclination); - state[5] = vCircular * std::sin(params::inclination); - - - double period = 2 * params::pi * std::sqrt(semimajor*semimajor*semimajor / params::mu); - - double tFinal = period * numberOfOrbits; - double t = 0.0; - file << "" << t << "," << state[0]<< "," << state[1]<< "," << state[2]<< "," << state[3]<< "," << state[4]<< "," << state[5] <<'\n'; - for(; t < tFinal; t += params::timeStep){ - if(static_cast(t) % 100 == 0){ - std::cout << "Time is " << t << '\n'; - } - imu::Vector<6> k1 = sat.dStateDt(state); - imu::Vector<6> k2 = sat.dStateDt(state + k1 * (params::timeStep / 2)); - imu::Vector<6> k3 = sat.dStateDt(state + k2 * (params::timeStep / 2)); - imu::Vector<6> k4 = sat.dStateDt(state + k3 * (params::timeStep)); - imu::Vector<6> k = (k1 + k2*2 + k3*2 + k4).scale(1/6.0); - - state = state + k * params::timeStep; - file << "" << t << "," << state[0]<< "," << state[1]<< "," << state[2]<< "," << state[3]<< "," << state[4]<< "," << state[5] <<'\n'; - - imu::Vector<3> B = sat.getLastBFieldNED(); // magnetic field log (N, E, D in tesla) - bfile << t << "," << B[0] << "," << B[1] << "," << B[2] << '\n'; + // RK4 integration step. + const State13 k1 = satellite_derivatives(t, state, ctx); + const State13 k2 = satellite_derivatives(t + dt / 2.0, add(state, scale(k1, dt / 2.0)), ctx); + const State13 k3 = satellite_derivatives(t + dt / 2.0, add(state, scale(k2, dt / 2.0)), ctx); + const State13 k4 = satellite_derivatives(t + dt, add(state, scale(k3, dt)), ctx); + + State13 incr{}; + for (int i = 0; i < 13; ++i) { + incr[i] = (1.0 / 6.0) * (k1[i] + 2.0 * k2[i] + 2.0 * k3[i] + k4[i]); + } + + for (int i = 0; i < 13; ++i) { + state[i] += dt * incr[i]; } - // file << std::endl; - std::cout << "Simulation Completed." << '\n'; -} -// code to run everything: -// cd C:\Users\chipc\HS2-ADCS_Sim\build; cmake --build .; .\MyProject.exe; cd ../scripts; python plotter.py; python plot_B.py + // Keep the quaternion from drifting numerically. + Quat q{state[6], state[7], state[8], state[9]}; + q = normalize(q); + state[6] = q.q0; + state[7] = q.q1; + state[8] = q.q2; + state[9] = q.q3; + } + + std::cout << "Done. Wrote adcs_output.csv\n"; + return 0; +} diff --git a/src/navigation.cpp b/src/navigation.cpp new file mode 100644 index 0000000..563aea3 --- /dev/null +++ b/src/navigation.cpp @@ -0,0 +1,36 @@ +#include "modules.hpp" + +// Very simple first-order smoothing filter. +// This is still not a real estimator, but it is cleaner than the old sentinel/bias-reset logic. +void navigation_update(const Vec3& BfieldMeasured, + const Vec3& pqrMeasured, + const Vec3& ptpMeasured, + SimContext& ctx) { + const double s = 0.3; + + // First pass: initialize directly from the measurements. + if (!ctx.navInitialized) { + ctx.BfieldNav = BfieldMeasured; + ctx.pqrNav = pqrMeasured; + ctx.ptpNav = ptpMeasured; + + ctx.BfieldNavPrev = ctx.BfieldNav; + ctx.pqrNavPrev = ctx.pqrNav; + ctx.ptpNavPrev = ctx.ptpNav; + ctx.Bdot = {0.0, 0.0, 0.0}; + ctx.navInitialized = true; + return; + } + + const Vec3 zeroBias{0.0, 0.0, 0.0}; + + ctx.BfieldNav = (1.0 - s) * ctx.BfieldNavPrev + s * (BfieldMeasured - zeroBias); + ctx.pqrNav = (1.0 - s) * ctx.pqrNavPrev + s * (pqrMeasured - zeroBias); + ctx.ptpNav = (1.0 - s) * ctx.ptpNavPrev + s * (ptpMeasured - zeroBias); + + ctx.Bdot = (ctx.BfieldNav - ctx.BfieldNavPrev) / ctx.nextSensorUpdate; + + ctx.BfieldNavPrev = ctx.BfieldNav; + ctx.pqrNavPrev = ctx.pqrNav; + ctx.ptpNavPrev = ctx.ptpNav; +} diff --git a/src/satellite.cpp b/src/satellite.cpp index 1486cfa..c184bd0 100644 --- a/src/satellite.cpp +++ b/src/satellite.cpp @@ -1,87 +1,191 @@ -#include "../include/satellite.hpp" -#include "params.hpp" -#include -#include -#include -#include - -using namespace GeographicLib; - -// ------------------------------------------------------------ -// 1. Magnetic field helper (ECEF position -> B_NED in tesla) -// ------------------------------------------------------------ -static imu::Vector<3> compute_B_ned(const imu::Vector<3>& r_ecef_m, - double decimalYear) -{ - static MagneticModel magModel("wmm2020"); // Change if needed, we will eventually use "wmm2025" - static Geocentric earth(Constants::WGS84_a(), Constants::WGS84_f()); - - double lat_deg, lon_deg, h_m; - earth.Reverse(r_ecef_m[0], r_ecef_m[1], r_ecef_m[2], - lat_deg, lon_deg, h_m); - - double Bn, Be, Bd; - magModel(decimalYear, lat_deg, lon_deg, h_m, Bn, Be, Bd); // nT - - imu::Vector<3> B_ned; - B_ned[0] = Bn; - B_ned[1] = Be; - B_ned[2] = Bd; - - // nT to tesla - B_ned[0] *= 1e-9; - B_ned[1] *= 1e-9; - B_ned[2] *= 1e-9; - - return B_ned; +#include "modules.hpp" +#include "frames.hpp" +#include "math.hpp" +#include "quaternion.hpp" +#include "wmm.hpp" +#include + +namespace { + +// Earth constants used by the orbit model. +void planet(SimContext& ctx) { + ctx.R = 6.371e6; + ctx.M = 5.972e24; + ctx.G = 6.67e-11; + ctx.mu = ctx.G * ctx.M; } -// ------------------------------------------------------------ -// 2. Dynamics (gravity + magnetic field readout) -// ------------------------------------------------------------ -imu::Vector<6> Satellite::dStateDt(const imu::Vector<6> &state) -{ - // Unpack state - double x = state[0]; - double y = state[1]; - double z = state[2]; - double xdot = state[3]; - double ydot = state[4]; - double zdot = state[5]; - - imu::Vector<3> position(x, y, z); // Treated as ECEF for now - imu::Vector<3> velocity(xdot, ydot, zdot); - - // Gravity - double rho = position.magnitude(); - imu::Vector<3> rhat = position / rho; - - imu::Vector<3> acceleration = - rhat * (-1.0 * (params::G * params::M / (rho * rho))); - - // Magnetic field model ( for testing only) - double decimalYear = 2025.0; - imu::Vector<3> B_ned = compute_B_ned(position, decimalYear); - - // remember this B-field inside the Satellite object - lastBField_NED = B_ned; - - static int counter = 0; - counter++; - if (counter % 1000 == 0) { - double Bmag = B_ned.magnitude(); - std::cout << "[DEBUG] |B| = " << Bmag << " T" << std::endl; - } - - // Pack derivative of state - imu::Vector<6> stateOut; - - stateOut[0] = velocity[0]; - stateOut[1] = velocity[1]; - stateOut[2] = velocity[2]; - stateOut[3] = acceleration[0]; - stateOut[4] = acceleration[1]; - stateOut[5] = acceleration[2]; - - return stateOut; -} \ No newline at end of file +// Basic spacecraft dimensions and mass. +void satellite_params(SimContext& ctx) { + ctx.ms = 2.6; + ctx.lx = 0.10; + ctx.ly = 0.10; + ctx.lz = 0.20; + ctx.Amax = ctx.lx * ctx.ly; + ctx.lmax = ctx.lz / 2.0; + ctx.CD = 1.0; + ctx.m = ctx.ms; +} + +// Box inertia model for the spacecraft bus. +void inertia_params(SimContext& ctx) { + Mat3 Is{}; + Is[0][0] = (ctx.ms / 12.0) * (ctx.ly * ctx.ly + ctx.lz * ctx.lz); + Is[1][1] = (ctx.ms / 12.0) * (ctx.lx * ctx.lx + ctx.lz * ctx.lz); + Is[2][2] = (ctx.ms / 12.0) * (ctx.lx * ctx.lx + ctx.ly * ctx.ly); + + ctx.Is = Is; + ctx.I = Is; + ctx.invI = inv3(ctx.I); +} + +// Coil parameters used by the magnetorquer model. +void magtorquer_params(SimContext& ctx) { + ctx.n_turns = 84.0; + ctx.A_turn = 0.02; + ctx.maxCurrent_mA = 120.0; +} + +Quat state_to_quat(const State13& s) { + return {s[6], s[7], s[8], s[9]}; +} + +Vec3 state_to_pqr(const State13& s) { + return {s[10], s[11], s[12]}; +} + +// Convert seconds since t=0 into decimal year. +double decimal_year_from_seconds(double year0, double t_seconds) { + const double seconds_per_year = 365.25 * 24.0 * 3600.0; + return year0 + t_seconds / seconds_per_year; +} + +} // namespace + +State13 satellite_derivatives(double t, const State13& state, SimContext& ctx) { + planet(ctx); + satellite_params(ctx); + inertia_params(ctx); + magtorquer_params(ctx); + + const Vec3 r_eci{state[0], state[1], state[2]}; + const Vec3 v_eci{state[3], state[4], state[5]}; + const Quat q = state_to_quat(state); + const Vec3 pqr_body = state_to_pqr(state); + + const double rho = norm(r_eci); + const Vec3 rhat = (rho > 0.0) ? (r_eci / rho) : Vec3{0.0, 0.0, 0.0}; + + // Central gravity only. + const Vec3 Fg = (-(ctx.mu * ctx.m) / (rho * rho)) * rhat; + + // Simple disturbance model. + const double altitude_m = rho - ctx.R; + Vec3 XYZD{0.0, 0.0, 0.0}; + Vec3 LMND{0.0, 0.0, 0.0}; + disturbance(altitude_m, ctx.Amax, ctx.lmax, v_eci, ctx.CD, ctx.BB_truth, XYZD, LMND); + + const Vec3 accel_eci = (1.0 / ctx.m) * (Fg + XYZD); + + // Attitude kinematics and human-readable angles. + const Quat qdot = quat_derivative(q, pqr_body); + const Vec3 ptp = quat_to_euler321(q); + + // Update the truth magnetic field at the requested sample rate. + if (t >= ctx.lastMagUpdate) { + ctx.lastMagUpdate += ctx.nextMagUpdate; + + // Step 1: move from the inertial orbit frame into Earth-fixed coordinates. + const double earth_angle = ctx.greenwichAngle0 + ctx.omegaE * t; + const Vec3 r_ecef = eci_to_ecef(r_eci, earth_angle); + + // Step 2: get the geodetic point needed by WMM. + const GeodeticLLA lla = ecef_to_geodetic_wgs84(r_ecef); + const double decimal_year = decimal_year_from_seconds(ctx.decimalYear0, t); + + // Step 3: ask WMM for the local North/East/Down field in nanoTesla. + double X_nT = 0.0; + double Y_nT = 0.0; + double Z_nT = 0.0; + wmm2025_geodetic_ned_nT(lla.lat_rad * 180.0 / M_PI, + lla.lon_rad * 180.0 / M_PI, + lla.alt_m / 1000.0, + decimal_year, + X_nT, + Y_nT, + Z_nT); + + // Step 4: turn local NED into an ECEF vector. + const Mat3 C_ecef_ned = ned_basis_ecef(lla.lat_rad, lla.lon_rad); + const Vec3 B_ned_nT{X_nT, Y_nT, Z_nT}; + const Vec3 B_ecef_nT = mul(C_ecef_ned, B_ned_nT); + + // Step 5: move ECEF -> ECI -> body frame. + const Vec3 B_eci_nT = ecef_to_eci(B_ecef_nT, earth_angle); + const Mat3 C_IB = TIBquat(q); + const Vec3 B_body_nT = mul(transpose(C_IB), B_eci_nT); + + // Store truth field in Tesla, because that keeps the torque math in SI units. + ctx.BB_truth = 1e-9 * B_body_nT; + } + + // Push truth through the sensor and nav models. + if (t >= ctx.lastSensorUpdate) { + ctx.lastSensorUpdate += ctx.nextSensorUpdate; + + Vec3 BBm = ctx.BB_truth; + Vec3 pqrm = pqr_body; + Vec3 ptpm = ptp; + + sensor_update(BBm, pqrm, ptpm, ctx); + ctx.BfieldMeasured = BBm; + ctx.pqrMeasured = pqrm; + ctx.ptpMeasured = ptpm; + + navigation_update(ctx.BfieldMeasured, ctx.pqrMeasured, ctx.ptpMeasured, ctx); + } + + // Clamp each coil current separately. + const double Imax_A = ctx.maxCurrent_mA * 1e-3; + auto clamp = [](double v, double lo, double hi) { + return (v < lo) ? lo : ((v > hi) ? hi : v); + }; + ctx.current.x = clamp(ctx.current.x, -Imax_A, Imax_A); + ctx.current.y = clamp(ctx.current.y, -Imax_A, Imax_A); + ctx.current.z = clamp(ctx.current.z, -Imax_A, Imax_A); + + // Magnetic dipole and magnetic torque in the body frame. + const Vec3 dipole_body = (ctx.n_turns * ctx.A_turn) * ctx.current; + const Vec3 LMN_mag = cross(dipole_body, ctx.BB_truth); + + const Vec3 LMN_total = LMN_mag + LMND; + + // Rigid-body rotational dynamics in the body frame. + const Vec3 H = mul(ctx.I, pqr_body); + const Vec3 pqrdot = mul(ctx.invI, (LMN_total - cross(pqr_body, H))); + + State13 dst{}; + + // Position derivative is velocity. + dst[0] = v_eci.x; + dst[1] = v_eci.y; + dst[2] = v_eci.z; + + // Velocity derivative is acceleration. + dst[3] = accel_eci.x; + dst[4] = accel_eci.y; + dst[5] = accel_eci.z; + + // Quaternion derivative. + dst[6] = qdot.q0; + dst[7] = qdot.q1; + dst[8] = qdot.q2; + dst[9] = qdot.q3; + + // Body-rate derivative. + dst[10] = pqrdot.x; + dst[11] = pqrdot.y; + dst[12] = pqrdot.z; + + return dst; +} diff --git a/src/sensor.cpp b/src/sensor.cpp new file mode 100644 index 0000000..451a366 --- /dev/null +++ b/src/sensor.cpp @@ -0,0 +1,45 @@ +#include "modules.hpp" + +// Pick bias values once at the start of the run. +// That is much closer to a real sensor than changing the bias every time the nav filter runs. +static void sensor_params(SimContext& ctx) { + ctx.nextSensorUpdate = 1.0; + ctx.fsensor = 1.0; + + const double MagscaleBias = (4e-7) * ctx.fsensor; + ctx.MagFieldBias = MagscaleBias * ctx.rng.rand_m11(); + + const double AngscaleBias = 0.01 * ctx.fsensor; + ctx.AngFieldBias = AngscaleBias * ctx.rng.rand_m11(); + + const double EulerScaleBias = (2.0 * M_PI / 180.0) * ctx.fsensor; + ctx.EulerBias = EulerScaleBias * ctx.rng.rand_m11(); + + ctx.sensorModelInitialized = true; +} + +// Noise gets re-drawn every sample, which is what we want. +static void sensor_noise(SimContext& ctx) { + const double MagscaleNoise = (1e-5) * ctx.fsensor; + ctx.MagFieldNoise = MagscaleNoise * ctx.rng.rand_m11(); + + const double AngscaleNoise = 0.001 * ctx.fsensor; + ctx.AngFieldNoise = AngscaleNoise * ctx.rng.rand_m11(); + + const double EulerScaleNoise = (1.0 * M_PI / 180.0) * ctx.fsensor; + ctx.EulerNoise = EulerScaleNoise * ctx.rng.rand_m11(); +} + +// Apply the sensor model to the truth signals. +void sensor_update(Vec3& BB, Vec3& pqr, Vec3& ptp, SimContext& ctx) { + if (!ctx.sensorModelInitialized) { + sensor_params(ctx); + } + + for (int idx = 0; idx < 3; ++idx) { + sensor_noise(ctx); + BB[idx] = BB[idx] + ctx.MagFieldBias + ctx.MagFieldNoise; + pqr[idx] = pqr[idx] + ctx.AngFieldBias + ctx.AngFieldNoise; + ptp[idx] = ptp[idx] + ctx.EulerBias + ctx.EulerNoise; + } +} diff --git a/src/wmm.cpp b/src/wmm.cpp new file mode 100644 index 0000000..a2ada4b --- /dev/null +++ b/src/wmm.cpp @@ -0,0 +1,326 @@ +#include "wmm.hpp" +#include +#include + +namespace { + +constexpr int kMaxOrd = 12; +constexpr int kSize = kMaxOrd + 1; +constexpr double kEpoch = 2025.0; + +struct WmmCoeff { + int n; + int m; + double g; + double h; + double gd; + double hd; +}; + +// Embedded WMM2025 coefficients. +// Keeping them in the code means the sim runs without any extra data files. +constexpr WmmCoeff kCoeff[] = { + {1,0,-29351.8,0.0,12.0,0.0}, + {1,1,-1410.8,4545.4,9.7,-21.5}, + {2,0,-2556.6,0.0,-11.6,0.0}, + {2,1,2951.1,-3133.6,-5.2,-27.7}, + {2,2,1649.3,-815.1,-8.0,-12.1}, + {3,0,1361.0,0.0,-1.3,0.0}, + {3,1,-2404.1,-56.6,-4.2,4.0}, + {3,2,1243.8,237.5,0.4,-0.3}, + {3,3,453.6,-549.5,-15.6,-4.1}, + {4,0,895.0,0.0,-1.6,0.0}, + {4,1,799.5,278.6,-2.4,-1.1}, + {4,2,55.7,-133.9,-6.0,4.1}, + {4,3,-281.1,212.0,5.6,1.6}, + {4,4,12.1,-375.6,-7.0,-4.4}, + {5,0,-233.2,0.0,0.6,0.0}, + {5,1,368.9,45.4,1.4,-0.5}, + {5,2,187.2,220.2,0.0,2.2}, + {5,3,-138.7,-122.9,0.6,0.4}, + {5,4,-142.0,43.0,2.2,1.7}, + {5,5,20.9,106.1,0.9,1.9}, + {6,0,64.4,0.0,-0.2,0.0}, + {6,1,63.8,-18.4,-0.4,0.3}, + {6,2,76.9,16.8,0.9,-1.6}, + {6,3,-115.7,48.8,1.2,-0.4}, + {6,4,-40.9,-59.8,-0.9,0.9}, + {6,5,14.9,10.9,0.3,0.7}, + {6,6,-60.7,72.7,0.9,0.9}, + {7,0,79.5,0.0,-0.0,0.0}, + {7,1,-77.0,-48.9,-0.1,0.6}, + {7,2,-8.8,-14.4,-0.1,0.5}, + {7,3,59.3,-1.0,0.5,-0.8}, + {7,4,15.8,23.4,-0.1,0.0}, + {7,5,2.5,-7.4,-0.8,-1.0}, + {7,6,-11.1,-25.1,-0.8,0.6}, + {7,7,14.2,-2.3,0.8,-0.2}, + {8,0,23.2,0.0,-0.1,0.0}, + {8,1,10.8,7.1,0.2,-0.2}, + {8,2,-17.5,-12.6,0.0,0.5}, + {8,3,2.0,11.4,0.5,-0.4}, + {8,4,-21.7,-9.7,-0.1,0.4}, + {8,5,16.9,12.7,0.3,-0.5}, + {8,6,15.0,0.7,0.2,-0.6}, + {8,7,-16.8,-5.2,-0.0,0.3}, + {8,8,0.9,3.9,0.2,0.2}, + {9,0,4.6,0.0,-0.0,0.0}, + {9,1,7.8,-24.8,-0.1,-0.3}, + {9,2,3.0,12.2,0.1,0.3}, + {9,3,-0.2,8.3,0.3,-0.3}, + {9,4,-2.5,-3.3,-0.3,0.3}, + {9,5,-13.1,-5.2,0.0,0.2}, + {9,6,2.4,7.2,0.3,-0.1}, + {9,7,8.6,-0.6,-0.1,-0.2}, + {9,8,-8.7,0.8,0.1,0.4}, + {9,9,-12.9,10.0,-0.1,0.1}, + {10,0,-1.3,0.0,0.1,0.0}, + {10,1,-6.4,3.3,0.0,0.0}, + {10,2,0.2,0.0,0.1,-0.0}, + {10,3,2.0,2.4,0.1,-0.2}, + {10,4,-1.0,5.3,-0.0,0.1}, + {10,5,-0.6,-9.1,-0.3,-0.1}, + {10,6,-0.9,0.4,0.0,0.1}, + {10,7,1.5,-4.2,-0.1,0.0}, + {10,8,0.9,-3.8,-0.1,-0.1}, + {10,9,-2.7,0.9,-0.0,0.2}, + {10,10,-3.9,-9.1,-0.0,-0.0}, + {11,0,2.9,0.0,0.0,0.0}, + {11,1,-1.5,0.0,-0.0,-0.0}, + {11,2,-2.5,2.9,0.0,0.1}, + {11,3,2.4,-0.6,0.0,-0.0}, + {11,4,-0.6,0.2,0.0,0.1}, + {11,5,-0.1,0.5,-0.1,-0.0}, + {11,6,-0.6,-0.3,0.0,-0.0}, + {11,7,-0.1,-1.2,-0.0,0.1}, + {11,8,1.1,-1.7,-0.1,-0.0}, + {11,9,-1.0,-2.9,-0.1,0.0}, + {11,10,-0.2,-1.8,-0.1,0.0}, + {11,11,2.6,-2.3,-0.1,0.0}, + {12,0,-2.0,0.0,0.0,0.0}, + {12,1,-0.2,-1.3,0.0,-0.0}, + {12,2,0.3,0.7,-0.0,0.0}, + {12,3,1.2,1.0,-0.0,-0.1}, + {12,4,-1.3,-1.4,-0.0,0.1}, + {12,5,0.6,-0.0,-0.0,-0.0}, + {12,6,0.6,0.6,0.1,-0.0}, + {12,7,0.5,-0.1,-0.0,-0.0}, + {12,8,-0.1,0.8,0.0,0.0}, + {12,9,-0.4,0.1,0.0,-0.0}, + {12,10,-0.2,-1.0,-0.1,-0.0}, + {12,11,-1.3,0.1,-0.0,0.0}, + {12,12,-0.7,0.2,-0.1,-0.1} +}; + +struct WmmTables { + double c[kSize][kSize]{}; + double cd[kSize][kSize]{}; + double snorm[kSize * kSize]{}; + double fn[kSize]{}; + double fm[kSize]{}; + double k[kSize][kSize]{}; +}; + +const WmmTables& tables() { + static const WmmTables t = [] { + WmmTables out{}; + + // Read the coefficient list into the legacy matrix layout. + for (const auto& entry : kCoeff) { + out.c[entry.m][entry.n] = entry.g; + out.cd[entry.m][entry.n] = entry.gd; + if (entry.m != 0) { + out.c[entry.n][entry.m - 1] = entry.h; + out.cd[entry.n][entry.m - 1] = entry.hd; + } + } + + // Convert Schmidt-normalized coefficients into the unnormalized form used by the old WMM code. + out.snorm[0] = 1.0; + out.fm[0] = 0.0; + + for (int n = 1; n <= kMaxOrd; ++n) { + out.snorm[n] = out.snorm[n - 1] * static_cast(2 * n - 1) / static_cast(n); + + int j = 2; + int m = 0; + while (m <= n) { + if (n > 1) { + out.k[m][n] = static_cast(((n - 1) * (n - 1)) - (m * m)) / + static_cast((2 * n - 1) * (2 * n - 3)); + } + + if (m > 0) { + const double flnmj = static_cast((n - m + 1) * j) / static_cast(n + m); + out.snorm[n + m * kSize] = out.snorm[n + (m - 1) * kSize] * std::sqrt(flnmj); + j = 1; + + out.c[n][m - 1] *= out.snorm[n + m * kSize]; + out.cd[n][m - 1] *= out.snorm[n + m * kSize]; + } + + out.c[m][n] *= out.snorm[n + m * kSize]; + out.cd[m][n] *= out.snorm[n + m * kSize]; + ++m; + } + + out.fn[n] = static_cast(n + 1); + out.fm[n] = static_cast(n); + } + + out.k[1][1] = 0.0; + return out; + }(); + + return t; +} + +} // namespace + +void wmm2025_geodetic_ned_nT(double latitude_deg, + double longitude_deg, + double altitude_km, + double decimal_year, + double& X_nT, + double& Y_nT, + double& Z_nT) { + if (decimal_year < 2025.0 || decimal_year > 2030.0) { + throw std::runtime_error("WMM2025 is only valid from 2025.0 to 2030.0."); + } + + const auto& wmm = tables(); + + // Working arrays used by the legacy WMM recurrence relations. + double tc[kSize][kSize]{}; + double dp[kSize][kSize]{}; + double sp[kSize]{}; + double cp[kSize]{}; + double pp[kSize]{}; + double p[kSize * kSize]{}; + + for (int i = 0; i < kSize * kSize; ++i) { + p[i] = wmm.snorm[i]; + } + + // WGS84 ellipsoid constants used by the legacy WMM code. + const double a = 6378.137; + const double b = 6356.7523142; + const double re = 6371.2; + const double a2 = a * a; + const double b2 = b * b; + const double c2 = a2 - b2; + const double a4 = a2 * a2; + const double b4 = b2 * b2; + const double c4 = a4 - b4; + + const double dt = decimal_year - kEpoch; + + const double rlon = longitude_deg * M_PI / 180.0; + const double rlat = latitude_deg * M_PI / 180.0; + const double srlon = std::sin(rlon); + const double srlat = std::sin(rlat); + const double crlon = std::cos(rlon); + const double crlat = std::cos(rlat); + const double srlat2 = srlat * srlat; + const double crlat2 = crlat * crlat; + + sp[0] = 0.0; + cp[0] = 1.0; + pp[0] = 1.0; + dp[0][0] = 0.0; + sp[1] = srlon; + cp[1] = crlon; + + // Convert geodetic input into the spherical quantities used by the field equations. + const double q = std::sqrt(a2 - c2 * srlat2); + const double q1 = altitude_km * q; + const double q2 = ((q1 + a2) / (q1 + b2)) * ((q1 + a2) / (q1 + b2)); + const double ct = srlat / std::sqrt(q2 * crlat2 + srlat2); + const double st = std::sqrt(1.0 - ct * ct); + const double r2 = (altitude_km * altitude_km) + 2.0 * q1 + (a4 - c4 * srlat2) / (q * q); + const double r = std::sqrt(r2); + const double d = std::sqrt(a2 * crlat2 + b2 * srlat2); + const double ca = (altitude_km + d) / r; + const double sa = c2 * crlat * srlat / (r * d); + + for (int m = 2; m <= kMaxOrd; ++m) { + sp[m] = sp[1] * cp[m - 1] + cp[1] * sp[m - 1]; + cp[m] = cp[1] * cp[m - 1] - sp[1] * sp[m - 1]; + } + + double aor = re / r; + double ar = aor * aor; + double br = 0.0; + double bt = 0.0; + double bp = 0.0; + double bpp = 0.0; + + for (int n = 1; n <= kMaxOrd; ++n) { + ar *= aor; + + int m = 0; + while (m <= n) { + // Compute the unnormalized associated Legendre polynomials and their derivatives. + if (n == m) { + p[n + m * kSize] = st * p[n - 1 + (m - 1) * kSize]; + dp[m][n] = st * dp[m - 1][n - 1] + ct * p[n - 1 + (m - 1) * kSize]; + } else if (n == 1 && m == 0) { + p[n + m * kSize] = ct * p[n - 1 + m * kSize]; + dp[m][n] = ct * dp[m][n - 1] - st * p[n - 1 + m * kSize]; + } else if (n > 1 && n != m) { + if (m > n - 2) { + p[n - 2 + m * kSize] = 0.0; + dp[m][n - 2] = 0.0; + } + p[n + m * kSize] = ct * p[n - 1 + m * kSize] - wmm.k[m][n] * p[n - 2 + m * kSize]; + dp[m][n] = ct * dp[m][n - 1] - st * p[n - 1 + m * kSize] - wmm.k[m][n] * dp[m][n - 2]; + } + + // Time-shift the coefficients from epoch 2025.0 to the requested decimal year. + tc[m][n] = wmm.c[m][n] + dt * wmm.cd[m][n]; + if (m != 0) { + tc[n][m - 1] = wmm.c[n][m - 1] + dt * wmm.cd[n][m - 1]; + } + + // Accumulate the spherical harmonic expansion. + const double par = ar * p[n + m * kSize]; + double temp1 = 0.0; + double temp2 = 0.0; + if (m == 0) { + temp1 = tc[m][n] * cp[m]; + temp2 = tc[m][n] * sp[m]; + } else { + temp1 = tc[m][n] * cp[m] + tc[n][m - 1] * sp[m]; + temp2 = tc[m][n] * sp[m] - tc[n][m - 1] * cp[m]; + } + + bt -= ar * temp1 * dp[m][n]; + bp += wmm.fm[m] * temp2 * par; + br += wmm.fn[n] * temp1 * par; + + // Special handling at the poles. + if (st == 0.0 && m == 1) { + if (n == 1) { + pp[n] = pp[n - 1]; + } else { + pp[n] = ct * pp[n - 1] - wmm.k[m][n] * pp[n - 2]; + } + const double parp = ar * pp[n]; + bpp += wmm.fm[m] * temp2 * parp; + } + + ++m; + } + } + + if (st == 0.0) { + bp = bpp; + } else { + bp /= st; + } + + // Rotate from spherical components into local geodetic north/east/down. + X_nT = -bt * ca - br * sa; + Y_nT = bp; + Z_nT = bt * sa - br * ca; +} From f7db195562abbf1ff91b5296a8a67b7a6516adde Mon Sep 17 00:00:00 2001 From: jtran34-ctrl Date: Wed, 25 Mar 2026 20:25:44 -0700 Subject: [PATCH 2/3] Remove build artifacts --- .gitignore | Bin 413 -> 477 bytes 1 file changed, 0 insertions(+), 0 deletions(-) diff --git a/.gitignore b/.gitignore index 3ad34c5be2fea384c4abb694ba0a3f075f2ae1fe..b921946aa291235a64c06ef08f533793f845b527 100644 GIT binary patch literal 477 zcmZ8dNp8b14AfbG{~#c@9^ywd4SFch0)2p?g>jh3lB&gxe_u*YU^Ex*GaPa!z9K~U zDVQ)r>iEmvbiCs`@#?357L52*7tmRpC~5ss)I7-?iUa=4W;*v0W^yZKmz1xVIbVlBK%;25wMie93JzgjI&AcL{VSA19*s zZc}1@NqS{MLr+izt?4_Sbi|NTzn{U)l+*JT-SJ@{C-oVIfr_*xluW^XPTCvj6)Oyx LWrp>vW1Rf~qqv3T literal 413 zcmZ9HO-{rx42AbOMWorWXb!NV=ks%X&~F6` zesrqTu&Rdvlsf3gv}6hh`qf?0H+|+r9&1Rf-y)afu0peg|800i3s{7m{Dn2MGV(%k z#Sf2P3_lfkmc5p(W%x}opQy8H5YLw-uV36!${APZ=U=TsKOV From fa401cf4b8d72e8c0fb11f05f25f91b540429cd1 Mon Sep 17 00:00:00 2001 From: jtran34-ctrl Date: Wed, 25 Mar 2026 20:35:22 -0700 Subject: [PATCH 3/3] Update README_WMM_UPDATE notes --- README_WMM_UPDATE.txt | 17 ++++++++++++++++- 1 file changed, 16 insertions(+), 1 deletion(-) diff --git a/README_WMM_UPDATE.txt b/README_WMM_UPDATE.txt index e18af4d..b17afea 100644 --- a/README_WMM_UPDATE.txt +++ b/README_WMM_UPDATE.txt @@ -25,4 +25,19 @@ Build Notes - This is a much better magnetic environment model than the old placeholder, but it is still not a full flight-quality simulator. - The disturbance and sensor models are still simple. -- The navigation block is still just a smoothing filter, not a real estimator. \ No newline at end of file +- The navigation block is still just a smoothing filter, not a real estimator. + +If you get an error, run +Remove-Item -Recurse -Force build -ErrorAction SilentlyContinue +cmake -S . -B build -G "MinGW Makefiles" +cmake --build build +if (Test-Path .\build\adcs.exe) { + .\build\adcs.exe +} elseif (Test-Path .\build\Debug\adcs.exe) { + .\build\Debug\adcs.exe +} elseif (Test-Path .\build\Release\adcs.exe) { + .\build\Release\adcs.exe +} else { + Write-Host "adcs.exe not found after build" +} +python .\plot_all.py \ No newline at end of file