├── .gitignore
├── README.md
├── adcs.png
├── attitude
    ├── Makefile
    ├── README.md
    ├── attitude.h
    ├── axan.c
    ├── axan.h
    ├── dcm.c
    ├── dcm.h
    ├── euler.c
    ├── euler.h
    ├── quat.c
    ├── quat.h
    └── test_att.c
├── igrf
    ├── README.md
    ├── c
    │   ├── Makefile
    │   ├── igrf.c
    │   ├── igrf.h
    │   ├── igrf_2025.h
    │   └── main.c
    └── matlab
    │   ├── igrf.m
    │   ├── igrf_coeffs
    │       ├── igrf_2020.txt
    │       └── norm_g_2020.txt
    │   └── main.m
└── spacetime
    ├── Makefile
    ├── README.md
    ├── frame.c
    ├── frame.h
    ├── iau06
        ├── iau06.c
        ├── iau06.h
        ├── iau06_data.c
        └── iau06_data.h
    ├── test_frame.c
    ├── tests.c
    ├── time.c
    └── time.h


/.gitignore:
--------------------------------------------------------------------------------
1 | build
2 | .vscode
3 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # adcs
 2 | 
 3 | <p align="center">
 4 |   <img width="150" src="adcs.png">
 5 | </p>
 6 | 
 7 | Utilities and algorithms for the Attitude Determination and Control System (ADCS) of spacecraft, targeted toward embedded hardware.
 8 | 
 9 | ## Contents
10 | 
11 | 1. [igrf](/igrf/) - International Geomagnetic Reference Field (IGRF-14) Model
12 | 2. [attitude](https://github.com/risherlock/adcs/tree/attitude/attitude) - Basic tools for rotational manipulations using SO(3), quaternions, and Euler angles
13 | 3. [spacetime](https://github.com/risherlock/adcs/tree/attitude/spacetime) - Coordinate transformations and time conversion utilities.
14 | 


--------------------------------------------------------------------------------
/adcs.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/risherlock/adcs/a3254bbc516fe128b21881ecd44f862df9ba0701/adcs.png


--------------------------------------------------------------------------------
/attitude/Makefile:
--------------------------------------------------------------------------------
 1 | CC = gcc
 2 | CFLAGS = -std=c99 -Wall -Wextra -Werror -lm -O3
 3 | 
 4 | TARGET = test
 5 | BUILD_DIR = build
 6 | 
 7 | INC =
 8 | SRC = dcm.c euler.c quat.c axan.c test_att.c
 9 | 
10 | OBJ = $(SRC:%.c=$(BUILD_DIR)/%.o)
11 | 
12 | $(BUILD_DIR)/%.o: %.c | $(BUILD_DIR)
13 | 	$(CC) $(CFLAGS) $(INC) -c $< -o $@
14 | 
15 | $(TARGET): $(OBJ)
16 | 	$(CC) $(CFLAGS) $(INC) -o $(BUILD_DIR)/$(TARGET) $(OBJ)
17 | 


--------------------------------------------------------------------------------
/attitude/README.md:
--------------------------------------------------------------------------------
 1 | # attitude
 2 | 
 3 | Basic rotation manipulations for ADCS implemented using the C99 standard.
 4 | 
 5 | ## Notes
 6 | 
 7 | 1. DCM represents rotation from intertial to body frame.
 8 | 
 9 | ### Todos
10 | 
11 | 1. Unit tests.
12 | 
13 | ### Queries
14 | 
15 | 1. Normalize error quaternion?
16 | 2. Normalize quaternion from axis-angle?
17 | 3. Euler sequences enum versus hard-code.
18 | 
19 | ## References
20 | 
21 | 1. Henderson - Euler Angles, quaternions, and transformation matrices: Working relationships (1977)
22 | 2. Bernardes, Viollet - Quaternion to Euler angles conversion: A direct, general and computationally efficient method (2022)
23 | 3. Motekew - Quaternion to DCM and back again (2014)
24 | 4. Waveren - From quaternion to matrix and back (2005)
25 | 5. Schaub, Junkins - Analytical mechanics of space systems - 4th Ed. (2018)
26 | 6. Shoemake - Animating rotation with Quaternion Curves (1985)
27 | 7. Markley et.al. - Averaging quaternions (2007)
28 | 


--------------------------------------------------------------------------------
/attitude/attitude.h:
--------------------------------------------------------------------------------
 1 | #ifndef _ATTITUDE_H_
 2 | #define _ATTITUDE_H_
 3 | 
 4 | #ifdef __cplusplus
 5 | extern "C" {
 6 | #endif
 7 | 
 8 | #include "dcm.h"
 9 | #include "quat.h"
10 | #include "euler.h"
11 | 
12 | #endif // attitude.h
13 | 


--------------------------------------------------------------------------------
/attitude/axan.c:
--------------------------------------------------------------------------------
 1 | #include <math.h>
 2 | #include "axan.h"
 3 | 
 4 | /**
 5 |  * @brief Computes the quaternion corresponding to a rotation by an angle `psi` [rad] about the unit
 6 |  *        axis vector `v`.
 7 |  *
 8 |  * @param psi Rotation angle in radians
 9 |  * @param v Unit vector representing the axis of rotation
10 |  * @param q Output quaternion
11 |  */
12 | void axan_to_quat(const double psi, const double v[3], double q[4])
13 | {
14 |   double hp = psi / 2.0;
15 |   double shp = sin(hp);
16 | 
17 |   q[0] = cos(hp);
18 |   q[1] = shp * v[0];
19 |   q[2] = shp * v[1];
20 |   q[3] = shp * v[2];
21 | }
22 | 


--------------------------------------------------------------------------------
/attitude/axan.h:
--------------------------------------------------------------------------------
 1 | // Basic axis-angle transformations.
 2 | // rms (2025-04-02 1:19:22 AM)
 3 | 
 4 | #ifndef _ATTITUDE_AXAN_H_
 5 | #define _ATTITUDE_AXAN_H_
 6 | 
 7 | #ifdef __cplusplus
 8 | extern "C" {
 9 | #endif
10 | 
11 | void axan_to_quat(const double psi, const double v[3], double q[4]);
12 | 
13 | #ifdef __cplusplus
14 | }
15 | #endif
16 | 
17 | #endif // axan.h
18 | 


--------------------------------------------------------------------------------
/attitude/dcm.c:
--------------------------------------------------------------------------------
  1 | #include <math.h>
  2 | #include <inttypes.h>
  3 | 
  4 | #include "dcm.h"
  5 | 
  6 | // DCM corresponding to a rotation by angle `xi` [rad] about the x-axis.
  7 | void dcm_x(const double xi, double m[3][3])
  8 | {
  9 |   m[0][0] = 1.0; m[1][0] = 0.0;     m[2][0] = 0.0;
 10 |   m[0][1] = 0.0; m[1][1] = cos(xi); m[2][1] = -sin(xi);
 11 |   m[0][2] = 0.0; m[1][2] = sin(xi); m[2][2] = cos(xi);
 12 | }
 13 | 
 14 | // DCM corresponding to a rotation by angle `xi` [rad] about the y-axis.
 15 | void dcm_y(const double xi, double m[3][3])
 16 | {
 17 |   m[0][0] = cos(xi);  m[1][0] = 0.0; m[2][0] = sin(xi);
 18 |   m[0][1] = 0.0;      m[1][1] = 1.0; m[2][1] = 0.0;
 19 |   m[0][2] = -sin(xi); m[1][2] = 0.0; m[2][2] = cos(xi);
 20 | }
 21 | 
 22 | // DCM corresponding to a rotation by angle `xi` [rad] about the z-axis.
 23 | void dcm_z(const double xi, double m[3][3])
 24 | {
 25 |   m[0][0] = cos(xi); m[1][0] = -sin(xi); m[2][0] = 0.0;
 26 |   m[0][1] = sin(xi); m[1][1] = cos(xi);  m[2][1] = 0.0;
 27 |   m[0][2] = 0.0;     m[1][2] = 0.0;      m[2][2] = 1.0;
 28 | }
 29 | 
 30 | // DCM corresponding to no rotation.
 31 | void dcm_unit(double m[3][3])
 32 | {
 33 |   m[0][0] = 1.0; m[0][1] = 0.0; m[0][2] = 0.0;
 34 |   m[1][0] = 0.0; m[1][1] = 1.0; m[1][2] = 0.0;
 35 |   m[2][0] = 0.0; m[2][1] = 0.0; m[2][2] = 1.0;
 36 | }
 37 | 
 38 | // Computes transpose of input DCM/
 39 | void dcm_trans(const double m[3][3], double t[3][3])
 40 | {
 41 |   for (int i = 0; i < 3; i++)
 42 |   {
 43 |     for (int j = 0; j < 3; j++) {
 44 |       t[j][i] = m[i][j];
 45 |     }
 46 |   }
 47 | }
 48 | 
 49 | // DCM multiplication: m = a * b
 50 | void dcm_prod(const double a[3][3], const double b[3][3], double m[3][3])
 51 | {
 52 |   for (int i = 0; i < 3; i++)
 53 |   {
 54 |     for (int j = 0; j < 3; j++)
 55 |     {
 56 |       m[i][j] = 0.0;
 57 | 
 58 |       for (int k = 0; k < 3; k++)
 59 |       {
 60 |         m[i][j] += a[i][k] * b[k][j];
 61 |       }
 62 |     }
 63 |   }
 64 | }
 65 | 
 66 | // Rotates the input vector `v` using the DCM `m` to produce the output vector `v_out`.
 67 | void dcm_rotate(const double m[3][3], const double v[3], double v_out[3])
 68 | {
 69 |   for (int i = 0; i < 3; i++)
 70 |   {
 71 |     v_out[i] = 0.0;
 72 | 
 73 |     for (int j = 0; j < 3; j++)
 74 |     {
 75 |       v_out[i] += m[i][j] * v[j];
 76 |     }
 77 |   }
 78 | }
 79 | 
 80 | // Computes the quaternion corresponding to the input DCM.
 81 | void dcm_to_quat(const double r[3][3], double q[4])
 82 | {
 83 |   double trace = r[0][0] + r[1][1] + r[2][2];
 84 |   double s;
 85 | 
 86 |   if (trace > 0)
 87 |   {
 88 |     s = 2.0 * sqrt(1.0 + trace);
 89 |     q[0] = 0.25 * s;
 90 |     q[1] = (r[2][1] - r[1][2]) / s;
 91 |     q[2] = (r[0][2] - r[2][0]) / s;
 92 |     q[3] = (r[1][0] - r[0][1]) / s;
 93 |   }
 94 |   else
 95 |   {
 96 |     if (r[0][0] > r[1][1] && r[0][0] > r[2][2])
 97 |     {
 98 |       s = 2.0 * sqrt(1.0 + r[0][0] - r[1][1] - r[2][2]);
 99 |       q[0] = (r[2][1] - r[1][2]) / s;
100 |       q[1] = 0.25 * s;
101 |       q[2] = (r[0][1] + r[1][0]) / s;
102 |       q[3] = (r[0][2] + r[2][0]) / s;
103 |     }
104 |     else if (r[1][1] > r[2][2])
105 |     {
106 |       s = 2.0 * sqrt(1.0 + r[1][1] - r[0][0] - r[2][2]);
107 |       q[0] = (r[0][2] - r[2][0]) / s;
108 |       q[1] = (r[0][1] + r[1][0]) / s;
109 |       q[2] = 0.25 * s;
110 |       q[3] = (r[1][2] + r[2][1]) / s;
111 |     }
112 |     else
113 |     {
114 |       s = 2.0 * sqrt(1.0 + r[2][2] - r[0][0] - r[1][1]) * 2;
115 |       q[0] = (r[1][0] - r[0][1]) / s;
116 |       q[1] = (r[0][2] + r[2][0]) / s;
117 |       q[2] = (r[1][2] + r[2][1]) / s;
118 |       q[3] = 0.25 * s;
119 |     }
120 |   }
121 | }
122 | 
123 | // Computes Euler angles from the input DCM using the sequence specified by `es`.
124 | void dcm_to_euler(const double m[3][3], double e[3], const euler_seq_t es)
125 | {
126 |   switch (es)
127 |   {
128 |   case EULER_XYZ:
129 |   {
130 |     e[0] = atan2(-m[2][1], m[2][2]);
131 |     e[1] = asin(m[2][0]);
132 |     e[2] = atan2(-m[1][0], m[0][0]);
133 |     break;
134 |   }
135 | 
136 |   case EULER_XZY:
137 |   {
138 |     e[0] = atan2(m[1][2], m[1][1]);
139 |     e[1] = asin(-m[1][0]);
140 |     e[2] = atan2(m[2][0], m[0][0]);
141 |     break;
142 |   }
143 | 
144 |   case EULER_XYX:
145 |   {
146 |     e[0] = atan2(m[0][1], -m[0][2]);
147 |     e[1] = acos(m[0][0]);
148 |     e[2] = atan2(m[1][0], m[2][0]);
149 |     break;
150 |   }
151 | 
152 |   case EULER_XZX:
153 |   {
154 |     e[0] = atan2(m[0][2], m[0][1]);
155 |     e[1] = acos(m[0][0]);
156 |     e[2] = atan2(m[2][0], -m[1][0]);
157 |     break;
158 |   }
159 | 
160 |   case EULER_YXZ:
161 |   {
162 |     e[0] = atan2(m[2][0], m[2][2]);
163 |     e[1] = asin(-m[2][1]);
164 |     e[2] = atan2(m[0][1], m[1][1]);
165 |     break;
166 |   }
167 | 
168 |   case EULER_YZX:
169 |   {
170 |     e[0] = atan2(-m[0][2], m[0][0]);
171 |     e[1] = asin(m[0][1]);
172 |     e[2] = atan2(-m[2][1], m[1][1]);
173 |     break;
174 |   }
175 | 
176 |   case EULER_YXY:
177 |   {
178 |     e[0] = atan2(m[1][0], m[1][2]);
179 |     e[1] = acos(m[1][1]);
180 |     e[2] = atan2(m[0][1], -m[2][1]);
181 |     break;
182 |   }
183 | 
184 |   case EULER_YZY:
185 |   {
186 |     e[0] = atan2(m[1][2], -m[1][0]);
187 |     e[1] = acos(m[1][1]);
188 |     e[2] = atan2(m[2][1], m[0][1]);
189 |     break;
190 |   }
191 | 
192 |   case EULER_ZXY:
193 |   {
194 |     e[0] = atan2(-m[1][0], m[1][1]);
195 |     e[1] = asin(m[1][2]);
196 |     e[2] = atan2(-m[0][2], m[2][2]);
197 |     break;
198 |   }
199 | 
200 |   case EULER_ZYX:
201 |   {
202 |     e[0] = atan2(m[0][1], m[0][0]);
203 |     e[1] = asin(-m[0][2]);
204 |     e[2] = atan2(m[1][2], m[2][2]);
205 |     break;
206 |   }
207 | 
208 |   case EULER_ZXZ:
209 |   {
210 |     e[0] = atan2(m[2][0], -m[2][1]);
211 |     e[1] = acos(m[2][2]);
212 |     e[2] = atan2(m[0][2], m[1][2]);
213 |     break;
214 |   }
215 | 
216 |   case EULER_ZYZ:
217 |   {
218 |     e[0] = atan2(m[2][1], m[2][0]);
219 |     e[1] = acos(m[2][2]);
220 |     e[2] = atan2(m[1][2], -m[0][2]);
221 |     break;
222 |   }
223 |   }
224 | }
225 | 
226 | /**
227 |  * @brief Time derivative of DCM for given angular velocity expressed in the body frame.
228 |  *
229 |  * @note  This function assumes that the input DCM represents a rotation from the inertial frame to
230 |  *        the body frame.
231 |  *
232 |  * @warning  If the DCM represents a rotation from the body frame to the inertial frame, the
233 |  *           following equation applies: m_dot = m x [w]. In this case, the angular velocity is
234 |  *           expressed in the inertial frame.
235 |  *
236 |  * @todo Explore how same function would be used for the case in the warning.
237 |  */
238 | void dcm_rate(const double w[3], const double m[3][3], double m_dot[3][3])
239 | {
240 |   // m_dot = -[w] x m where, [w] is skew-symmetric matrix of w.
241 |   const double wx[3][3] = {{0.0, w[2],  -w[1]}, {-w[2], 0.0, w[0]}, {w[1], -w[0], 0.0}};
242 |   dcm_prod(wx, m, m_dot);
243 | }
244 | 


--------------------------------------------------------------------------------
/attitude/dcm.h:
--------------------------------------------------------------------------------
 1 | // Basic Direction Cosine Matrix manipulations.
 2 | // 2024-12-23
 3 | 
 4 | #ifndef _ATTITUDE_DCM_H_
 5 | #define _ATTITUDE_DCM_H_
 6 | 
 7 | #include "euler.h"
 8 | 
 9 | #ifdef __cplusplus
10 | extern "C" {
11 | #endif
12 | 
13 | void dcm_unit(double m[3][3]);
14 | void dcm_x(const double xi, double m[3][3]);
15 | void dcm_y(const double xi, double m[3][3]);
16 | void dcm_z(const double xi, double m[3][3]);
17 | void dcm_trans(const double m[3][3], double t[3][3]);
18 | void dcm_prod(const double a[3][3], const double b[3][3], double m[3][3]);
19 | void dcm_rotate(const double m[3][3], const double v[3], double v_out[3]);
20 | void dcm_to_euler(const double m[3][3], double e[3], const euler_seq_t es);
21 | void dcm_rate(const double w[3], const double m[3][3], double m_dot[3][3]);
22 | 
23 | #ifdef __cplusplus
24 | }
25 | #endif
26 | 
27 | #endif // dcm.h
28 | 


--------------------------------------------------------------------------------
/attitude/euler.c:
--------------------------------------------------------------------------------
  1 | #include <math.h>
  2 | #include <stdbool.h>
  3 | #include <inttypes.h>
  4 | 
  5 | #include "euler.h"
  6 | 
  7 | // Computes quaternion from the input Euler angles using the sequence specified by `es`.
  8 | void euler_to_quat(const double e[3], const euler_seq_t es, double q[4])
  9 | {
 10 |   const double c[3] = {cos(0.5 * e[0]), cos(0.5 * e[1]), cos(0.5 * e[2])};
 11 |   const double s[3] = {sin(0.5 * e[0]), sin(0.5 * e[1]), sin(0.5 * e[2])};
 12 |   const double h[3] = {0.5 * e[0], 0.5 * e[1], 0.5 * e[2]};
 13 |   const double ch = cos(h[1]);
 14 |   const double sh = sin(h[1]);
 15 | 
 16 |   switch (es)
 17 |   {
 18 |   case EULER_XYZ:
 19 |   {
 20 |     q[0] = c[0] * c[1] * c[2] - s[0] * s[1] * s[2];
 21 |     q[1] = s[0] * c[1] * c[2] + c[0] * s[1] * s[2];
 22 |     q[2] = c[0] * s[1] * c[2] - s[0] * c[1] * s[2];
 23 |     q[3] = c[0] * c[1] * s[2] + s[0] * s[1] * c[2];
 24 |     break;
 25 |   }
 26 | 
 27 |   case EULER_XZY:
 28 |   {
 29 |     q[0] = c[0] * c[1] * c[2] + s[0] * s[1] * s[2];
 30 |     q[1] = s[0] * c[1] * c[2] - c[0] * s[1] * s[2];
 31 |     q[2] = c[0] * c[1] * s[2] - s[0] * s[1] * c[2];
 32 |     q[3] = c[0] * s[1] * c[2] + s[0] * c[1] * s[2];
 33 |     break;
 34 |   }
 35 | 
 36 |   case EULER_XYX:
 37 |   {
 38 |     q[0] = ch * cos(h[0] + h[2]);
 39 |     q[1] = ch * sin(h[0] + h[2]);
 40 |     q[2] = sh * cos(h[0] - h[2]);
 41 |     q[3] = sh * sin(h[0] - h[2]);
 42 |     break;
 43 |   }
 44 | 
 45 |   case EULER_XZX:
 46 |   {
 47 |     q[0] = ch * cos(h[0] + h[2]);
 48 |     q[1] = ch * sin(h[0] + h[2]);
 49 |     q[2] = sh * sin(-h[0] + h[2]);
 50 |     q[3] = sh * cos(-h[0] + h[2]);
 51 |     break;
 52 |   }
 53 | 
 54 |   case EULER_YXZ:
 55 |   {
 56 |     q[0] = c[0] * c[1] * c[2] + s[0] * s[1] * s[2];
 57 |     q[1] = c[0] * s[1] * c[2] + s[0] * c[1] * s[2];
 58 |     q[2] = s[0] * c[1] * c[2] - c[0] * s[1] * s[2];
 59 |     q[3] = c[0] * c[1] * s[2] - s[0] * s[1] * c[2];
 60 |     break;
 61 |   }
 62 | 
 63 |   case EULER_YZX:
 64 |   {
 65 |     q[0] = c[0] * c[1] * c[2] - s[0] * s[1] * s[2];
 66 |     q[1] = c[0] * c[1] * s[2] + s[0] * s[1] * c[2];
 67 |     q[2] = s[0] * c[1] * c[2] + c[0] * s[1] * s[2];
 68 |     q[3] = c[0] * s[1] * c[2] - s[0] * c[1] * s[2];
 69 |     break;
 70 |   }
 71 | 
 72 |   case EULER_YXY:
 73 |   {
 74 |     q[0] = ch * cos(h[0] + h[2]);
 75 |     q[1] = sh * cos(-h[0] + h[2]);
 76 |     q[2] = ch * sin(h[0] + h[2]);
 77 |     q[3] = sh * sin(-h[0] + h[2]);
 78 |     break;
 79 |   }
 80 | 
 81 |   case EULER_YZY:
 82 |   {
 83 |     q[0] = ch * cos(h[0] + h[2]);
 84 |     q[1] = sh * sin(h[0] - h[2]);
 85 |     q[2] = ch * sin(h[0] + h[2]);
 86 |     q[3] = sh * cos(h[0] - h[2]);
 87 |     break;
 88 |   }
 89 | 
 90 |   case EULER_ZXY:
 91 |   {
 92 |     q[0] = c[0] * c[1] * c[2] - s[0] * s[1] * s[2];
 93 |     q[1] = c[0] * s[1] * c[2] - s[0] * c[1] * s[2];
 94 |     q[2] = c[0] * c[1] * s[2] + s[0] * s[1] * c[2];
 95 |     q[3] = s[0] * c[1] * c[2] + c[0] * s[1] * s[2];
 96 |     break;
 97 |   }
 98 | 
 99 |   case EULER_ZYX:
100 |   {
101 |     q[0] = c[0] * c[1] * c[2] + s[0] * s[1] * s[2];
102 |     q[1] = c[0] * c[1] * s[2] - s[0] * s[1] * c[2];
103 |     q[2] = c[0] * s[1] * c[2] + s[0] * c[1] * s[2];
104 |     q[3] = s[0] * c[1] * c[2] - c[0] * s[1] * s[2];
105 | 
106 |     break;
107 |   }
108 | 
109 |   case EULER_ZXZ:
110 |   {
111 |     q[0] = ch * cos(h[0] + h[2]);
112 |     q[1] = sh * cos(h[0] - h[2]);
113 |     q[2] = sh * sin(h[0] - h[2]);
114 |     q[3] = ch * sin(h[0] + h[2]);
115 |     break;
116 |   }
117 | 
118 |   case EULER_ZYZ:
119 |   {
120 |     q[0] = ch * cos(h[0] + h[2]);
121 |     q[1] = sh * sin(-h[0] + h[2]);
122 |     q[2] = sh * cos(-h[0] + h[2]);
123 |     q[3] = ch * sin(h[0] + h[2]);
124 |     break;
125 |   }
126 |   }
127 | }
128 | 
129 | // Computes DCM from the input Euler angles using the sequence specified by `es`.
130 | void euler_to_dcm(const double e[3], const euler_seq_t es, double m[3][3])
131 | {
132 |   const double c[3] = {cos(e[0]), cos(e[1]), cos(e[2])};
133 |   const double s[3] = {sin(e[0]), sin(e[1]), sin(e[2])};
134 | 
135 |   switch (es)
136 |   {
137 |   case EULER_XYZ:
138 |   {
139 |     m[0][0] = c[1] * c[2];
140 |     m[0][1] = c[2] * s[0] * s[1] + c[0] * s[2];
141 |     m[0][2] = s[0] * s[2] - c[0] * c[2] * s[1];
142 |     m[1][0] = -c[1] * s[2];
143 |     m[1][1] = c[0] * c[2] - s[0] * s[1] * s[2];
144 |     m[1][2] = c[2] * s[0] + c[0] * s[1] * s[2];
145 |     m[2][0] = s[1];
146 |     m[2][1] = -c[1] * s[0];
147 |     m[2][2] = c[0] * c[1];
148 |     break;
149 |   }
150 | 
151 |   case EULER_XZY:
152 |   {
153 |     m[0][0] = c[1] * c[2];
154 |     m[0][1] = c[0] * c[2] * s[1] + s[0] * s[2];
155 |     m[0][2] = c[2] * s[0] * s[1] - c[0] * s[2];
156 |     m[1][0] = -s[1];
157 |     m[1][1] = c[0] * c[1];
158 |     m[1][2] = c[1] * s[0];
159 |     m[2][0] = c[1] * s[2];
160 |     m[2][1] = -c[2] * s[0] + c[0] * s[1] * s[2];
161 |     m[2][2] = c[0] * c[2] + s[0] * s[1] * s[2];
162 |     break;
163 |   }
164 | 
165 |   case EULER_XYX:
166 |   {
167 |     m[0][0] = c[1];
168 |     m[0][1] = s[0] * s[1];
169 |     m[0][2] = -c[0] * s[1];
170 |     m[1][0] = s[1] * s[2];
171 |     m[1][1] = c[0] * c[2] - c[1] * s[0] * s[2];
172 |     m[1][2] = c[2] * s[0] + c[0] * c[1] * s[2];
173 |     m[2][0] = c[2] * s[1];
174 |     m[2][1] = -c[1] * c[2] * s[0] - c[0] * s[2];
175 |     m[2][2] = c[0] * c[1] * c[2] - s[0] * s[2];
176 |     break;
177 |   }
178 | 
179 |   case EULER_XZX:
180 |   {
181 |     m[0][0] = c[1];
182 |     m[0][1] = c[0] * s[1];
183 |     m[0][2] = s[0] * s[1];
184 |     m[1][0] = -c[2] * s[1];
185 |     m[1][1] = c[0] * c[1] * c[2] - s[0] * s[2];
186 |     m[1][2] = c[1] * c[2] * s[0] + c[0] * s[2];
187 |     m[2][0] = s[1] * s[2];
188 |     m[2][1] = -c[2] * s[0] - c[0] * c[1] * s[2];
189 |     m[2][2] = c[0] * c[2] - c[1] * s[0] * s[2];
190 |     break;
191 |   }
192 | 
193 |   case EULER_YXZ:
194 |   {
195 |     m[0][0] = c[0] * c[2] + s[0] * s[1] * s[2];
196 |     m[0][1] = c[1] * s[2];
197 |     m[0][2] = -c[2] * s[0] + c[0] * s[1] * s[2];
198 |     m[1][0] = c[2] * s[0] * s[1] - c[0] * s[2];
199 |     m[1][1] = c[1] * c[2];
200 |     m[1][2] = c[0] * c[2] * s[1] + s[0] * s[2];
201 |     m[2][0] = c[1] * s[0];
202 |     m[2][1] = -s[1];
203 |     m[2][2] = c[0] * c[1];
204 |     break;
205 |   }
206 | 
207 |   case EULER_YZX:
208 |   {
209 |     m[0][0] = c[0] * c[1];
210 |     m[0][1] = s[1];
211 |     m[0][2] = -c[1] * s[0];
212 |     m[1][0] = -c[0] * c[2] * s[1] + s[0] * s[2];
213 |     m[1][1] = c[1] * c[2];
214 |     m[1][2] = c[2] * s[0] * s[1] + c[0] * s[2];
215 |     m[2][0] = c[2] * s[0] + c[0] * s[1] * s[2];
216 |     m[2][1] = -c[1] * s[2];
217 |     m[2][2] = c[0] * c[2] - s[0] * s[1] * s[2];
218 |     break;
219 |   }
220 | 
221 |   case EULER_YXY:
222 |   {
223 |     m[0][0] = c[0] * c[2] - c[1] * s[0] * s[2];
224 |     m[0][1] = s[1] * s[2];
225 |     m[0][2] = -c[2] * s[0] - c[0] * c[1] * s[2];
226 |     m[1][0] = s[0] * s[1];
227 |     m[1][1] = c[1];
228 |     m[1][2] = c[0] * s[1];
229 |     m[2][0] = c[1] * c[2] * s[0] + c[0] * s[2];
230 |     m[2][1] = -c[2] * s[1];
231 |     m[2][2] = c[0] * c[1] * c[2] - s[0] * s[2];
232 |     break;
233 |   }
234 | 
235 |   case EULER_YZY:
236 |   {
237 |     m[0][0] = c[0] * c[1] * c[2] - s[0] * s[2];
238 |     m[0][1] = c[2] * s[1];
239 |     m[0][2] = -c[1] * c[2] * s[0] - c[0] * s[2];
240 |     m[1][0] = -c[0] * s[1];
241 |     m[1][1] = c[1];
242 |     m[1][2] = s[0] * s[1];
243 |     m[2][0] = c[2] * s[0] + c[0] * c[1] * s[2];
244 |     m[2][1] = s[1] * s[2];
245 |     m[2][2] = c[0] * c[2] - c[1] * s[0] * s[2];
246 |     break;
247 |   }
248 | 
249 |   case EULER_ZXY:
250 |   {
251 |     m[0][0] = c[0] * c[2] - s[0] * s[1] * s[2];
252 |     m[0][1] = c[2] * s[0] + c[0] * s[1] * s[2];
253 |     m[0][2] = -c[1] * s[2];
254 |     m[1][0] = -c[1] * s[0];
255 |     m[1][1] = c[0] * c[1];
256 |     m[1][2] = s[1];
257 |     m[2][0] = c[2] * s[0] * s[1] + c[0] * s[2];
258 |     m[2][1] = s[0] * s[2] - c[0] * c[2] * s[1];
259 |     m[2][2] = c[1] * c[2];
260 |     break;
261 |   }
262 | 
263 |   case EULER_ZYX:
264 |   {
265 |     m[0][0] = c[1] * c[0];
266 |     m[0][1] = c[1] * s[0];
267 |     m[0][2] = -s[1];
268 |     m[1][0] = s[2] * s[1] * c[0] - c[2] * s[0];
269 |     m[1][1] = s[2] * s[1] * s[0] + c[2] * c[0];
270 |     m[1][2] = s[2] * c[1];
271 |     m[2][0] = c[2] * s[1] * c[0] + s[2] * s[0];
272 |     m[2][1] = c[2] * s[1] * s[0] - s[2] * c[0];
273 |     m[2][2] = c[2] * c[1];
274 |     break;
275 |   }
276 | 
277 |   case EULER_ZXZ:
278 |   {
279 |     m[0][0] = c[2] * c[0] - s[2] * c[1] * s[0];
280 |     m[0][1] = c[2] * s[0] + s[2] * c[1] * c[0];
281 |     m[0][2] = s[2] * s[1];
282 |     m[1][0] = -s[2] * c[0] - c[2] * c[1] * s[0];
283 |     m[1][1] = -s[2] * s[0] + c[2] * c[1] * c[0];
284 |     m[1][2] = c[2] * s[1];
285 |     m[2][0] = s[1] * s[0];
286 |     m[2][1] = -s[1] * c[0];
287 |     m[2][2] = c[1];
288 |     break;
289 |   }
290 | 
291 |   case EULER_ZYZ:
292 |   {
293 |     m[0][0] = c[0] * c[1] * c[2] - s[0] * s[2];
294 |     m[0][1] = c[1] * c[2] * s[0] + c[0] * s[2];
295 |     m[0][2] = -c[2] * s[1];
296 |     m[1][0] = -c[2] * s[0] - c[0] * c[1] * s[2];
297 |     m[1][1] = c[0] * c[2] - c[1] * s[0] * s[2];
298 |     m[1][2] = s[1] * s[2];
299 |     m[2][0] = c[0] * s[1];
300 |     m[2][1] = s[0] * s[1];
301 |     m[2][2] = c[1];
302 |     break;
303 |   }
304 |   }
305 | }
306 | 


--------------------------------------------------------------------------------
/attitude/euler.h:
--------------------------------------------------------------------------------
 1 | // Basic Euler angles manipulations.
 2 | // 2024-12-22
 3 | 
 4 | #ifndef _ATTITUDE_EULER_H_
 5 | #define _ATTITUDE_EULER_H_
 6 | 
 7 | #ifdef __cplusplus
 8 | extern "C" {
 9 | #endif
10 | 
11 | typedef enum
12 | {
13 |   EULER_XYX = 121,
14 |   EULER_XYZ = 123,
15 |   EULER_XZX = 131,
16 |   EULER_XZY = 132,
17 |   EULER_YXY = 212,
18 |   EULER_YXZ = 213,
19 |   EULER_YZX = 231,
20 |   EULER_YZY = 232,
21 |   EULER_ZXY = 312,
22 |   EULER_ZXZ = 313,
23 |   EULER_ZYX = 321,
24 |   EULER_ZYZ = 323
25 | } euler_seq_t;
26 | 
27 | void euler_to_quat(const double e[3], const euler_seq_t es, double q[4]);
28 | void euler_to_dcm(const double e[3], const euler_seq_t es, double m[3][3]);
29 | 
30 | #ifdef __cplusplus
31 | }
32 | #endif
33 | 
34 | #endif // euler.h
35 | 


--------------------------------------------------------------------------------
/attitude/quat.c:
--------------------------------------------------------------------------------
  1 | #include <math.h>
  2 | #include <inttypes.h>
  3 | 
  4 | #include "quat.h"
  5 | 
  6 | #define PI_2 1.57079632679489661923
  7 | #define PI 2 * 1.57079632679489661923
  8 | 
  9 | #define QUAT_ANGLE_ON_SINGULARITY_RAD 0.0f
 10 | #define QUAT_COMPARISON_EPSILON 1e-5f
 11 | #define QUAT_DIVISION_EPSILON 1e-6f
 12 | #define EULER_SINGULARITY_EPSILON 1e-7f
 13 | 
 14 | // Initializes input array to the unit quaternion.
 15 | void quat_unit(double q[4])
 16 | {
 17 |   q[0] = 1.0;
 18 |   q[1] = 0.0;
 19 |   q[2] = 0.0;
 20 |   q[3] = 0.0;
 21 | }
 22 | 
 23 | // Enforce q[0] >= 0 i.e. the canonical form.
 24 | void quat_canonize(double q[4])
 25 | {
 26 |   if (q[0] < 0)
 27 |   {
 28 |     quat_neg(q);
 29 |   }
 30 | }
 31 | 
 32 | // Negates all the elements of the input quaternion.
 33 | void quat_neg(double q[4])
 34 | {
 35 |   q[0] = -q[0];
 36 |   q[1] = -q[1];
 37 |   q[2] = -q[2];
 38 |   q[3] = -q[3];
 39 | }
 40 | 
 41 | // Normalizes the input quaternion.
 42 | void quat_normalize(double q[4])
 43 | {
 44 |   double norm = quat_norm(q);
 45 | 
 46 |   if (norm > QUAT_DIVISION_EPSILON)
 47 |   {
 48 |     double inv_norm = 1.0 / norm;
 49 |     q[0] = q[0] * inv_norm;
 50 |     q[1] = q[1] * inv_norm;
 51 |     q[2] = q[2] * inv_norm;
 52 |     q[3] = q[3] * inv_norm;
 53 |   }
 54 | }
 55 | 
 56 | // Computes norm of the input quaternion.
 57 | double quat_norm(const double q[4])
 58 | {
 59 |   return sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
 60 | }
 61 | 
 62 | // Computes conjugate of the input quaternion.
 63 | void quat_conj(const double q[4], double q_conj[4])
 64 | {
 65 |   q_conj[0] =  q[0];
 66 |   q_conj[1] = -q[1];
 67 |   q_conj[2] = -q[2];
 68 |   q_conj[3] = -q[3];
 69 | }
 70 | 
 71 | // Copies the contents of `q_src` to `q_dest`.
 72 | void quat_copy(const double q_src[4], double q_dest[4])
 73 | {
 74 |   q_dest[0] = q_src[0];
 75 |   q_dest[1] = q_src[1];
 76 |   q_dest[2] = q_src[2];
 77 |   q_dest[3] = q_src[3];
 78 | }
 79 | 
 80 | // Returns true if quaternions represent the same rotation (q1 == +/-q2).
 81 | bool quat_equal(const double q1[4], const double q2[4])
 82 | {
 83 |   const double dot = quat_dot(q1, q2);
 84 |   return fabs(1.0 - fabs(dot)) <= QUAT_COMPARISON_EPSILON;
 85 | }
 86 | 
 87 | // Computes the dot product of the input quaternions.
 88 | double quat_dot(const double q1[4], const double q2[4])
 89 | {
 90 |   return q1[0] * q2[0] + q1[1] * q2[1] + q1[2] * q2[2] + q1[3] * q2[3];
 91 | }
 92 | 
 93 | // Angular distance [rad] between two quaternions.
 94 | double quat_dist(const double q1[4], const double q2[4])
 95 | {
 96 |   double qe[4];
 97 |   quat_err(q1, q2, qe); // Rotation from q2 to q1
 98 |   quat_normalize(qe);   // Confirm domain for acos
 99 |   quat_canonize(qe);    // Ensure the shortest path
100 | 
101 |   return 2.0 * acos(fabs(qe[0]));
102 | }
103 | 
104 | // Quaternion product: q = q1 * q2
105 | void quat_prod(const double q1[4], const double q2[4], double q[4])
106 | {
107 |   q[0] = q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] - q1[3] * q2[3];
108 |   q[1] = q1[0] * q2[1] + q1[1] * q2[0] + q1[2] * q2[3] - q1[3] * q2[2];
109 |   q[2] = q1[0] * q2[2] - q1[1] * q2[3] + q1[2] * q2[0] + q1[3] * q2[1];
110 |   q[3] = q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1] + q1[3] * q2[0];
111 | }
112 | 
113 | /**
114 |  * @brief Computes the error quaternion representing the rotation from q2 to q1.
115 |  *
116 |  * @note In the setting of real numbers, x = x1 - x2 represents the offset that converts x2 to x1.
117 |  *       Analogously the quaternion q (q1 - q2) represents the rotation which rotates q2 to q1,
118 |  *       i.e. q1 = q * q2 => q = q1 * inv(q2).
119 |  */
120 | void quat_err(const double q1[4], const double q2[4], double q[4])
121 | {
122 |   double q2_inv[4];
123 |   quat_conj(q2, q2_inv);
124 |   quat_prod(q1, q2_inv, q);
125 | }
126 | 
127 | /**
128 |  * @brief Computes the angle-axis representation corresponding to the input quaternion, where the
129 |  *        rotation occurs by an angle `psi` [rad] about the unit axis vector `v`.
130 |  *
131 |  * @param q Input quaternion
132 |  * @param psi Rotation angle in radians
133 |  * @param v Unit vector representing the axis of rotation
134 |  *
135 |  * @return Status of the conversion; `true` if successful, `false` if there are infinite solutions,
136 |  *         in which case the axis is chosen to be [1, 0, 0]'.
137 |  */
138 | bool quat_to_axan(const double q[4], double *psi, double v[3])
139 | {
140 |   bool status = true;
141 |   *psi = 2.0 * acos(q[0]);
142 |   double divisor = sqrt(1.0 - q[0] * q[0]);
143 | 
144 |   if (divisor > QUAT_DIVISION_EPSILON)
145 |   {
146 |     double inv_divisor = 1.0 / divisor;
147 |     v[0] = q[1] * inv_divisor;
148 |     v[1] = q[2] * inv_divisor;
149 |     v[2] = q[3] * inv_divisor;
150 |   }
151 |   else // Infinite solutions
152 |   {
153 |     // Arbitrary axis
154 |     v[0] = 1;
155 |     v[1] = 0;
156 |     v[2] = 0;
157 | 
158 |     status = false;
159 |   }
160 | 
161 |   return status;
162 | }
163 | 
164 | /**
165 |  * @brief Computes the rate of change of the quaternion using the input quaternion and angular velocity.
166 |  *
167 |  * The angular velocity vector `w` is assumed to be expressed in the body frame.
168 |  *
169 |  * @param q Input quaternion
170 |  * @param w Angular velocity vector in the body frame
171 |  * @param q_dot Output rate of change of the quaternion
172 |  */
173 | void quat_rate(const double q[4], const double w[3], double q_dot[4])
174 | {
175 |   q_dot[0] = 0.5 * (-w[0] * q[1] - w[1] * q[2] - w[2] * q[3]);
176 |   q_dot[1] = 0.5 * ( w[0] * q[0] + w[2] * q[2] - w[1] * q[3]);
177 |   q_dot[2] = 0.5 * ( w[1] * q[0] - w[2] * q[1] + w[0] * q[3]);
178 |   q_dot[3] = 0.5 * ( w[2] * q[0] + w[1] * q[1] - w[0] * q[2]);
179 | }
180 | 
181 | 
182 | // Rotates the input vector `v` using the quaternion `q` to produce the output vector `v_out`.
183 | void quat_rotate(const double q[4], const double v[3], double v_out[3])
184 | {
185 |   double q0_sq = q[0] * q[0];
186 |   double q1_sq = q[1] * q[1];
187 |   double q2_sq = q[2] * q[2];
188 |   double q3_sq = q[3] * q[3];
189 |   double q0q1x2 = 2.0 * q[0] * q[1];
190 |   double q0q2x2 = 2.0 * q[0] * q[2];
191 |   double q0q3x2 = 2.0 * q[0] * q[3];
192 |   double q1q2x2 = 2.0 * q[1] * q[2];
193 |   double q1q3x2 = 2.0 * q[1] * q[3];
194 |   double q2q3x2 = 2.0 * q[2] * q[3];
195 | 
196 |   v_out[0] = (q0_sq + q1_sq - q2_sq - q3_sq) * v[0] + (q1q2x2 - q0q3x2) * v[1] + (q1q3x2 + q0q2x2) * v[2];
197 |   v_out[1] = (q1q2x2 + q0q3x2) * v[0] + (q0_sq - q1_sq + q2_sq - q3_sq) * v[1] + (q2q3x2 - q0q1x2) * v[2];
198 |   v_out[2] = (q1q3x2 - q0q2x2) * v[0] + (q2q3x2 + q0q1x2) * v[1] + (q0_sq - q1_sq - q2_sq + q3_sq) * v[2];
199 | }
200 | 
201 | // Computes the DCM `m` corresponding to the input quaternion `q`.
202 | void quat_to_dcm(const double q[4], double m[3][3])
203 | {
204 |   m[0][0] = q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3];
205 |   m[0][1] = 2.0 * (q[1] * q[2] + q[0] * q[3]);
206 |   m[0][2] = 2.0 * (q[1] * q[3] - q[0] * q[2]);
207 |   m[1][0] = 2.0 * (q[1] * q[2] - q[0] * q[3]);
208 |   m[1][1] = q[0] * q[0] - q[1] * q[1] + q[2] * q[2] - q[3] * q[3];
209 |   m[1][2] = 2.0 * (q[2] * q[3] + q[0] * q[1]);
210 |   m[2][0] = 2.0 * (q[1] * q[3] + q[0] * q[2]);
211 |   m[2][1] = 2.0 * (q[2] * q[3] - q[0] * q[1]);
212 |   m[2][2] = q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3];
213 | }
214 | 
215 | // Quaternion to intrinsic Euler angles as described in Ref.[2].
216 | void quat_to_euler(const double q[4], double e[3], const euler_seq_t es)
217 | {
218 |   // Parse Euler sequence
219 |   const uint8_t i = (es / 100) % 10;
220 |   const uint8_t j = (es / 10) % 10;
221 |   uint8_t k = es % 10;
222 | 
223 |   // Tait-Bryan angles
224 |   bool not_proper = true;
225 | 
226 |   // Proper Euler angles
227 |   if (i == k)
228 |   {
229 |     k = 6 - i - j;
230 |     not_proper = false;
231 |   }
232 | 
233 |   // Is permutation even or odd?
234 |   const int epsilon = -(i - j) * (j - k) * (k - i) / 2.0f;
235 |   double a, b, c, d;
236 | 
237 |   if (not_proper)
238 |   {
239 |     a = q[0] - q[j];
240 |     b = q[i] + q[k] * epsilon;
241 |     c = q[j] + q[0];
242 |     d = q[k] * epsilon - q[i];
243 |   }
244 |   else
245 |   {
246 |     a = q[0];
247 |     b = q[i];
248 |     c = q[j];
249 |     d = q[k] * epsilon;
250 |   }
251 | 
252 |   const double a_sq = a * a;
253 |   const double b_sq = b * b;
254 |   const double c_sq = c * c;
255 |   const double d_sq = d * d;
256 |   const double hyp_ab = a_sq + b_sq;
257 |   const double hyp_cd = c_sq + d_sq;
258 | 
259 |   e[1] = acos(2.0 * ((hyp_ab) / (hyp_ab + hyp_cd)) - 1.0);
260 |   const double theta_plus = atan2(b, a);
261 |   const double theta_minus = atan2(d, c);
262 | 
263 |   // Check singularity
264 |   if (fabs(e[1]) < EULER_SINGULARITY_EPSILON)
265 |   {
266 |     e[0] = QUAT_ANGLE_ON_SINGULARITY_RAD;
267 |     e[2] = 2 * theta_plus - QUAT_ANGLE_ON_SINGULARITY_RAD;
268 |   }
269 |   else if (fabs(fabs(e[1]) - PI_2) < EULER_SINGULARITY_EPSILON)
270 |   {
271 |     e[0] = QUAT_ANGLE_ON_SINGULARITY_RAD;
272 |     e[2] = 2 * theta_minus + QUAT_ANGLE_ON_SINGULARITY_RAD;
273 |   }
274 |   else // Safe
275 |   {
276 |     e[0] = theta_plus - theta_minus;
277 |     e[2] = theta_plus + theta_minus;
278 |   }
279 | 
280 |   if (not_proper)
281 |   {
282 |     e[1] -= PI_2;
283 |     e[2] *= epsilon;
284 |   }
285 | 
286 |   // Normalize to [-pi, pi]
287 |   for (uint8_t i = 0; i < 3; i++)
288 |   {
289 |     if (e[i] < -PI)
290 |     {
291 |       e[i] += 2.0 * PI;
292 |     }
293 |     else if (e[i] > PI)
294 |     {
295 |       e[i] -= 2 * PI;
296 |     }
297 |   }
298 | }
299 | 
300 | // Weighted average of two quaternions (n=2 case) as described in Ref.[7].
301 | void quat_mean(const double *q[], const int n, const double *w, double qm[4])
302 | {
303 |   if (n != 2)
304 |   {
305 |     // TODO
306 |     return;
307 |   }
308 | 
309 |   const double *q1 = q[0];
310 |   const double *q2 = q[1];
311 |   const double w1 = w[0];
312 |   const double w2 = w[1];
313 | 
314 |   double dot12 = quat_dot(q1, q2);
315 |   double dot11 = quat_dot(q1, q1);
316 |   double z = sqrt((w1 - w2) * (w1 - w2) + 4.0 * w1 * w2 * dot11 * dot11); // Eqn.(18)
317 | 
318 |   // Eqn.(19)
319 |   double temp = z * (w1 + w2 + z);
320 |   double a = sqrt(w1 * (w1 - w2 + z) / temp);
321 |   double b = sqrt(w2 * (w2 - w1 + z) / temp);
322 |   double sign = (dot12 >= 0) ? 1.0 : -1.0;
323 | 
324 |   for (int i = 0; i < 4; i++)
325 |   {
326 |     qm[i] = a * q1[i] + sign * b * q2[i];
327 |   }
328 | 
329 |   quat_normalize(qm);
330 |   quat_canonize(qm);
331 | }
332 | 
333 | /**
334 |  * @brief Spherical linear interpolation (SLERP) between two quaternions.
335 |  *
336 |  * @param q1 Start quaternion (must be a unit quaternion)
337 |  * @param q2 End quaternion (must be a unit quaternion)
338 |  * @param u Interpolation parameter in range [0,1]
339 |  * @param q Interpolated output quaternion
340 |  *
341 |  * @note When q1 and q2 are antipodal (q1 = -q2), the interpolation plane is chosen arbitrarily due
342 |  *       to infinite possible solutions.
343 |  *
344 |  *       The parameter `u` determines the interpolation between q1 and q2. u = 0 and u = 1 returns
345 |  *       exactly q1 and q2 (or its antipodal equivalent -q2), respectively. Intermediate values
346 |  *       (e.g. u = 0.6) results in a quaternion representing 60% of the rotation from q1 toward q2
347 |  *       along the shortest path.
348 |  */
349 | void quat_slerp(const double q1[4], const double q2[4], const double u, double q[4])
350 | {
351 |   // If q1 and q2 represent same orientation.
352 |   if(quat_equal(q1, q2))
353 |   {
354 |     quat_copy(q1, q);
355 |     return;
356 |   }
357 | 
358 |   double qb[4] = {q2[0], q2[1], q2[2], q2[3]};
359 |   double dot = quat_dot(q1, q2);
360 | 
361 |   // Ensure shortest angular path
362 |   if (dot < 0.0)
363 |   {
364 |     quat_neg(qb);
365 |     dot = -dot;
366 |   }
367 | 
368 |   // Ensure dot is in valid domain for acos i.e. [-1, 1].
369 |   dot = (dot < -1.0) ? -1.0 : (dot > 1.0) ? 1.0 : dot;
370 | 
371 |   const double theta = acos(fabs(dot));
372 |   const double st = sqrt(1.0 - dot * dot);
373 |   double a, b;
374 | 
375 |   // Fall back to LERP if sin(theta) -> 0.
376 |   if (st <= QUAT_DIVISION_EPSILON)
377 |   {
378 |     a = 1.0 - u;
379 |     b = u;
380 |   }
381 |   else // SLERP
382 |   {
383 |     const double stinv = 1 / st;
384 |     const double ut = u * theta;
385 | 
386 |     a = sin(theta - ut) * stinv;
387 |     b = sin(ut) * stinv;
388 |   }
389 | 
390 |   q[0] = a * q1[0] + b * qb[0];
391 |   q[1] = a * q1[1] + b * qb[1];
392 |   q[2] = a * q1[2] + b * qb[2];
393 |   q[3] = a * q1[3] + b * qb[3];
394 | 
395 |   quat_normalize(q);
396 | }
397 | 


--------------------------------------------------------------------------------
/attitude/quat.h:
--------------------------------------------------------------------------------
 1 | // Basic quaternion manipulations.
 2 | // 2024-12-22
 3 | 
 4 | #ifndef _ATTITUDE_QUAT_H_
 5 | #define _ATTITUDE_QUAT_H_
 6 | 
 7 | #include <stdbool.h>
 8 | #include "euler.h"
 9 | 
10 | #ifdef __cplusplus
11 | extern "C" {
12 | #endif
13 | 
14 | void quat_neg(double q[4]);
15 | void quat_unit(double q[4]);
16 | void quat_canonize(double q[4]);
17 | void quat_normalize(double q[4]);
18 | double quat_norm(const double q[4]);
19 | void quat_to_dcm(const double q[4], double m[3][3]);
20 | void quat_conj(const double q[4], double q_conj[4]);
21 | void quat_copy(const double q_src[4], double q_dest[4]);
22 | bool quat_equal(const double q1[4], const double q2[4]);
23 | double quat_dot(const double q1[4], const double q2[4]);
24 | double quat_dist(const double q1[4], const double q2[4]);
25 | bool quat_to_axan(const double q[4], double *psi, double v[3]);
26 | void quat_prod(const double q1[4], const double q2[4], double q[4]);
27 | void quat_err(const double q1[4], const double q2[4], double qe[4]);
28 | void quat_rate(const double q[4], const double w[3], double q_dot[4]);
29 | void quat_rotate(const double q[4], const double v[3], double v_out[3]);
30 | void quat_to_euler(const double q[4], double e[3], const euler_seq_t es);
31 | void quat_mean(const double *q[], const int n, const double *w, double qm[4]);
32 | void quat_slerp(const double q1[4], const double q2[4], const double u, double q[4]);
33 | 
34 | #ifdef __cplusplus
35 | }
36 | #endif
37 | 
38 | #endif // quat.h
39 | 


--------------------------------------------------------------------------------
/attitude/test_att.c:
--------------------------------------------------------------------------------
  1 | #include <math.h>
  2 | #include <stdio.h>
  3 | #include "attitude.h"
  4 | 
  5 | #define PI 3.14159265358979323846
  6 | 
  7 | #define RED "\x1b[31m"
  8 | #define GREEN "\x1b[32m"
  9 | #define RESET "\x1b[0m"
 10 | 
 11 | // Error tolerance
 12 | #define TOLERANCE 1e-9
 13 | 
 14 | // Is the deviation within threshold?
 15 | bool compare_matrices(double mat1[3][3], double mat2[3][3])
 16 | {
 17 |   for (int i = 0; i < 3; i++)
 18 |   {
 19 |     for (int j = 0; j < 3; j++)
 20 |     {
 21 |       if (fabs(mat1[i][j] - mat2[i][j]) > TOLERANCE)
 22 |       {
 23 |         return false;
 24 |       }
 25 |     }
 26 |   }
 27 | 
 28 |   return true;
 29 | }
 30 | 
 31 | bool compare_vectors(double vec1[3], double vec2[3])
 32 | {
 33 |   for (int i = 0; i < 3; i++)
 34 |   {
 35 |     if (fabs(vec1[i] - vec2[i]) > TOLERANCE)
 36 |     {
 37 |       return false;
 38 |     }
 39 |   }
 40 |   return true;
 41 | }
 42 | 
 43 | int main(void)
 44 | {
 45 | 
 46 |   euler_seq_t es[] =
 47 |   {
 48 |     EULER_XYX, EULER_XYZ, EULER_XZX, EULER_XZY,
 49 |     EULER_YXY, EULER_YXZ, EULER_YZX, EULER_YZY,
 50 |     EULER_ZXY, EULER_ZXZ, EULER_ZYX, EULER_ZYZ
 51 |   };
 52 | 
 53 |   double e[][3] =
 54 |   {
 55 |     {           0,           0,       0}, // Identity
 56 |     {      PI / 2,      PI / 2,  PI / 2}, // Gimbal lock
 57 |     {     -PI / 6,     -PI / 2,  PI / 4}, // Negative angle
 58 |     { 50 * PI / 7, 45 * PI / 3,  8 * PI}, // Large angles
 59 |     {-50 * PI / 7, 45 * PI / 3, -8 * PI}, // Negative and large angles
 60 | 
 61 |     // Random angles
 62 |     {0.433012701892219, -0.789149130992431, 0.435595740399158},
 63 |     {0.75, 0.047367172745252, -0.659739608441123},
 64 |     {0.5, 0.612372435695794, 0.612372435695794}
 65 |   };
 66 | 
 67 |   double turth_dcm[][12][3][3] =
 68 |   {
 69 |     {
 70 |       // {0.000000, 0.000000, 0.000000}
 71 |       {{1, 0, -0}, {0, 1, 0}, {0, -0, 1}}, // XYX
 72 |       {{1, 0, 0}, {-0, 1, 0}, {0, -0, 1}}, // XYZ
 73 |       {{1, 0, 0}, {-0, 1, 0}, {0, -0, 1}}, // XZX
 74 |       {{1, 0, 0}, {-0, 1, 0}, {0, 0, 1}}, // XZY
 75 |       {{1, 0, -0}, {0, 1, 0}, {0, -0, 1}}, // YXY
 76 |       {{1, 0, 0}, {0, 1, 0}, {0, -0, 1}}, // YXZ
 77 |       {{1, 0, -0}, {0, 1, 0}, {0, -0, 1}}, // YZX
 78 |       {{1, 0, -0}, {-0, 1, 0}, {0, 0, 1}}, // YZY
 79 |       {{1, 0, -0}, {-0, 1, 0}, {0, 0, 1}}, // ZXY
 80 |       {{1, 0, 0}, {-0, 1, 0}, {0, -0, 1}}, // ZXZ
 81 |       {{1, 0, -0}, {0, 1, 0}, {0, 0, 1}}, // ZYX
 82 |       {{1, 0, -0}, {-0, 1, 0}, {0, 0, 1}}, // ZYZ
 83 |     },
 84 | 
 85 |     {
 86 |       // {1.570796, 1.570796, 1.570796}
 87 |       {{6.123233996e-17, 1, -6.123233996e-17}, {1, -6.123233996e-17, 6.123233996e-17}, {6.123233996e-17, -6.123233996e-17, -1}}, // XYX
 88 |       {{3.749399457e-33, 1.224646799e-16, 1}, {-6.123233996e-17, -1, 1.224646799e-16}, {1, -6.123233996e-17, 3.749399457e-33}}, // XYZ
 89 |       {{6.123233996e-17, 6.123233996e-17, 1}, {-6.123233996e-17, -1, 6.123233996e-17}, {1, -6.123233996e-17, -6.123233996e-17}}, // XZX
 90 |       {{3.749399457e-33, 1, 0}, {-1, 3.749399457e-33, 6.123233996e-17}, {6.123233996e-17, 0, 1}}, // XZY
 91 |       {{-6.123233996e-17, 1, -6.123233996e-17}, {1, 6.123233996e-17, 6.123233996e-17}, {6.123233996e-17, -6.123233996e-17, -1}}, // YXY
 92 |       {{1, 6.123233996e-17, 0}, {0, 3.749399457e-33, 1}, {6.123233996e-17, -1, 3.749399457e-33}}, // YXZ
 93 |       {{3.749399457e-33, 1, -6.123233996e-17}, {1, 3.749399457e-33, 1.224646799e-16}, {1.224646799e-16, -6.123233996e-17, -1}}, // YZX
 94 |       {{-1, 6.123233996e-17, -6.123233996e-17}, {-6.123233996e-17, 6.123233996e-17, 1}, {6.123233996e-17, 1, -6.123233996e-17}}, // YZY
 95 |       {{-1, 1.224646799e-16, -6.123233996e-17}, {-6.123233996e-17, 3.749399457e-33, 1}, {1.224646799e-16, 1, 3.749399457e-33}}, // ZXY
 96 |       {{-6.123233996e-17, 6.123233996e-17, 1}, {-6.123233996e-17, -1, 6.123233996e-17}, {1, -6.123233996e-17, 6.123233996e-17}}, // ZXZ
 97 |       {{3.749399457e-33, 6.123233996e-17, -1}, {0, 1, 6.123233996e-17}, {1, 0, 3.749399457e-33}}, // ZYX
 98 |       {{-1, 6.123233996e-17, -6.123233996e-17}, {-6.123233996e-17, -6.123233996e-17, 1}, {6.123233996e-17, 1, 6.123233996e-17}}, // ZYZ
 99 |     },
100 | 
101 |     {
102 |       // {-0.523599, -1.570796, 0.785398}
103 |       {{6.123233996e-17, 0.5, 0.8660254038}, {-0.7071067812, 0.6123724357, -0.3535533906}, {-0.7071067812, -0.6123724357, 0.3535533906}}, // XYX
104 |       {{4.329780281e-17, 0.9659258263, 0.2588190451}, {-4.329780281e-17, 0.2588190451, -0.9659258263}, {-1, 3.061616998e-17, 5.302876194e-17}}, // XYZ
105 |       {{6.123233996e-17, -0.8660254038, 0.5}, {0.7071067812, 0.3535533906, 0.6123724357}, {-0.7071067812, 0.3535533906, 0.6123724357}}, // XZX
106 |       {{4.329780281e-17, -0.9659258263, -0.2588190451}, {1, 5.302876194e-17, -3.061616998e-17}, {4.329780281e-17, -0.2588190451, 0.9659258263}}, // XZY
107 |       {{0.6123724357, -0.7071067812, 0.3535533906}, {0.5, 6.123233996e-17, -0.8660254038}, {0.6123724357, 0.7071067812, 0.3535533906}}, // YXY
108 |       {{0.9659258263, 4.329780281e-17, -0.2588190451}, {-0.2588190451, 4.329780281e-17, -0.9659258263}, {-3.061616998e-17, 1, 5.302876194e-17}}, // YXZ
109 |       {{5.302876194e-17, -1, 3.061616998e-17}, {0.2588190451, 4.329780281e-17, 0.9659258263}, {-0.9659258263, -4.329780281e-17, 0.2588190451}}, // YZX
110 |       {{0.3535533906, -0.7071067812, -0.6123724357}, {0.8660254038, 6.123233996e-17, 0.5}, {-0.3535533906, -0.7071067812, 0.6123724357}}, // YZY
111 |       {{0.2588190451, -0.9659258263, -4.329780281e-17}, {3.061616998e-17, 5.302876194e-17, -1}, {0.9659258263, 0.2588190451, 4.329780281e-17}}, // ZXY
112 |       {{0.6123724357, -0.3535533906, -0.7071067812}, {-0.6123724357, 0.3535533906, -0.7071067812}, {0.5, 0.8660254038, 6.123233996e-17}}, // ZXZ
113 |       {{5.302876194e-17, -3.061616998e-17, 1}, {-0.2588190451, 0.9659258263, 4.329780281e-17}, {-0.9659258263, -0.2588190451, 4.329780281e-17}}, // ZYX
114 |       {{0.3535533906, 0.6123724357, 0.7071067812}, {0.3535533906, 0.6123724357, -0.7071067812}, {-0.8660254038, 0.5, 6.123233996e-17}}, // ZYZ
115 |     },
116 | 
117 |     {
118 |       // {22.439948, 47.123890, 25.132741}
119 |       {{-1, 7.444331965e-16, -1.545831461e-15}, {1.680943809e-30, -0.9009688679, -0.4338837391}, {-1.71574348e-15, -0.4338837391, 0.9009688679}}, // XYX
120 |       {{-1, 1.627128109e-15, -1.120747995e-15}, {-9.797174393e-16, -0.9009688679, -0.4338837391}, {-1.71574348e-15, -0.4338837391, 0.9009688679}}, // XYZ
121 |       {{-1, 1.545831461e-15, 7.444331965e-16}, {1.71574348e-15, 0.9009688679, 0.4338837391}, {1.680943809e-30, 0.4338837391, -0.9009688679}}, // XZX
122 |       {{-1, 1.970914927e-15, -1.382617157e-16}, {1.71574348e-15, 0.9009688679, 0.4338837391}, {9.797174393e-16, 0.4338837391, -0.9009688679}}, // XZY
123 |       {{-0.9009688679, 1.680943809e-30, 0.4338837391}, {7.444331965e-16, -1, 1.545831461e-15}, {0.4338837391, 1.71574348e-15, 0.9009688679}}, // YXY
124 |       {{-0.9009688679, 9.797174393e-16, 0.4338837391}, {-1.382617157e-16, -1, 1.970914927e-15}, {0.4338837391, 1.71574348e-15, 0.9009688679}}, // YXZ
125 |       {{0.9009688679, -1.71574348e-15, -0.4338837391}, {-1.120747995e-15, -1, 1.627128109e-15}, {-0.4338837391, -9.797174393e-16, -0.9009688679}}, // YZX
126 |       {{0.9009688679, -1.71574348e-15, -0.4338837391}, {-1.545831461e-15, -1, 7.444331965e-16}, {-0.4338837391, 1.680943809e-30, -0.9009688679}}, // YZY
127 |       {{-0.9009688679, -0.4338837391, -9.797174393e-16}, {-0.4338837391, 0.9009688679, -1.71574348e-15}, {1.627128109e-15, -1.120747995e-15, -1}}, // ZXY
128 |       {{-0.9009688679, -0.4338837391, 1.680943809e-30}, {-0.4338837391, 0.9009688679, -1.71574348e-15}, {7.444331965e-16, -1.545831461e-15, -1}}, // ZXZ
129 |       {{0.9009688679, 0.4338837391, 1.71574348e-15}, {0.4338837391, -0.9009688679, 9.797174393e-16}, {1.970914927e-15, -1.382617157e-16, -1}}, // ZYX
130 |       {{0.9009688679, 0.4338837391, 1.71574348e-15}, {0.4338837391, -0.9009688679, 1.680943809e-30}, {1.545831461e-15, 7.444331965e-16, -1}}, // ZYZ
131 |     },
132 | 
133 |     {
134 |       // {-22.439948, 47.123890, -25.132741}
135 |       {{-1, -7.444331965e-16, -1.545831461e-15}, {-1.680943809e-30, -0.9009688679, 0.4338837391}, {-1.71574348e-15, 0.4338837391, 0.9009688679}}, // XYX
136 |       {{-1, -1.627128109e-15, -1.120747995e-15}, {9.797174393e-16, -0.9009688679, 0.4338837391}, {-1.71574348e-15, 0.4338837391, 0.9009688679}}, // XYZ
137 |       {{-1, 1.545831461e-15, -7.444331965e-16}, {1.71574348e-15, 0.9009688679, -0.4338837391}, {-1.680943809e-30, -0.4338837391, -0.9009688679}}, // XZX
138 |       {{-1, 1.970914927e-15, 1.382617157e-16}, {1.71574348e-15, 0.9009688679, -0.4338837391}, {-9.797174393e-16, -0.4338837391, -0.9009688679}}, // XZY
139 |       {{-0.9009688679, -1.680943809e-30, -0.4338837391}, {-7.444331965e-16, -1, 1.545831461e-15}, {-0.4338837391, 1.71574348e-15, 0.9009688679}}, // YXY
140 |       {{-0.9009688679, -9.797174393e-16, -0.4338837391}, {1.382617157e-16, -1, 1.970914927e-15}, {-0.4338837391, 1.71574348e-15, 0.9009688679}}, // YXZ
141 |       {{0.9009688679, -1.71574348e-15, 0.4338837391}, {-1.120747995e-15, -1, -1.627128109e-15}, {0.4338837391, 9.797174393e-16, -0.9009688679}}, // YZX
142 |       {{0.9009688679, -1.71574348e-15, 0.4338837391}, {-1.545831461e-15, -1, -7.444331965e-16}, {0.4338837391, -1.680943809e-30, -0.9009688679}}, // YZY
143 |       {{-0.9009688679, 0.4338837391, 9.797174393e-16}, {0.4338837391, 0.9009688679, -1.71574348e-15}, {-1.627128109e-15, -1.120747995e-15, -1}}, // ZXY
144 |       {{-0.9009688679, 0.4338837391, -1.680943809e-30}, {0.4338837391, 0.9009688679, -1.71574348e-15}, {-7.444331965e-16, -1.545831461e-15, -1}}, // ZXZ
145 |       {{0.9009688679, -0.4338837391, 1.71574348e-15}, {-0.4338837391, -0.9009688679, -9.797174393e-16}, {1.970914927e-15, 1.382617157e-16, -1}}, // ZYX
146 |       {{0.9009688679, -0.4338837391, 1.71574348e-15}, {-0.4338837391, -0.9009688679, -1.680943809e-30}, {1.545831461e-15, -7.444331965e-16, -1}}, // ZYZ
147 |     },
148 | 
149 |     {
150 |       // {0.433013, -0.789149, 0.435596}
151 |       {{0.7044494784, -0.2978180513, 0.6442478876}, {-0.299481173, 0.6982178044, 0.6502329772}, {-0.6434764638, -0.6509963947, 0.4026682688}}, // XYX
152 |       {{0.638667162, 0.1129995065, 0.761140833}, {-0.29724287, 0.9486075983, 0.1085831506}, {-0.7097541352, -0.2955921783, 0.6394328203}}, // XYZ
153 |       {{0.7044494784, -0.6442478876, -0.2978180513}, {0.6434764638, 0.4026682688, 0.6509963947}, {-0.299481173, -0.6502329772, 0.6982178044}}, // XZX
154 |       {{0.638667162, -0.4070336995, -0.6530144131}, {0.7097541352, 0.6394328203, 0.2955921783}, {0.29724287, -0.6522646977, 0.697278596}}, // XZY
155 |       {{0.6982178044, -0.299481173, -0.6502329772}, {-0.2978180513, 0.7044494784, -0.6442478876}, {0.6509963947, 0.6434764638, 0.4026682688}}, // YXY
156 |       {{0.697278596, 0.29724287, -0.6522646977}, {-0.6530144131, 0.638667162, -0.4070336995}, {0.2955921783, 0.7097541352, 0.6394328203}}, // YXZ
157 |       {{0.6394328203, -0.7097541352, -0.2955921783}, {0.761140833, 0.638667162, 0.1129995065}, {0.1085831506, -0.29724287, 0.9486075983}}, // YZX
158 |       {{0.4026682688, -0.6434764638, -0.6509963947}, {0.6442478876, 0.7044494784, -0.2978180513}, {0.6502329772, -0.299481173, 0.6982178044}}, // YZY
159 |       {{0.9486075983, 0.1085831506, -0.29724287}, {-0.2955921783, 0.6394328203, -0.7097541352}, {0.1129995065, 0.761140833, 0.638667162}}, // ZXY
160 |       {{0.6982178044, 0.6502329772, -0.299481173}, {-0.6509963947, 0.4026682688, -0.6434764638}, {-0.2978180513, 0.6442478876, 0.7044494784}}, // ZXZ
161 |       {{0.6394328203, 0.2955921783, 0.7097541352}, {-0.6522646977, 0.697278596, 0.29724287}, {-0.4070336995, -0.6530144131, 0.638667162}}, // ZYX
162 |       {{0.4026682688, 0.6509963947, 0.6434764638}, {-0.6502329772, 0.6982178044, -0.299481173}, {-0.6442478876, -0.2978180513, 0.7044494784}}, // ZYZ
163 |     },
164 | 
165 |     {
166 |       // {0.750000, 0.047367, -0.659740}
167 |       {{0.9988783852, 0.03227522869, -0.03464507442}, {-0.02902101208, 0.9954607029, 0.09064088348}, {0.03741326538, -0.0895337842, 0.9952808393}}, // XYX
168 |       {{0.7892656092, -0.4229579151, -0.4451588483}, {0.6122236737, 0.5979271638, 0.5173637793}, {0.04734946218, -0.6808742239, 0.7308681958}}, // XYZ
169 |       {{0.9988783852, 0.03464507442, 0.03227522869}, {-0.03741326538, 0.9952808393, 0.0895337842}, {-0.02902101208, -0.09064088348, 0.9954607029}}, // XZX
170 |       {{0.7892656092, -0.3904091087, 0.4739625788}, {-0.04734946218, 0.7308681958, 0.6808742239}, {-0.6122236737, -0.5598324823, 0.5583634705}}, // XZY
171 |       {{0.9954607029, -0.02902101208, -0.09064088348}, {0.03227522869, 0.9988783852, 0.03464507442}, {0.0895337842, -0.03741326538, 0.9952808393}}, // YXY
172 |       {{0.5583634705, -0.6122236737, -0.5598324823}, {0.4739625788, 0.7892656092, -0.3904091087}, {0.6808742239, -0.04734946218, 0.7308681958}}, // YXZ
173 |       {{0.7308681958, 0.04734946218, -0.6808742239}, {-0.4451588483, 0.7892656092, -0.4229579151}, {0.5173637793, 0.6122236737, 0.5979271638}}, // YZX
174 |       {{0.9952808393, 0.03741326538, -0.0895337842}, {-0.03464507442, 0.9988783852, 0.03227522869}, {0.09064088348, -0.02902101208, 0.9954607029}}, // YZY
175 |       {{0.5979271638, 0.5173637793, 0.6122236737}, {-0.6808742239, 0.7308681958, 0.04734946218}, {-0.4229579151, -0.4451588483, 0.7892656092}}, // ZXY
176 |       {{0.9954607029, 0.09064088348, -0.02902101208}, {-0.0895337842, 0.9952808393, 0.03741326538}, {0.03227522869, -0.03464507442, 0.9988783852}}, // ZXZ
177 |       {{0.7308681958, 0.6808742239, -0.04734946218}, {-0.5598324823, 0.5583634705, -0.6122236737}, {-0.3904091087, 0.4739625788, 0.7892656092}}, // ZYX
178 |       {{0.9952808393, 0.0895337842, -0.03741326538}, {-0.09064088348, 0.9954607029, -0.02902101208}, {0.03464507442, 0.03227522869, 0.9988783852}}, // ZYZ
179 |     },
180 | 
181 |     {
182 |       // {0.500000, 0.612372, 0.612372}
183 |       {{0.8182866212, 0.2755787896, -0.5044435907}, {0.3304070055, 0.4926116328, 0.8050869456}, {0.4703596669, -0.8254635728, 0.3120443459}}, // XYX
184 |       {{0.6695929945, 0.7299460274, -0.1372006519}, {-0.4703596669, 0.5597085129, 0.6822669305}, {0.5748104083, -0.3923075041, 0.7181140694}}, // XYZ
185 |       {{0.8182866212, 0.5044435907, 0.2755787896}, {-0.4703596669, 0.3120443459, 0.8254635728}, {0.3304070055, -0.8050869456, 0.4926116328}}, // XZX
186 |       {{0.6695929945, 0.6883582311, -0.2789411541}, {-0.5748104083, 0.7181140694, 0.3923075041}, {0.4703596669, -0.1023480778, 0.876519626}}, // XZY
187 |       {{0.4926116328, 0.3304070055, -0.8050869456}, {0.2755787896, 0.8182866212, 0.5044435907}, {0.8254635728, -0.4703596669, 0.3120443459}}, // YXY
188 |       {{0.876519626, 0.4703596669, -0.1023480778}, {-0.2789411541, 0.6695929945, 0.6883582311}, {0.3923075041, -0.5748104083, 0.7181140694}}, // YXZ
189 |       {{0.7181140694, 0.5748104083, -0.3923075041}, {-0.1372006519, 0.6695929945, 0.7299460274}, {0.6822669305, -0.4703596669, 0.5597085129}}, // YZX
190 |       {{0.3120443459, 0.4703596669, -0.8254635728}, {-0.5044435907, 0.8182866212, 0.2755787896}, {0.8050869456, 0.3304070055, 0.4926116328}}, // YZY
191 |       {{0.5597085129, 0.6822669305, -0.4703596669}, {-0.3923075041, 0.7181140694, 0.5748104083}, {0.7299460274, -0.1372006519, 0.6695929945}}, // ZXY
192 |       {{0.4926116328, 0.8050869456, 0.3304070055}, {-0.8254635728, 0.3120443459, 0.4703596669}, {0.2755787896, -0.5044435907, 0.8182866212}}, // ZXZ
193 |       {{0.7181140694, 0.3923075041, -0.5748104083}, {-0.1023480778, 0.876519626, 0.4703596669}, {0.6883582311, -0.2789411541, 0.6695929945}}, // ZYX
194 |       {{0.3120443459, 0.8254635728, -0.4703596669}, {-0.8050869456, 0.4926116328, 0.3304070055}, {0.5044435907, 0.2755787896, 0.8182866212}}, // ZYZ
195 |     }
196 |   };
197 | 
198 |   int count = 0;
199 | 
200 |   for (int i = 0; i < 8; i++)
201 |   {
202 |     for (int j = 0; j < 12; j ++)
203 |     {
204 |       double dcm[3][3], euler[3];
205 |       euler_to_dcm(e[i], es[j], dcm);
206 |       dcm_to_euler(dcm, euler, es[j]);
207 | 
208 |       if(compare_matrices(turth_dcm[i][j], dcm))
209 |       {
210 |         printf(GREEN "euler_to_dcm: %d/95 PASSED!  ", count);
211 |       }
212 |       else
213 |       {
214 |         printf(RED "euler_to_dcm: %d/95 FAILED.  ", count);
215 |       }
216 | 
217 |       if(compare_vectors(euler, e[i]))
218 |       {
219 |         printf(GREEN "dcm_to_euler: %d/95 PASSED!\n", count);
220 |       }
221 |       else
222 |       {
223 |         printf(RED "dcm_to_euler: %d/95 FAILED.\n", count);
224 |       }
225 | 
226 |       count ++;
227 |     }
228 |   }
229 | 
230 |   printf(RESET);
231 | 
232 |   // 0.174461059412466  + 0.595055005141286i - 0.732945697135043j + 0.279756116387487k
233 |   // 0.219081163106642  - 0.898748787861072i - 0.297999382249188j + 0.235479146569706k
234 |   double q1[4] = {0.174461059412466,  0.595055005141286,  0.732945697135043,  0.279756116387487};
235 |   double q2[4] = {0.219081163106642, -0.898748787861072, 0.297999382249188, 0.235479146569706};
236 |   double u = 0.3;
237 | 
238 |   double q[4];
239 |   quat_slerp(q1, q2, u, q);
240 | 
241 |   // 0.0564395763769562 + 0.859378789689511i - 0.489280556589716j + 0.137430735257576k
242 |   printf("q-slerp: %.15f, %.15f, %.15f, %.15f\n", q[0], q[1], q[2], q[3]);
243 | 
244 |   // q1 = 0.637500352868806 + 0.49338212432488i   - 0.240485730112637j + 0.54067919610524k
245 |   // q2 = 0.697035155263178 - 0.0605889265769267i + 0.686790776520073j - 0.19695025663971k
246 |   // qe = 2.854979893415346
247 |   double q3[4] = {0.637500352868806,  0.49338212432488,  -0.240485730112637,  0.54067919610524};
248 |   double q4[4] = {0.697035155263178, -0.0605889265769267, 0.686790776520073, -0.19695025663971};
249 | 
250 |   printf("q-dist: %.15f\n", quat_dist(q4, q3));
251 | 
252 |   const double *qin[2] = {q3, q4};  // Array of pointers
253 |   double w[2] = {0.23, 0.32};
254 |   double qm[4];
255 |   quat_mean(qin, 2, w, qm);
256 |   printf("q-mean: %.15f, %.15f, %.15f, %.15f\n", qm[0], qm[1], qm[2], qm[3]);
257 | 
258 |   return 0;
259 | }
260 | 


--------------------------------------------------------------------------------
/igrf/README.md:
--------------------------------------------------------------------------------
 1 | # IGRF
 2 | 
 3 | International Geomagnetic Reference Field (IGRF-14) Model.
 4 | 
 5 | ## References
 6 | 
 7 | 1. Davis - Mathematical Modeling of Earth's Magnetic Field (2004)
 8 | 2. Yang - Spacecraft Modeling Attitude Determination and Control (2019)
 9 | 3. [IGRF-14](https://doi.org/10.5281/zenodo.14218973) - International Association of Geomagnetism and Aeronomy (2024)
10 | 4. Wertz (ed.) - Spacecraft Attitude Determination and Control (1980)
11 | 5. Barton, Tarlowski - Geomagnetic, geocentric, and geodetic coordinate transformations (1991)
12 | 


--------------------------------------------------------------------------------
/igrf/c/Makefile:
--------------------------------------------------------------------------------
 1 | CC = gcc
 2 | CFLAGS = -std=c99 -Wall -Wextra -Werror -pedantic -fsanitize=undefined,address -lm
 3 | 
 4 | TARGET = igrf
 5 | BUILD_DIR = build
 6 | 
 7 | INC = \
 8 | 
 9 | SRC = \
10 | igrf.c \
11 | main.c
12 | 
13 | all: $(TARGET)
14 | 
15 | $(TARGET): $(SRC)
16 | 	@mkdir -p $(BUILD_DIR)
17 | 	$(CC) $(CFLAGS) $(INC) -o $(BUILD_DIR)/$(TARGET) $(SRC)
18 | 
19 | run: clean all
20 | 	./$(BUILD_DIR)/$(TARGET)
21 | 
22 | clean:
23 | 	rm -rf $(BUILD_DIR)
24 | 
25 | .PHONY: all clean
26 | 


--------------------------------------------------------------------------------
/igrf/c/igrf.c:
--------------------------------------------------------------------------------
  1 | #include "igrf.h"
  2 | #include <math.h>
  3 | 
  4 | // World Geodetic System 1984 (WGS84) params
  5 | static const double wgs84_f = 1 / 298.257223563;
  6 | static const double wgs84_a = 6378.137;
  7 | static const double wgs84_b = wgs84_a * (1 - wgs84_f);
  8 | 
  9 | // Decimal years since January 1, IGRF_START_YEAR
 10 | double get_years(const igrf_time_t dt)
 11 | {
 12 |   if ((dt.year < IGRF_START_YEAR) || (dt.year >= IGRF_END_YEAR) ||
 13 |       (dt.month < 1) || (dt.month > 12) ||
 14 |       (dt.day < 1) || (dt.hour > 23) ||
 15 |       (dt.minute > 59) || (dt.second > 59))
 16 |   {
 17 |     return -1;
 18 |   }
 19 | 
 20 |   int days_in_month[] = {31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31};
 21 |   int is_leap = (((dt.year % 4) == 0) && (((dt.year % 100) != 0) || ((dt.year % 400) == 0)));
 22 | 
 23 |   // Adjust for leap year
 24 |   if (is_leap)
 25 |   {
 26 |     days_in_month[1] = 29;
 27 |   }
 28 | 
 29 |   // Check valid day in the month
 30 |   if (dt.day > days_in_month[dt.month - 1])
 31 |   {
 32 |     return -1;
 33 |   }
 34 | 
 35 |   // Days since IGRF_START_YEAR
 36 |   int years = dt.year - IGRF_START_YEAR;
 37 |   int days_arr[] = {0, 31, 59, 90, 120, 151, 182, 212, 243, 273, 304, 334};
 38 |   double days = days_arr[dt.month - 1] + dt.day + (dt.month > 2 ? is_leap : 0);
 39 |   double hours = dt.hour + (dt.minute / 60.0) + (dt.second / 3600.0);
 40 |   int total_days = is_leap ? 366 : 365;
 41 | 
 42 |   // Decimal years
 43 |   return years + days / total_days + hours / 24.0;
 44 | }
 45 | 
 46 | /**
 47 |  * @brief Computes magnetic field strength [nT] in specified coordinate frame.
 48 |  *
 49 |  * @param t Time between January 1, IGRF_START_YEAR and December 31, IGRF_END_YEAR.
 50 |  * @param x Spatial coordinates: {latitude [deg], longitude [deg], altitude [km]}.
 51 |  *          - For geodetic frame, altitude represents height above Earth's surface.
 52 |  *          - For geocentric frame, altitude represents radius from the center of the Earth.
 53 |  * @param b Output magnetic field intensity in specified frame.
 54 |  * @param f The frame of reference for the input coordinates and the output field values.
 55 |  *
 56 |  * @return false if the time is out of range; true otherwise.
 57 |  */
 58 | bool igrf(const igrf_time_t t, const double x[3], const igrf_frame_t f, double b[3])
 59 | {
 60 |   const double a = 6371.2;                  // Radius of Earth [km]
 61 |   const double theta = M_PI_2 - x[0] * D2R; // Colattitude [rad]
 62 |   const double phi = x[1] * D2R;            // Longitude [rad]
 63 | 
 64 |   double cd, sd, r;
 65 |   double ct = cos(theta);
 66 |   double st = sin(theta);
 67 | 
 68 |   // Geodetic to geocentric conversion
 69 |   // https://github.com/wb-bgs/m_IGRF
 70 |   if (f == IGRF_GEODETIC)
 71 |   {
 72 |     // Radius
 73 |     const double h = x[2];
 74 |     const double rho = hypot(wgs84_a * st, wgs84_b * ct);
 75 |     r = sqrt(h * h + 2 * h * rho + (pow(wgs84_a, 4) * st * st + pow(wgs84_b, 4) * ct * ct) / (rho * rho));
 76 | 
 77 |     // Latitude
 78 |     cd = (h + rho) / r;
 79 |     sd = (wgs84_a * wgs84_a - wgs84_b * wgs84_b) / rho * ct * st / r;
 80 |     const double temp = ct;
 81 |     ct = cd * ct - sd * st;
 82 |     st = cd * st + sd * temp;
 83 |   }
 84 |   else if (f == IGRF_GEOCENTRIC)
 85 |   {
 86 |     r = x[2]; // Radius
 87 |   }
 88 | 
 89 |   // Avoid singularity on pole
 90 |   const double epsilon = 1e-8;
 91 | 
 92 |   if (st < epsilon && st > -epsilon)
 93 |   {
 94 |     st = epsilon;
 95 |   }
 96 | 
 97 |   double years = get_years(t);
 98 | 
 99 |   if (years < 0)
100 |   {
101 |     return false;
102 |   }
103 | 
104 |   // [a] Re-occurring power factors
105 |   // Optimizations [a] and [b] by Alar Leibak.
106 |   double ar_pow[IGRF_DEGREE + 1];
107 |   const double ar = a / r;
108 |   ar_pow[0] = ar * ar * ar;
109 | 
110 |   for (uint8_t i = 1; i <= IGRF_DEGREE; i++)
111 |   {
112 |     ar_pow[i] = ar_pow[i - 1] * ar;
113 |   }
114 | 
115 |   // [b] Re-occurring sines and cosines
116 |   double sines[IGRF_DEGREE + 1], cosines[IGRF_DEGREE + 1];
117 |   sines[0] = 0;
118 |   cosines[0] = 1;
119 |   sines[1] = sin(phi);
120 |   cosines[1] = cos(phi);
121 | 
122 |   for (uint8_t i = 2; i <= IGRF_DEGREE; i++)
123 |   {
124 |     if (i & 1)
125 |     {
126 |       sines[i] = sines[i - 1] * cosines[1] + cosines[i - 1] * sines[1];
127 |       cosines[i] = cosines[i - 1] * cosines[1] - sines[i - 1] * sines[1];
128 |     }
129 |     else // even
130 |     {
131 |       sines[i] = 2 * sines[i >> 1] * cosines[i >> 1];
132 |       cosines[i] = 2 * cosines[i >> 1] * cosines[i >> 1] - 1;
133 |     }
134 |   }
135 | 
136 |   // Associated Legendre polynomials and its derivative
137 |   double pnm = 0.0f, dpnm = 0.0f; // (n, m)
138 |   double p11 = 1.0f, dp11 = 0.0f; // (n, n)
139 |   double p10 = 1.0f, dp10 = 0.0f; // (n-1, m)
140 |   double p20 = 0.0f, dp20 = 0.0f; // (n-2, m)
141 | 
142 |   // Field components: radial, theta, and phi
143 |   double br = 0.0f, bt = 0.0f, bp = 0.0f;
144 | 
145 |   for (uint8_t m = 0; m <= IGRF_DEGREE; m++)
146 |   {
147 |     for (uint8_t n = 1; n <= IGRF_DEGREE; n++)
148 |     {
149 |       if (m <= n)
150 |       {
151 |         if (n == m)
152 |         {
153 |           pnm = st * p11;
154 |           dpnm = st * dp11 + ct * p11;
155 | 
156 |           p11 = pnm; dp11 = dpnm;
157 |           p20 = 0.0; dp20 = 0.0;
158 |         }
159 |         else
160 |         {
161 |           double Knm = 0.0;
162 | 
163 |           if (n > 1)
164 |           {
165 |             Knm = (pow(n - 1, 2) - pow(m, 2)) / ((2.0f * n - 1) * (2.0f * n - 3));
166 |           }
167 | 
168 |           pnm = ct * p10 - Knm * p20;
169 |           dpnm = ct * dp10 - st * p10 - Knm * dp20;
170 |         }
171 | 
172 |         p20 = p10; dp20 = dp10;
173 |         p10 = pnm; dp10 = dpnm;
174 | 
175 |         // Linear interpolation of g and h
176 |         const int k = (0.5 * n * (n + 1) + m) - 1;
177 |         const double g = g_val[k] + g_sv[k] * years;
178 |         const double h = h_val[k] + h_sv[k] * years;
179 | 
180 |         if (m == 0)
181 |         {
182 |           const double temp = ar_pow[n - 1] * g;
183 |           br += temp * (n + 1.0) * pnm;
184 |           bt -= temp * dpnm;
185 |         }
186 |         else
187 |         {
188 |           const double hsin = h * sines[m];
189 |           const double hcos = h * cosines[m];
190 |           const double gsin = g * sines[m];
191 |           const double gcos = g * cosines[m];
192 |           const double temp = ar_pow[n - 1] * (gcos + hsin);
193 | 
194 |           br += temp * (n + 1.0) * pnm;
195 |           bt -= temp * dpnm;
196 |           bp -= ar_pow[n - 1] * m * (-gsin + hcos) * pnm;
197 |         }
198 |       }
199 |     }
200 |   }
201 | 
202 |   bp = bp / st;
203 | 
204 |   // Geocentric NED
205 |   b[0] = -bt;
206 |   b[1] = bp;
207 |   b[2] = -br;
208 | 
209 |   // Geocentric to geodetic NED
210 |   if (f == IGRF_GEODETIC)
211 |   {
212 |     double temp = b[0];
213 |     b[0] = cd * b[0] + sd * b[2];
214 |     b[2] = cd * b[2] - sd * temp;
215 |   }
216 | 
217 |   return true;
218 | }
219 | 
220 | // Magnetic inclination [rad]
221 | double igrf_inc(const double b[3])
222 | {
223 |   const double h = sqrt(b[0] * b[0] + b[1] * b[1]);
224 |   return atan(b[2] / h);
225 | }
226 | 
227 | // Magnetic declination [rad]
228 | double igrf_dec(const double b[3])
229 | {
230 |   return atan2(b[1], b[0]);
231 | }
232 | 
233 | // Magnitude of magnetic field
234 | double igrf_mag(const double b[3])
235 | {
236 |   return sqrt(b[0] * b[0] + b[1] * b[1] + b[2] * b[2]);
237 | }
238 | 


--------------------------------------------------------------------------------
/igrf/c/igrf.h:
--------------------------------------------------------------------------------
 1 | /**
 2 |  * @brief Implementation of International Geomagnetic Reference Field (IGRF-14) Model.
 3 |  * @cite Davis - Mathematical Modeling of Earth's Magnetic Field (2004)
 4 |  * @author risherlock
 5 |  * @date 2021-12-26
 6 |  */
 7 | 
 8 | #ifndef _IGRF_H_
 9 | #define _IGRF_H_
10 | 
11 | #ifdef __cplusplus
12 | extern "C"
13 | {
14 | #endif
15 | 
16 | #include "igrf_2025.h"
17 | 
18 | #include <stdbool.h>
19 | #include <inttypes.h>
20 | 
21 | #define R2D 57.2957795131
22 | #define D2R 0.01745329251
23 | #define M_PI_2 1.57079632679489661923
24 | 
25 | typedef struct
26 | {
27 |   uint16_t year;
28 |   uint8_t month;
29 |   uint8_t day;
30 |   uint8_t hour;
31 |   uint8_t minute;
32 |   uint8_t second;
33 | } igrf_time_t;
34 | 
35 | typedef enum
36 | {
37 |   IGRF_GEODETIC,
38 |   IGRF_GEOCENTRIC
39 | } igrf_frame_t;
40 | 
41 | double igrf_mag(const double b[3]);
42 | double igrf_inc(const double b[3]);
43 | double igrf_dec(const double b[3]);
44 | bool igrf(const igrf_time_t t, const double x[3], const igrf_frame_t f, double b[3]);
45 | 
46 | #ifdef __cplusplus
47 | }
48 | #endif
49 | 
50 | #endif // igrf.h
51 | 


--------------------------------------------------------------------------------
/igrf/c/igrf_2025.h:
--------------------------------------------------------------------------------
 1 | #ifndef _IGRF_2025_H_
 2 | #define _IGRF_2025_H_
 3 | 
 4 | #define IGRF_DEGREE 13
 5 | #define IGRF_END_YEAR 2030
 6 | #define IGRF_START_YEAR 2025
 7 | 
 8 | // Schmidt quasi-normalized coefficients
 9 | static const double g_val[] = {-29350.00000, -1410.30000, -3834.30000, 5111.10873, 1427.81608, 3402.25000, -7361.32905, 2408.60834, 358.44417, 3914.31250, 4424.97513, 218.35204, -587.96283, 8.87412, -1834.08750, 3751.46849, 1438.67189, -652.75169, -314.80940, 14.66262, 928.33125, 1206.01936, 1146.22262, -1152.69845, -223.18566, 34.66953, -40.83895, 2134.27500, -2727.61251, -254.85513, 1214.36824, 195.11311, 15.43616, -27.12444, 9.25582, 1161.31641, 730.64062, -981.44143, 82.83915, -582.84959, 250.63691, 102.29189, -42.11469, 0.62671, 446.31641, 1019.22773, 325.95012, -16.59657, -140.93935, -441.35141, 41.75503, 64.78831, -22.48061, -7.79583, -234.55352, -1557.03087, 42.13838, 330.56068, -116.87085, -36.95781, -37.18808, 30.06478, 7.36434, -6.90244, -2.31515, 1033.34766, -652.94103, -1022.61993, 787.12322, -143.70838, 0.00000, -56.47059, -4.96044, 25.03604, -8.81492, -0.27203, 1.50795, -1320.38867, -89.70275, 318.05194, 779.06498, -584.29873, 200.41281, 104.14946, 58.52694, -5.85269, -12.77163, -1.88649, -3.33778, -0.39744, 253.92090, -1557.16135, 928.51164, 898.19471, -196.82397, 347.93891, 45.15452, 186.99640, 0.00000, 20.42190, 5.67768, 4.01473, -1.41942, -0.22270};
10 | static const double h_val[] = {0.00000, 4545.50000, 0.00000, -5427.55441, -705.11788, 0.00000, -174.21996, 460.11042, -434.49695, 0.00000, 1541.76847, -524.35794, 443.42981, -277.61204, 0.00000, 460.54613, 1690.74688, -578.39353, 95.17493, 74.50575, 0.00000, -347.81750, 251.06310, 487.18197, -326.32035, 25.36227, 48.89927, 0.00000, -1734.46361, -417.03566, -20.47839, 290.19988, -45.69104, -60.78780, -1.42397, 0.00000, 482.62500, -706.63783, 476.32509, -259.34133, 188.34845, 4.80566, -13.03550, 2.44416, 0.00000, -3159.60598, 1314.66550, 688.75757, -191.67751, -178.56202, 125.26510, -4.52011, 2.06718, 5.96868, 0.00000, 802.84404, 21.06919, 413.20085, 631.10259, -665.24054, 16.52803, -84.18138, -31.09387, 2.38931, -5.34265, 0.00000, 0.00000, 1145.33433, -196.78080, 23.95140, 79.21180, -28.23529, -59.52522, -38.69206, -25.56328, -4.89662, -1.33395, 0.00000, -1076.43295, 477.07792, 649.22081, -730.37341, 0.00000, 124.97935, -23.41078, 46.82155, 2.55433, -8.48922, 0.27815, 0.11355, 0.00000, -1557.16135, 1083.26357, 1539.76235, -295.23595, -904.64116, -45.15452, 53.42754, -28.55822, 34.03650, 17.03304, -6.02209, -0.85165, -0.27837};
11 | 
12 | // Secular variations
13 | static const double g_sv[] = {12.60000, 10.00000, -16.80000, -9.17987, -7.18801, -3.75000, -13.47219, 0.77460, -12.33288, -7.43750, -12.72817, -22.69609, 11.29491, -5.02867, 4.72500, 13.21656, 0.00000, 3.29435, 5.10262, 0.70156, -2.88750, -5.67094, 11.95539, 11.95539, -4.36549, 0.93073, 0.60452, -2.68125, -3.54696, -2.89608, 10.23919, -1.23489, -4.93957, -1.93746, 0.58253, -5.02734, 13.40625, 0.00000, 16.56783, -2.67362, 4.44918, 0.68652, 0.00000, 0.18801, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000};
14 | static const double h_sv[] = {0.00000, -21.50000, 0.00000, -47.28499, -9.61288, 0.00000, 11.63508, -0.38730, -3.08322, 0.00000, -7.19418, 16.04379, 3.34664, -3.03199, 0.00000, -5.08329, 16.13895, 2.35311, 3.77150, 1.33297, 0.00000, 5.67094, -23.91077, -3.98513, 4.36549, 1.62877, 0.60452, 0.00000, 21.28176, 14.48040, -14.33487, 0.00000, -5.55702, 1.21091, -0.19418, 0.00000, -20.10938, 22.43295, -12.42587, 10.69449, -7.41529, -4.11914, 0.75205, 0.12534, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000, 0.00000};
15 | 
16 | #endif
17 | 


--------------------------------------------------------------------------------
/igrf/c/main.c:
--------------------------------------------------------------------------------
 1 | // Use case of IGRF-14 implementation.
 2 | // 2021-12-17
 3 | 
 4 | #include <stdio.h>
 5 | #include "igrf.h"
 6 | 
 7 | int main()
 8 | {
 9 |   // Miss Violet Smith disturbed Holmes 130 years ago on this day
10 |   const igrf_time_t dt = {.year = 2025, .month = 04, .day = 23, 0, 0, 0};
11 | 
12 |   // Geodetic coordinates of 221B Baker Street on LEO
13 |   const double latitude = 51.523788; // deg
14 |   const double longitude = -0.158611; // deg
15 |   const double altitude = 400.0; // km
16 |   const double x[3] = {latitude, longitude, altitude};
17 | 
18 |   // Magnetic field in NED frame
19 |   double b[3] = {0.0};
20 |   bool status = igrf(dt, x, IGRF_GEODETIC, b);
21 | 
22 |   if (status)
23 |   {
24 |     printf("Inputs:\n");
25 |     printf("  Time      : %d-%d-%d, %d:%d:%d\n", dt.year, dt.month, dt.day, dt.hour, dt.minute, dt.second);
26 |     printf("  Latitude  : %f deg\n", latitude);
27 |     printf("  Longitude : %f deg\n", longitude);
28 |     printf("  Altitude  : %f km\n", altitude);
29 |     printf("\nOutputs:\n");
30 |     printf("  Bn          : %f nT\n", b[0]);
31 |     printf("  Be          : %f nT\n", b[1]);
32 |     printf("  Bd          : %f nT\n", b[2]);
33 |     printf("  Magnitude   : %f nT\n", igrf_mag(b));
34 |     printf("  Inclination : %f deg\n", igrf_inc(b) * R2D);
35 |     printf("  Declination : %f deg\n", igrf_dec(b) * R2D);
36 |   }
37 |   else
38 |   {
39 |     printf("Date error!\n");
40 |   }
41 | 
42 |   return 0;
43 | }
44 | 
45 | /*
46 | Expected output:
47 | 
48 | Inputs:
49 |   Time      : 2025-4-23, 0:0:0
50 |   Latitude  : 51.523788 deg
51 |   Longitude : -0.158611 deg
52 |   Altitude  : 400.000000 km
53 | 
54 | Outputs:
55 |   Bn          : 16625.026351 nT
56 |   Be          : 69.051911 nT
57 |   Bd          : 37532.201921 nT
58 |   Magnitude   : 41049.512182 nT
59 |   Inclination : 66.108694 deg
60 |   Declination : 0.237976 deg
61 | */
62 | 


--------------------------------------------------------------------------------
/igrf/matlab/igrf.m:
--------------------------------------------------------------------------------
  1 | function [Bn,Be,Bd] = igrf(theta,phi,r,days)
  2 | % Calculates magnetic field strength in local spherical coordinates
  3 | %
  4 | % Inputs:
  5 | % theta = Latitude measured in degrees positive from equator
  6 | % phi   = Longitude measured in degrees positive east from Greenwich
  7 | % r     = Geocentric radius, km
  8 | % days  = Decimal days since January 1, 2020
  9 | %
 10 | % Output:
 11 | %   Br, Bt, Bp = B in radial, theta and phi direction
 12 | %   Bn, Be, Bd = B in North, East and Down direction
 13 | %
 14 | % References:
 15 | % [1] Davis - Mathematical Modeling of Earth's Magnetic Field (2004)
 16 | % [2] Yaguang - Spacecraft Modeling Attitude Determination and Control:
 17 | %           Quaternion Based Approach (2019)
 18 | % 
 19 | % Note:
 20 | %   Variable names used in this code follows reference [2].
 21 | %
 22 | % Test: http://www.geomag.bgs.ac.uk/data_service/models_compass/igrf_calc.html
 23 | % Latest IGRF model: https://www.ngdc.noaa.gov/IAGA/vmod/igrf.html
 24 | %
 25 | % Rishav (2020/5/25)
 26 | 
 27 | % Check pole to avoid singularities
 28 | if (theta>-0.00000001 && theta<0.00000001)
 29 |     theta = 0.00000001;
 30 | elseif(theta<180.00000001 && theta>179.99999999)
 31 |     theta = 179.99999999;
 32 | end
 33 | 
 34 | deg2rad = pi/180;
 35 | theta = (90-theta)*deg2rad;
 36 | phi   = phi*deg2rad;
 37 | a     = 6371.2; % Radius of Earth, Km
 38 | 
 39 | % Read in the g and h Schmidt quasi-normalized coefficients
 40 | norm_g = fopen('./igrf_coeffs/norm_g_2020.txt');
 41 | norm_h = fopen('./igrf_coeffs/norm_h_2020.txt');
 42 | read_h = textscan(norm_h,'%f %f %f %f');
 43 | read_g = textscan(norm_g,'%f %f %f %f');
 44 | 
 45 | % Unpack the file values to variables
 46 | hn = read_h{1}; hm = read_h{2}; h_val = read_h{3}; h_svi = read_h{4};
 47 | gn = read_g{1}; gm = read_g{2}; g_val = read_g{3}; g_svi = read_g{4};
 48 | 
 49 | % Close files
 50 | fclose(norm_g);
 51 | fclose(norm_h);
 52 | 
 53 | % Gauss coefficients
 54 | N = max(gn);
 55 | g = zeros(N,N+1);
 56 | h = zeros(N,N+1);
 57 | for x=1:length(gn)
 58 |     g(gn(x),gm(x)+1) = g_val(x) + g_svi(x)*days/365;
 59 |     h(hn(x),hm(x)+1) = h_val(x) + h_svi(x)*days/365;
 60 | end
 61 | 
 62 | % 1. Calculate Legendre polynomials and its derivatives recursively
 63 | % 2. Compute magnetic field strength using terms computed in 1
 64 | Br  = 0; Bt = 0; Bp = 0;
 65 | Pnpnp = 1; Pnpm = 1;
 66 | dPnpnp = 0; dPnpm = 0;
 67 | for m = 0:N
 68 |     for n = 1:N
 69 |         if m <= n
 70 |             if (n == m)
 71 |                 % Eqn(5.24b) and (5.25b) ...[2]
 72 |                 Pnm = sin(theta)*Pnpnp;
 73 |                 dPnm = sin(theta)*dPnpnp + cos(theta)*Pnpnp;
 74 |                 
 75 |                 Pnpnp  = Pnm; Pnpm = Pnpnp; Pnppm = 0;
 76 |                 dPnpnp = dPnm; dPnpm = dPnpnp; dPnppm = 0;
 77 |                 
 78 |             elseif (n == 1)
 79 |                 % K = 0
 80 |                 % Eqn(5.24c) and (5.25c) ...[2]
 81 |                 Pnm = cos(theta)*Pnpm;
 82 |                 dPnm = cos(theta)*dPnpm - sin(theta)*Pnpm;
 83 |                 
 84 |                 Pnppm = Pnpm; Pnpm = Pnm;
 85 |                 dPnppm = dPnpm; dPnpm = dPnm;
 86 |                 
 87 |             else % (n != m) && (n > 1)
 88 |                 % Eqn(5.24e), (5.24c) and (5.25c) ...[2]
 89 |                 Knm = ((n-1)^2-m^2)/((2*n-1)*(2*n-3));
 90 |                 Pnm = cos(theta)*Pnpm - Knm*Pnppm;
 91 |                 dPnm = cos(theta)*dPnpm - sin(theta)*Pnpm - Knm*dPnppm;
 92 |                 
 93 |                 Pnppm = Pnpm; Pnpm = Pnm;
 94 |                 dPnppm = dPnpm; dPnpm = dPnm;
 95 |             end
 96 |             
 97 |             % Eqn(5.14a), (5.14b) and (5.14c) ...[2]
 98 |             Br = Br + (a/r)^(n+2)*(n+1)*...
 99 |                 ((g(n,m+1)*cos(m*phi) + h(n,m+1)*sin(m*phi))*Pnm);
100 |             
101 |             Bt = Bt + (a/r)^(n+2)*...
102 |                 ((g(n,m+1)*cos(m*phi) + h(n,m+1)*sin(m*phi))*dPnm);
103 |             
104 |             Bp = Bp + (a/r)^(n+2)*...
105 |                 (m*(-g(n,m+1)*sin(m*phi) + h(n,m+1)*cos(m*phi))* Pnm);
106 |         end
107 |     end
108 | end
109 | Bt = -Bt;
110 | Bp = -Bp/sin(theta);
111 | 
112 | Bn = -Bt;
113 | Be = Bp;
114 | Bd = -Br;
115 | end
116 | 


--------------------------------------------------------------------------------
/igrf/matlab/igrf_coeffs/igrf_2020.txt:
--------------------------------------------------------------------------------
  1 | g	1	0   -2.9405e+04 5.7000                                            
  2 | g	1	1   -1.4509e+03	7.4000                      
  3 | h	1	1   4.6525e+03	-25.9000                        
  4 | g	2	0   -2.4996e+03	-11                     
  5 | g	2	1   2982    -7                      
  6 | h	2	1   -2.9916e+03 -30.2000                        
  7 | g	2	2   1677    -2.1000                     
  8 | h	2	2   -734.6000	-22.4000                        
  9 | g	3	0   1.3632e+03	2.2000                      
 10 | g	3	1   -2.3812e+03	-5.9000                     
 11 | h	3	1   -82.1000	6                       
 12 | g	3	2   1.2362e+03	3.1000                      
 13 | h	3	2   241.9000	-1.1000                     
 14 | g	3	3   525.7000	-12                     
 15 | h	3	3   -543.4000	0.5000                      
 16 | g	4	0   903	-1.2000                     
 17 | g	4	1   809.5000	-1.6000                     
 18 | h	4	1   281.9000	-0.1000                     
 19 | g	4	2   86.3000	-5.9000                     
 20 | h	4	2   -158.4000	6.5000                      
 21 | g	4	3   -309.4000	5.2000                      
 22 | h	4	3   199.7000	3.6000                      
 23 | g	4	4   48	-5.1000                     
 24 | h	4	4   -349.7000	-5                      
 25 | g	5	0   -234.3000	-0.3000                     
 26 | g	5	1   363.2000	0.5000                      
 27 | h	5	1   47.7000	0                       
 28 | g	5	2   187.8000	-0.6000                     
 29 | h	5	2   208.3000	2.5000                      
 30 | g	5	3   -140.7000	0.2000                      
 31 | h	5	3   -121.2000	-0.6000                     
 32 | g	5	4   -151.2000	1.3000                      
 33 | h	5	4   32.3000	3                       
 34 | g	5	5   13.5000	0.9000                      
 35 | h	5	5   98.9000	0.3000                      
 36 | g	6	0   66	-0.5000                     
 37 | g	6	1   65.5000	-0.3000                     
 38 | h	6	1   -19.1000	0                       
 39 | g	6	2   72.9000	0.4000                      
 40 | h	6	2   25.1000	-1.6000                     
 41 | g	6	3   -121.5000	1.3000                      
 42 | h	6	3   52.8000	-1.3000                     
 43 | g	6	4   -36.2000	-1.4000                     
 44 | h	6	4   -64.5000	0.8000                      
 45 | g	6	5   13.5000	0                       
 46 | h	6	5   8.9000	0                       
 47 | g	6	6   -64.7000	0.9000                      
 48 | h	6	6   68.1000	1                       
 49 | g	7	0   80.6000	-0.1000                     
 50 | g	7	1   -76.7000	-0.2000                     
 51 | h	7	1   -51.5000	0.6000                      
 52 | g	7	2   -8.2000	0                       
 53 | h	7	2   -16.9000	0.6000                      
 54 | g	7	3   56.5000	0.7000                      
 55 | h	7	3   2.2000	-0.8000                     
 56 | g	7	4   15.8000	0.1000                      
 57 | h	7	4   23.5000	-0.2000                     
 58 | g	7	5   6.4000	-0.5000                     
 59 | h	7	5   -2.2000	-1.1000                     
 60 | g	7	6   -7.2000	-0.8000                     
 61 | h	7	6   -27.2000	0.1000                      
 62 | g	7	7   9.8000	0.8000                      
 63 | h	7	7   -1.8000	0.3000                      
 64 | g	8	0   23.7000	0                       
 65 | g	8	1   9.7000	0.1000                      
 66 | h	8	1   8.4000	-0.2000                     
 67 | g	8	2   -17.6000	-0.1000                     
 68 | h	8	2   -15.3000	0.6000                      
 69 | g	8	3   -0.5000	0.4000                      
 70 | h	8	3   12.8000	-0.2000                     
 71 | g	8	4   -21.1000	-0.1000                     
 72 | h	8	4   -11.7000	0.5000                      
 73 | g	8	5   15.3000	0.4000                      
 74 | h	8	5   14.9000	-0.3000                     
 75 | g	8	6   13.7000	0.3000                      
 76 | h	8	6   3.6000	-0.4000                     
 77 | g	8	7   -16.5000	-0.1000                     
 78 | h	8	7   -6.9000	0.5000                      
 79 | g	8	8   -0.3000	0.4000                      
 80 | h	8	8   2.8000	0                       
 81 | g	9	0   5	0                       
 82 | g	9	1   8.4000	0
 83 | h	9	1   -23.4000	0
 84 | g	9	2   2.9000	0
 85 | h	9	2   11	0
 86 | g	9	3   -1.5000	0
 87 | h	9	3   9.8000	0
 88 | g	9	4   -1.1000	0
 89 | h	9	4   -5.1000	0
 90 | g	9	5   -13.2000	0
 91 | h	9	5   -6.3000	0
 92 | g	9	6   1.1000	0
 93 | h	9	6   7.8000	0
 94 | g	9	7   8.8000	0
 95 | h	9	7   0.4000	0
 96 | g	9	8   -9.3000	0
 97 | h	9	8   -1.4000	0
 98 | g	9	9   -11.9000	0
 99 | h	9	9   9.6000	0
100 | g	10	0   -1.9000	0
101 | g	10	1   -6.2000	0
102 | h	10	1   3.4000	0
103 | g	10	2   -0.1000	0
104 | h	10	2   -0.2000	0
105 | g	10	3   1.7000	0
106 | h	10	3   3.6000	0
107 | g	10	4   -0.9000	0
108 | h	10	4   4.8000	0
109 | g	10	5   0.7000	0
110 | h	10	5   -8.6000	0
111 | g	10	6   -0.9000	0
112 | h	10	6   -0.1000	0
113 | g	10	7   1.9000	0
114 | h	10	7   -4.3000	0
115 | g	10	8   1.4000	0
116 | h	10	8   -3.4000	0
117 | g	10	9   -2.4000	0
118 | h	10	9   -0.1000	0
119 | g	10	10  -3.8000	0
120 | h	10	10  -8.8000	0
121 | g	11	0   3	0
122 | g	11	1   -1.4000	0
123 | h	11	1   0	0
124 | g	11	2   -2.5000	0
125 | h	11	2   2.5000	0
126 | g	11	3   2.3000	0
127 | h	11	3   -0.6000	0
128 | g	11	4   -0.9000	0
129 | h	11	4   -0.4000	0
130 | g	11	5   0.3000	0
131 | h	11	5   0.6000	0
132 | g	11	6   -0.7000	0
133 | h	11	6   -0.2000	0
134 | g	11	7   -0.1000	0
135 | h	11	7   -1.7000	0
136 | g	11	8   1.4000	0
137 | h	11	8   -1.6000	0
138 | g	11	9   -0.6000	0
139 | h	11	9   -3	0
140 | g	11	10  0.2000	0
141 | h	11	10  -2	0
142 | g	11	11  3.1000	0
143 | h	11	11  -2.6000	0
144 | g	12	0   -2	0
145 | g	12	1   -0.1000	0
146 | h	12	1   -1.2000	0
147 | g	12	2   0.5000	0
148 | h	12	2   0.5000	0
149 | g	12	3   1.3000	0
150 | h	12	3   1.4000	0
151 | g	12	4   -1.2000	0
152 | h	12	4   -1.8000	0
153 | g	12	5   0.7000	0
154 | h	12	5   0.1000	0
155 | g	12	6   0.3000	0
156 | h	12	6   0.8000	0
157 | g	12	7   0.5000	0
158 | h	12	7   -0.2000	0
159 | g	12	8   -0.3000	0
160 | h	12	8   0.6000	0
161 | g	12	9   -0.5000	0
162 | h	12	9   0.2000	0
163 | g	12	10  0.1000	0
164 | h	12	10  -0.9000	0
165 | g	12	11  -1.1000	0
166 | h	12	11  0	0
167 | g	12	12  -0.3000	0
168 | h   12	12  0.5000	0
169 | g	13	0   0.1000	0
170 | g	13	1   -0.9000	0
171 | h	13	1   -0.9000	0
172 | g	13	2   0.5000	0
173 | h	13	2   0.6000	0
174 | g	13	3   0.7000	0
175 | h	13	3   1.4000	0
176 | g	13	4   -0.3000	0
177 | h	13	4   -0.4000	0
178 | g	13	5   0.8000	0
179 | h	13	5   -1.3000	0
180 | g	13	6   0	0
181 | h	13	6   -0.1000	0
182 | g	13	7   0.8000	0
183 | h	13	7   0.3000	0
184 | g	13	8   0	0
185 | h	13	8   -0.1000	0
186 | g	13	9   0.4000	0
187 | h	13	9   0.5000	0
188 | g	13	10  0.1000	0
189 | h	13	10  0.5000	0
190 | g	13	11  0.5000	0
191 | h	13	11  -0.4000	0
192 | g	13	12  -0.5000	0
193 | h	13	12  -0.4000	0
194 | g	13	13  -0.4000	0
195 | h	13	13  -0.6000	0


--------------------------------------------------------------------------------
/igrf/matlab/igrf_coeffs/norm_g_2020.txt:
--------------------------------------------------------------------------------
  1 | 1	0	-29405	5.7
  2 | 1	1	-1450.9	7.4
  3 | 2	0	-3749.4	-16.5
  4 | 2	1	5165	-12.124
  5 | 2	2	1452.3	-1.8187
  6 | 3	0	3408	5.5
  7 | 3	1	-7290.9	-18.065
  8 | 3	2	2393.9	6.0031
  9 | 3	3	415.6	-9.4868
 10 | 4	0	3950.6	-5.25
 11 | 4	1	4479.8	-8.8544
 12 | 4	2	337.7	-23.087
 13 | 4	3	-647.16	10.877
 14 | 4	4	35.496	-3.7715
 15 | 5	0	-1845.1	-2.3625
 16 | 5	1	3692.5	5.0833
 17 | 5	2	1443.3	-4.6111
 18 | 5	3	-662.16	0.94124
 19 | 5	4	-335.44	2.8841
 20 | 5	5	9.4711	0.6314
 21 | 6	0	952.88	-7.2188
 22 | 6	1	1238.2	-5.6709
 23 | 6	2	1089.4	5.9777
 24 | 6	3	-1210.5	12.952
 25 | 6	4	-197.54	-7.6396
 26 | 6	5	31.412	0
 27 | 6	6	-43.459	0.60452
 28 | 7	0	2161.1	-2.6813
 29 | 7	1	-2720.5	-7.0939
 30 | 7	2	-237.48	0
 31 | 7	3	1157	14.335
 32 | 7	4	195.11	1.2349
 33 | 7	5	39.517	-3.0872
 34 | 7	6	-17.437	-1.9375
 35 | 7	7	6.3431	0.51781
 36 | 8	0	1191.5	0
 37 | 8	1	650.2	6.7031
 38 | 8	2	-987.05	-5.6082
 39 | 8	3	-20.71	16.568
 40 | 8	4	-564.13	-2.6736
 41 | 8	5	226.91	5.9322
 42 | 8	6	94.054	2.0596
 43 | 8	7	-41.363	-0.25068
 44 | 8	8	-0.18801	0.25068
 45 | 9	0	474.8	0
 46 | 9	1	1070.2	0
 47 | 9	2	315.09	0
 48 | 9	3	-124.47	0
 49 | 9	4	-62.013	0
 50 | 9	5	-444.72	0
 51 | 9	6	19.138	0
 52 | 9	7	66.295	0
 53 | 9	8	-24.031	0
 54 | 9	9	-7.2477	0
 55 | 10	0	-342.81	0
 56 | 10	1	-1508.4	0
 57 | 10	2	-21.069	0
 58 | 10	3	280.98	0
 59 | 10	4	-105.18	0
 60 | 10	5	51.741	0
 61 | 10	6	-37.188	0
 62 | 10	7	38.082	0
 63 | 10	8	11.456	0
 64 | 10	9	-6.3715	0
 65 | 10	10	-2.2558	0
 66 | 11	0	1033.3	0
 67 | 11	1	-652.94	0
 68 | 11	2	-1022.6	0
 69 | 11	3	754.33	0
 70 | 11	4	-215.56	0
 71 | 11	5	47.527	0
 72 | 11	6	-65.882	0
 73 | 11	7	-4.9604	0
 74 | 11	8	31.864	0
 75 | 11	9	-5.289	0
 76 | 11	10	0.54407	0
 77 | 11	11	1.7979	0
 78 | 12	0	-1320.4	0
 79 | 12	1	-89.703	0
 80 | 12	2	397.56	0
 81 | 12	3	843.99	0
 82 | 12	4	-584.3	0
 83 | 12	5	233.81	0
 84 | 12	6	62.49	0
 85 | 12	7	58.527	0
 86 | 12	8	-17.558	0
 87 | 12	9	-12.772	0
 88 | 12	10	0.94325	0
 89 | 12	11	-3.0596	0
 90 | 12	12	-0.17033	0
 91 | 13	0	126.96	0
 92 | 13	1	-1557.2	0
 93 | 13	2	773.76	0
 94 | 13	3	898.19	0
 95 | 13	4	-295.24	0
 96 | 13	5	556.7	0
 97 | 13	6	0	0
 98 | 13	7	213.71	0
 99 | 13	8	0	0
100 | 13	9	27.229	0
101 | 13	10	2.8388	0
102 | 13	11	5.0184	0
103 | 13	12	-1.4194	0
104 | 13	13	-0.2227	0
105 | 


--------------------------------------------------------------------------------
/igrf/matlab/main.m:
--------------------------------------------------------------------------------
 1 | % Rishav (2021/12/27)
 2 | clc
 3 | clear
 4 | close all
 5 | 
 6 | latitude = 28.3949*0;
 7 | longitude = 84.1240*0;
 8 | radius =  6371.2;
 9 | days = 0;
10 | 
11 | [Bn, Be, Bd] = igrf(latitude, longitude, radius, days);


--------------------------------------------------------------------------------
/spacetime/Makefile:
--------------------------------------------------------------------------------
 1 | CC = gcc
 2 | CFLAGS = -std=c99 -Wall -Wextra -Werror
 3 | 
 4 | TARGET = tests
 5 | BUILD_DIR = build
 6 | 
 7 | INC = \
 8 | -I../attitude \
 9 | -Iiau06
10 | 
11 | SRC = \
12 | ../attitude/dcm.c \
13 | ../attitude/maps.c \
14 | iau06/iau06.c \
15 | iau06/iau06_data.c \
16 | time.c \
17 | frame.c \
18 | test_frame.c
19 | 
20 | ifeq ($(OS),Windows_NT)
21 |     TARGET := $(TARGET).exe
22 |     RM = del /q
23 |     MKDIR = mkdir
24 | else
25 |     RM = rm -rf
26 |     MKDIR = mkdir -p
27 | endif
28 | 
29 | all: $(TARGET)
30 | 
31 | $(TARGET): $(SRC)
32 | 	@if not exist $(BUILD_DIR) $(MKDIR) $(BUILD_DIR)
33 | 	$(CC) $(CFLAGS) $(INC) -o $(BUILD_DIR)/$(TARGET) $(SRC)
34 | 
35 | run: all
36 | 	$(BUILD_DIR)/$(TARGET)
37 | 
38 | clean:
39 | 	$(RM) $(BUILD_DIR)
40 | 
41 | .PHONY: all run clean
42 | 


--------------------------------------------------------------------------------
/spacetime/README.md:
--------------------------------------------------------------------------------
 1 | # spacetime
 2 | 
 3 | Useful coordinate transformations and time conversions.
 4 | 
 5 | ## Coordinates
 6 | 
 7 | Index of coordinates used in the software.
 8 | 
 9 | 1. `eci`  - Earth Centered Inertial
10 | 2. `ecef` - Earth Centered Earth Fixed
11 | 3. `lvlh` - Local Vertical Local Horizontal
12 | 4. `llh`  - Latitude Longitude Height (Geodetic)
13 | 5. `llr`  - Latitude Longitude Radius (Geocentric)
14 | 
15 | ## Notes
16 | 
17 | 1. Computation of Julian day at 0 UT (`j0` in software) holds from 1900 to 2100 [1].
18 | 
19 | ## References
20 | 
21 | 1. Curtis - Orbital mechanics for engineering students (2020)
22 | 


--------------------------------------------------------------------------------
/spacetime/frame.c:
--------------------------------------------------------------------------------
 1 | #include <stdio.h>
 2 | #include <math.h>
 3 | 
 4 | #include "attitude.h"
 5 | #include "time.h"
 6 | #include "iau06.h"
 7 | 
 8 | #define D2R 0.01745329251
 9 | #define R2D 57.2957795131
10 | #define PI_2 1.57079632679489661923
11 | #define PI 2 * 1.57079632679489661923
12 | 
13 | void print_dcm(const double dcm[3][3])
14 | {
15 |   for (int i = 0; i < 3; i++)
16 |   {
17 |     for (int j = 0; j < 3; j++)
18 |     {
19 |       printf("  %.20f ", dcm[i][j]);
20 |     }
21 |     printf("\n");
22 |   }
23 | }
24 | 
25 | void frame_eci_to_ecef_dcm(const utc_t utc, double dcm[3][3])
26 | {
27 |   double W[3][3], R[3][3], Q[3][3];
28 | 
29 |   const double jd_since_j2000 = 51544.5;
30 |   const double jd = time_julian_date(utc) - 2400000.5f;
31 |   const double jd_elapsed = jd - jd_since_j2000;
32 |   const double jd_fraction = fmod((fmod(jd_elapsed, 1.0) + 1.0), 1.0);
33 |   double t = jd_elapsed / 36525.0;
34 | 
35 |   double xp = 0.0, yp = 0.0;
36 |   const double s_prime = -0.000047 * t * D2R / 3600.0;
37 |   const double zyx[3] = {s_prime, xp, yp};
38 |   euler_to_dcm(zyx, EULER_ZYX, W);
39 | 
40 |   const double era = fmod(2.0 * PI * (jd_fraction + 0.7790572732640 + 0.00273781191135448 * jd_elapsed), 2.0 * PI);
41 |   dcm_z(era, R);
42 | 
43 |   double x, y, s;
44 |   iau06_get_xys(t, &x, &y, &s);
45 | 
46 |   double E = atan2(y, x);
47 |   double d = atan(sqrt((x * x + y * y) / (1.0 - x * x - y * y)));
48 |   const double zyz[3] = {E, d, -E - s};
49 |   euler_to_dcm(zyz, EULER_ZYZ, Q);
50 | 
51 |   double WR[3][3];
52 |   dcm_prod(W, R, WR);
53 |   dcm_prod(WR, Q, dcm);
54 | }
55 | 
56 | // void frame_ecef_to_ned(const double v_ecef[3], const double llr[3], double v_ned[3])
57 | // {
58 | //   const double st = sin(llr[0]);
59 | //   const double ct = cos(llr[0]);
60 | //   const double sp = sin(llr[1]);
61 | //   const double cp = cos(llr[1]);
62 | //   const double t  =  cp * v_ned[0] + sp * v_ned[1];
63 | 
64 | //   v_ned[0] = -st * t + ct * v_ned[2];
65 | //   v_ned[1] = -sp * v_ned[0] + cp * v_ned[1];
66 | //   v_ned[2] = -ct * t - st * v_ned[2];
67 | // }
68 | 
69 | // void frame_ned_to_ecef(const double v_ned[3], const double llr[3], double v_ecef[3])
70 | // {
71 | //   const double st = sin(llr[0]);
72 | //   const double ct = cos(llr[0]);
73 | //   const double sp = sin(llr[1]);
74 | //   const double cp = cos(llr[1]);
75 | //   const double t  = - ct * v_ned[2] - st * v_ned[0];
76 | 
77 | //   v_ecef[0] = cp * t - sp * v_ned[1];
78 | //   v_ecef[1] = sp * t + cp * v_ned[1];
79 | //   v_ecef[2] = - st * v_ned[2] - ct * v_ned[0];
80 | // }
81 | 


--------------------------------------------------------------------------------
/spacetime/frame.h:
--------------------------------------------------------------------------------
 1 | // rms (2025-02-03)
 2 | 
 3 | #ifndef _ADCS_FRAME_H_
 4 | #define _ADCS_FRAME_H_
 5 | 
 6 | #include "frame.h"
 7 | #include "time.h"
 8 | 
 9 | #ifdef __cplusplus
10 | extern "C"
11 | {
12 | #endif
13 | 
14 | void frame_eci_to_ecef_dcm(const utc_t t, double dcm[3][3]);
15 | 
16 | void frame_ecef_to_ned(const double v_ecef[3], const double llr[3], double v_ned[3]);
17 | void frame_ned_to_ecef(const double v_ned[3], const double llr[3], double v_ecef[3]);
18 | 
19 | // void frame_eci_to_ned();
20 | // void frame_ned_to_eci();
21 | 
22 | // void frame_ned_to_ecef_dcm(const double llr[3]);
23 | // void frame_ecef_to_ned_dcm(const double llr[3]);
24 | 
25 | #ifdef __cplusplus
26 | }
27 | #endif
28 | 
29 | #endif // frame.h
30 | 


--------------------------------------------------------------------------------
/spacetime/iau06/iau06.c:
--------------------------------------------------------------------------------
  1 | #include <math.h>
  2 | 
  3 | #include "iau06.h"
  4 | #include "iau06_data.h"
  5 | 
  6 | #define PI 2 * 1.57079632679489661923
  7 | #define D2R 0.01745329251
  8 | 
  9 | void iau06_compute_nutation_v(const double t[5], double nv[14])
 10 | {
 11 |   double m_moon = 485868.249036 + 1717915923.2178 * t[0] + 31.8792 * t[1] + 0.051635 * t[2] - 0.00024470 * t[3];
 12 |   double m_sun = 1287104.793048 + 129596581.0481 * t[0] - 0.5532 * t[1] + 0.000136 * t[2] - 0.00001149 * t[3];
 13 |   double mu_moon = 335779.526232 + 1739527262.8478 * t[0] - 12.7512 * t[1] - 0.001037 * t[2] + 0.00000417 * t[3];
 14 |   double d_sun = 1072260.703692 + 1602961601.2090 * t[0] - 6.3706 * t[1] + 0.006593 * t[2] - 0.00003169 * t[3];
 15 |   double w_moon = 450160.398036 - 6962890.5431 * t[0] + 7.4722 * t[1] + 0.007702 * t[2] - 0.00005939 * t[3];
 16 | 
 17 |   double l_mercury = 4.402608842 + 2608.7903141574 * t[0];
 18 |   double l_venus = 3.176146697 + 1021.3285546211 * t[0];
 19 |   double l_earth = 1.753470314 + 628.3075849991 * t[0];
 20 |   double l_mars = 6.203480913 + 334.06124267 * t[0];
 21 |   double l_jupiter = 0.599546497 + 52.9690962641 * t[0];
 22 |   double l_saturn = 0.874016757 + 21.329910496 * t[0];
 23 |   double l_uranus = 5.481293872 + 7.4781598567 * t[0];
 24 |   double l_neptune = 5.311886287 + 3.8133035638 * t[0];
 25 | 
 26 |   double pa = 0.02438175 * t[0] + 0.00000538691 * t[1];
 27 | 
 28 |   double arcsec_to_rad = D2R / 3600.0;
 29 |   nv[0] = fmod(m_moon * arcsec_to_rad, 2 * PI);
 30 |   nv[1] = fmod(m_sun * arcsec_to_rad, 2 * PI);
 31 |   nv[2] = fmod(mu_moon * arcsec_to_rad, 2 * PI);
 32 |   nv[3] = fmod(d_sun * arcsec_to_rad, 2 * PI);
 33 |   nv[4] = fmod(w_moon * arcsec_to_rad, 2 * PI);
 34 |   nv[5] = fmod(l_mercury, 2 * PI);
 35 |   nv[6] = fmod(l_venus, 2 * PI);
 36 |   nv[7] = fmod(l_earth, 2 * PI);
 37 |   nv[8] = fmod(l_mars, 2 * PI);
 38 |   nv[9] = fmod(l_jupiter, 2 * PI);
 39 |   nv[10] = fmod(l_saturn, 2 * PI);
 40 |   nv[11] = fmod(l_uranus, 2 * PI);
 41 |   nv[12] = fmod(l_neptune, 2 * PI);
 42 |   nv[13] = fmod(pa, 2 * PI);
 43 | }
 44 | 
 45 | double iau06_compute_x(const double nv[14], const double t[5])
 46 | {
 47 |   double q;
 48 |   double x = 0.0;
 49 | 
 50 |   for (int i = 0; i < 1306; i++)
 51 |   {
 52 |     q = 0.0;
 53 |     for (int k = 0; k < 14; k++)
 54 |     {
 55 |       q += iau06_cip_x0[i][k + 3] * nv[k];
 56 |     }
 57 |     x += (iau06_cip_x0[i][1] * sin(q) + iau06_cip_x0[i][2] * cos(q));
 58 |   }
 59 | 
 60 |   for (int i = 0; i < 253; i++)
 61 |   {
 62 |     q = 0.0;
 63 |     for (int k = 0; k < 14; k++)
 64 |     {
 65 |       q += iau06_cip_x1[i][k + 3] * nv[k];
 66 |     }
 67 |     x += (iau06_cip_x1[i][1] * sin(q) + iau06_cip_x1[i][2] * cos(q)) * t[0];
 68 |   }
 69 | 
 70 |   for (int i = 0; i < 36; i++)
 71 |   {
 72 |     q = 0.0;
 73 |     for (int k = 0; k < 14; k++)
 74 |     {
 75 |       q += iau06_cip_x2[i][k + 3] * nv[k];
 76 |     }
 77 |     x += (iau06_cip_x2[i][1] * sin(q) + iau06_cip_x2[i][2] * cos(q)) * t[1];
 78 |   }
 79 | 
 80 |   for (int i = 0; i < 4; i++)
 81 |   {
 82 |     q = 0.0;
 83 |     for (int k = 0; k < 14; k++)
 84 |     {
 85 |       q += iau06_cip_x3[i][k + 3] * nv[k];
 86 |     }
 87 |     x += (iau06_cip_x3[i][1] * sin(q) + iau06_cip_x3[i][2] * cos(q)) * t[2];
 88 |   }
 89 | 
 90 |   q = 0.0;
 91 |   for (int k = 0; k < 14; k++)
 92 |   {
 93 |     q += iau06_cip_x4[k + 3] * nv[k];
 94 |   }
 95 |   x += (iau06_cip_x4[1] * sin(q) + iau06_cip_x4[2] * cos(q)) * t[3];
 96 | 
 97 |   return x;
 98 | }
 99 | 
100 | double iau06_compute_y(const double nv[14], const double t[5])
101 | {
102 |   double q;
103 |   double y = 0.0;
104 | 
105 |   for (int i = 0; i < 962; i++)
106 |   {
107 |     q = 0.0;
108 |     for (int k = 0; k < 14; k++)
109 |     {
110 |       q += iau06_cip_y0[i][k + 3] * nv[k];
111 |     }
112 |     y += (iau06_cip_y0[i][1] * sin(q) + iau06_cip_y0[i][2] * cos(q));
113 |   }
114 | 
115 |   for (int i = 0; i < 277; i++)
116 |   {
117 |     q = 0.0;
118 |     for (int k = 0; k < 14; k++)
119 |     {
120 |       q += iau06_cip_y1[i][k + 3] * nv[k];
121 |     }
122 |     y += (iau06_cip_y1[i][1] * sin(q) + iau06_cip_y1[i][2] * cos(q)) * t[0];
123 |   }
124 | 
125 |   for (int i = 0; i < 30; i++)
126 |   {
127 |     q = 0.0;
128 |     for (int k = 0; k < 14; k++)
129 |     {
130 |       q += iau06_cip_y2[i][k + 3] * nv[k];
131 |     }
132 |     y += (iau06_cip_y2[i][1] * sin(q) + iau06_cip_y2[i][2] * cos(q)) * t[1];
133 |   }
134 | 
135 |   for (int i = 0; i < 5; i++)
136 |   {
137 |     q = 0.0;
138 |     for (int k = 0; k < 14; k++)
139 |     {
140 |       q += iau06_cip_y3[i][k + 3] * nv[k];
141 |     }
142 |     y += (iau06_cip_y3[i][1] * sin(q) + iau06_cip_y3[i][2] * cos(q)) * t[2];
143 |   }
144 | 
145 |   q = 0.0;
146 |   for (int k = 0; k < 14; k++)
147 |   {
148 |     q += iau06_cip_y4[k + 3] * nv[k];
149 |   }
150 |   y += (iau06_cip_y4[1] * sin(q) + iau06_cip_y4[2] * cos(q)) * t[3];
151 | 
152 |   return y;
153 | }
154 | 
155 | double iau06_compute_s(const double nv[14], const double t[5])
156 | {
157 |   double q;
158 |   double s = 0.0;
159 | 
160 |   for (int i = 0; i < 33; i++)
161 |   {
162 |     q = 0.0;
163 |     for (int k = 0; k < 8; k++)
164 |     {
165 |       int nut_index = (k < 5) ? k : (k + 2);
166 |       q += iau06_cip_s0[i][k + 3] * nv[nut_index];
167 |     }
168 |     s += (iau06_cip_s0[i][1] * sin(q) + iau06_cip_s0[i][2] * cos(q));
169 |   }
170 | 
171 |   for (int i = 0; i < 3; i++)
172 |   {
173 |     q = 0.0;
174 |     for (int k = 0; k < 8; k++)
175 |     {
176 |       int nut_index = (k < 5) ? k : (k + 2);
177 |       q += iau06_cip_s1[i][k + 3] * nv[nut_index];
178 |     }
179 |     s += (iau06_cip_s1[i][1] * sin(q) + iau06_cip_s1[i][2] * cos(q)) * t[0];
180 |   }
181 | 
182 |   for (int i = 0; i < 25; i++)
183 |   {
184 |     q = 0.0;
185 |     for (int k = 0; k < 8; k++)
186 |     {
187 |       int nut_index = (k < 5) ? k : (k + 2);
188 |       q += iau06_cip_s2[i][k + 3] * nv[nut_index];
189 |     }
190 |     s += (iau06_cip_s2[i][1] * sin(q) + iau06_cip_s2[i][2] * cos(q)) * t[1];
191 |   }
192 | 
193 |   for (int i = 0; i < 4; i++)
194 |   {
195 |     q = 0.0;
196 |     for (int k = 0; k < 8; k++)
197 |     {
198 |       int nut_index = (k < 5) ? k : (k + 2);
199 |       q += iau06_cip_s3[i][k + 3] * nv[nut_index];
200 |     }
201 |     s += (iau06_cip_s3[i][1] * sin(q) + iau06_cip_s3[i][2] * cos(q)) * t[2];
202 |   }
203 | 
204 |   q = 0.0;
205 |   for (int k = 0; k < 8; k++)
206 |   {
207 |     int nut_index = (k < 5) ? k : (k + 2);
208 |     q += iau06_cip_s4[k + 3] * nv[nut_index];
209 |   }
210 |   s += (iau06_cip_s4[1] * sin(q) + iau06_cip_s4[2] * cos(q)) * t[3];
211 | 
212 |   return s;
213 | }
214 | 
215 | void iau06_get_xys(const double tt, double *x, double *y, double *s)
216 | {
217 |   double t[5] = {tt, pow(tt, 2.0), pow(tt, 3.0), pow(tt, 4.0), pow(tt, 5.0)};
218 |   double nv[14];
219 |   iau06_compute_nutation_v(t, nv);
220 | 
221 |   double x0 = -16617.0 + 2004191898.0 * t[0] - 429782.9 * t[1] - 198618.34 * t[2] + 7.578 * t[3] + 5.9285 * t[4];
222 |   double y0 = -6951.0 - 25896.0 * t[0] - 22407274.7 * t[1] + 1900.59 * t[2] + 1112.526 * t[3] + 0.1358 * t[4];
223 |   double s0 = 94.0 + 3808.65 * t[0] - 122.68 * t[1] - 72574.11 * t[2] + 27.98 * t[3] + 15.62 * t[4];
224 | 
225 |   double x_temp = x0 + iau06_compute_x(nv, t);
226 |   double y_temp = y0 + iau06_compute_y(nv, t);
227 |   double s_temp = s0 + iau06_compute_s(nv, t);
228 | 
229 |   *x = (x_temp * 1e-6 / 3600.0) * D2R;
230 |   *y = (y_temp * 1e-6 / 3600.0) * D2R;
231 |   *s = (s_temp * 1e-6 / 3600.0) * D2R - (*x) * (*y) / 2.0;
232 | }
233 | 


--------------------------------------------------------------------------------
/spacetime/iau06/iau06.h:
--------------------------------------------------------------------------------
 1 | #ifndef _IAU06_H_
 2 | #define _IAU06_H_
 3 | 
 4 | #ifdef __cplusplus
 5 | extern "C"
 6 | {
 7 | #endif
 8 | 
 9 | void iau06_get_xys(const double t, double *x, double *y, double *s);
10 | 
11 | #ifdef __cplusplus
12 | }
13 | #endif
14 | 
15 | #endif // iau06_.h
16 | 


--------------------------------------------------------------------------------
/spacetime/iau06/iau06_data.h:
--------------------------------------------------------------------------------
 1 | #ifndef _IAU06_DATA_H_
 2 | #define _IAU06_DATA_H_
 3 | 
 4 | #ifdef __cplusplus
 5 | extern "C"
 6 | {
 7 | #endif
 8 | 
 9 | extern const double iau06_cip_s0[33][17];
10 | extern const double iau06_cip_s1[3][17];
11 | extern const double iau06_cip_s2[25][17];
12 | extern const double iau06_cip_s3[4][17];
13 | extern const double iau06_cip_s4[17];
14 | 
15 | extern const double iau06_cip_x0[1306][17];
16 | extern const double iau06_cip_x1[253][17];
17 | extern const double iau06_cip_x2[36][17];
18 | extern const double iau06_cip_x3[4][17];
19 | extern const double iau06_cip_x4[17];
20 | 
21 | extern const double iau06_cip_y0[962][17];
22 | extern const double iau06_cip_y1[277][17];
23 | extern const double iau06_cip_y2[30][17];
24 | extern const double iau06_cip_y3[5][17];
25 | extern const double iau06_cip_y4[17];
26 | 
27 | #ifdef __cplusplus
28 | }
29 | #endif
30 | 
31 | #endif // iau06_data.h
32 | 


--------------------------------------------------------------------------------
/spacetime/test_frame.c:
--------------------------------------------------------------------------------
  1 | // rms (2025-02-27 00:58:43)
  2 | 
  3 | #include <math.h>
  4 | #include <stdio.h>
  5 | #include <string.h>
  6 | #include <stdbool.h>
  7 | 
  8 | #include "time.h"
  9 | #include "frame.h"
 10 | #include "attitude.h"
 11 | 
 12 | #define RED "\x1b[31m"
 13 | #define GREEN "\x1b[32m"
 14 | #define RESET "\x1b[0m"
 15 | 
 16 | // Error tolerance
 17 | #define TOLERANCE 1e-8
 18 | 
 19 | // Is the deviation within threshold?
 20 | bool compare_matrices(double mat1[3][3], double mat2[3][3])
 21 | {
 22 |   for (int i = 0; i < 3; i++)
 23 |   {
 24 |     for (int j = 0; j < 3; j++)
 25 |     {
 26 |       if (fabs(mat1[i][j] - mat2[i][j]) > TOLERANCE)
 27 |       {
 28 |         return false;
 29 |       }
 30 |     }
 31 |   }
 32 | 
 33 |   return true;
 34 | }
 35 | 
 36 | bool test_single_case(utc_t t, double ground_truth[3][3], const char *test_name)
 37 | {
 38 |   double dcm[3][3];
 39 |   frame_eci_to_ecef_dcm(t, dcm);
 40 | 
 41 |   bool status = compare_matrices(dcm, ground_truth);
 42 | 
 43 |   if (status)
 44 |   {
 45 |     printf(GREEN "Test %s PASSED!\n", test_name);
 46 |   }
 47 |   else
 48 |   {
 49 |     printf(RED "Test %s FAILED.\n", test_name);
 50 |   }
 51 | 
 52 |   return status;
 53 | }
 54 | 
 55 | bool test_frame_eci_to_ecef_dcm()
 56 | {
 57 |   bool status = true;
 58 | 
 59 |   // Test cases
 60 |   utc_t test_times[] =
 61 |   {
 62 |     {.year = 2024, .month = 1, .day = 12, .hour = 4, .minute = 52, .second = 12},
 63 |     {.year = 2023, .month = 6, .day = 15, .hour = 12, .minute = 0, .second = 0},
 64 |     {.year = 2022, .month = 12, .day = 31, .hour = 23, .minute = 59, .second = 59},
 65 |     {.year = 2021, .month = 3, .day = 20, .hour = 12, .minute = 0, .second = 0},
 66 |     {.year = 2020, .month = 7, .day = 4, .hour = 18, .minute = 30, .second = 0},
 67 |     {.year = 2019, .month = 9, .day = 23, .hour = 6, .minute = 0, .second = 0},
 68 |     {.year = 2018, .month = 2, .day = 14, .hour = 10, .minute = 15, .second = 30},
 69 |     {.year = 2017, .month = 5, .day = 1, .hour = 0, .minute = 0, .second = 0},
 70 |     {.year = 2016, .month = 8, .day = 8, .hour = 8, .minute = 8, .second = 8},
 71 |     {.year = 2015, .month = 11, .day = 11, .hour = 11, .minute = 11, .second = 11},
 72 |     {.year = 2014, .month = 4, .day = 22, .hour = 16, .minute = 45, .second = 0},
 73 |     {.year = 2013, .month = 10, .day = 31, .hour = 20, .minute = 0, .second = 0}
 74 |   };
 75 | 
 76 |   // Ground truth DCMs from MATLAB for each test case
 77 |   double ground_truths[][3][3] = {
 78 |     { // Test Case 1: 2024-01-12 04:52:12
 79 |       {-0.997637610071718, -0.068657153281647, 0.002322557566614},
 80 |       {0.068657043997502, -0.997640313048689, -0.000126844940965},
 81 |       {0.002325785870385, 0.000032914653284, 0.999997294814696}
 82 |     },
 83 |     { // Test Case 2: 2023-06-15 12:00:00
 84 |       {0.118135345851863, 0.992997458396542, -0.000296105502455},
 85 |       {-0.992994928395361, 0.118135713335329, 0.002241743929254},
 86 |       {0.002261026658881, 0.000029202067814, 0.999997443449576}
 87 |     },
 88 |     { // Test Case 3: 2022-12-31 23:59:59
 89 |       {-0.175236910751489, 0.984526228084138, 0.000362387491732},
 90 |       {-0.984523804613696, -0.175237283521158, 0.002184631112492},
 91 |       {0.002214330428570, 0.000026048895180, 0.999997548028098}
 92 |     },
 93 |     { // Test Case 4: 2021-03-20 12:00:00
 94 |       {0.999370507366738, -0.035418595751463, -0.002027826556472},
 95 |       {0.035418503094663, 0.999372564652670, -0.000081597043715},
 96 |       {0.002029444279118, 0.000009723097811, 0.999997940628569}
 97 |     },
 98 |     { // Test Case 5: 2020-07-04 18:30:00
 99 |       {-0.936892730101000, -0.349612136140604, 0.001834815276248},
100 |       {0.349611453711205, -0.936894526737318, -0.000690799242776},
101 |       {0.001960540188802, -0.000005732352496, 0.999998078122807}
102 |     },
103 |     { // Test Case 6: 2019-09-23 06:00:00
104 |       {-0.027062346343195, 0.999633745658313, 0.000062861216759},
105 |       {-0.999631972868165, -0.027062416739603, 0.001882662984409},
106 |       {0.001883674627362, -0.000011888804394, 0.999998225812703}
107 |     },
108 |     { // Test Case 7: 2018-02-14 10:15:30
109 |       {0.469961212326295, -0.882686660628696, -0.000847382416903},
110 |       {0.882685353543536, 0.469961975056226, -0.001519421262892},
111 |       {0.001739410394906, -0.000033902989508, 0.999998486649888}
112 |     },
113 |     { // Test Case 8: 2017-05-01 00:00:00
114 |       {-0.778312296779783, -0.627876069100959, 0.001269067258962},
115 |       {0.627875145428585, -0.778313330273630, -0.001077808612440},
116 |       {0.001664462199486, -0.000042055906858, 0.999998613897483}
117 |     },
118 |     { // Test Case 9: 2016-08-08 08:08:08
119 |       {0.189298101569868, 0.981919630977741, -0.000258925822983},
120 |       {-0.981918352376658, 0.189298273100650, 0.001585265599304},
121 |       {0.001605617623422, -0.000045843750941, 0.999998709944367}
122 |     },
123 |     { // Test Case 10: 2015-11-11 11:11:11
124 |       {-0.789350900983387, -0.613941165684193, 0.001183298376960},
125 |       {0.613940383190589, -0.789351786440114, -0.000981392680737},
126 |       {0.001536556054151, -0.000048188537779, 0.999998818335980}
127 |     },
128 |     { // Test Case 11: 2014-04-22 16:45:00
129 |       {-0.203297890121490, 0.979116878640318, 0.000325321059835},
130 |       {-0.979115925705476, -0.203298146341569, 0.001366646988283},
131 |       {0.001404244301801, -0.000040690581393, 0.999999013220622}
132 |     },
133 |     { // Test Case 12: 2013-10-31 20:00:00
134 |       {0.940301701551049, -0.340339594015884, -0.001292596342967},
135 |       {0.340339325798953, 0.940302589276643, -0.000428852370439},
136 |       {0.001361387129830, -0.000036670754258, 0.999999072639739}
137 |     }
138 |   };
139 | 
140 |   // Input for each test case
141 |   const char *test_names[] =
142 |   {
143 |     "2024-01-12 04:52:12",
144 |     "2023-06-15 12:00:00",
145 |     "2022-12-31 23:59:59",
146 |     "2021-03-20 12:00:00",
147 |     "2020-07-04 18:30:00",
148 |     "2019-09-23 06:00:00",
149 |     "2018-02-14 10:15:30",
150 |     "2017-05-01 00:00:00",
151 |     "2016-08-08 08:08:08",
152 |     "2015-11-11 11:11:11",
153 |     "2014-04-22 16:45:00",
154 |     "2013-10-31 20:00:00"
155 |   };
156 | 
157 |   for (size_t i = 0; i < sizeof(test_times) / sizeof(*test_times); i++)
158 |   {
159 |     status &= test_single_case(test_times[i], ground_truths[i], test_names[i]);
160 |   }
161 | 
162 |   return status;
163 | }
164 | 
165 | int main()
166 | {
167 |   test_frame_eci_to_ecef_dcm();
168 |   printf(RESET);
169 | 
170 |   return 0;
171 | }
172 | 


--------------------------------------------------------------------------------
/spacetime/tests.c:
--------------------------------------------------------------------------------
 1 | // Numerical validations
 2 | // 2024-12-25
 3 | 
 4 | #include <stdio.h>
 5 | #include "time.h"
 6 | 
 7 | int main(void)
 8 | {
 9 |   // Example 5.04. Ref [1].
10 |   utc_t utc;
11 |   utc.year = 2004;
12 |   utc.month = 5;
13 |   utc.day = 12;
14 |   utc.hour = 14;
15 |   utc.minute = 45;
16 |   utc.second = 30;
17 | 
18 |   printf("Julian day number: %f\n", time_julian_date(utc)); // 2453138.115
19 | 
20 |   // Example 5.06. Ref [1].
21 |   utc.year = 2004;
22 |   utc.month = 3;
23 |   utc.day = 3;
24 |   utc.hour = 4;
25 |   utc.minute = 30;
26 |   utc.second = 0;
27 | 
28 |   const double el = 139.0 + 47.0 / 60.0 + 0.0 / 3600.0;
29 | 
30 |   printf("Local sidereal time [deg]: %f\n", time_local_sidereal_deg(utc, el)); // 8.57688
31 |   printf("Local sidereal time [hr]: %f\n", time_local_sidereal_hr(utc, el)); // 0.571792
32 | 
33 |   return 0;
34 | }
35 | 


--------------------------------------------------------------------------------
/spacetime/time.c:
--------------------------------------------------------------------------------
 1 | #include <math.h>
 2 | #include "time.h"
 3 | 
 4 | double fix(double x)
 5 | {
 6 |   if (x > 0)
 7 |   {
 8 |     return floor(x);
 9 |   }
10 | 
11 |   return ceil(x);
12 | }
13 | 
14 | /**
15 |  * @brief UTC to Julian date.
16 |  *
17 |  * Number of decimal days since noon on November 24, 4714 BCE in the proleptic
18 |  * Gregorian calendar, or January 1, 4713 BCE in the proleptic Julian calendar.
19 |  *
20 |  * @param t UTC
21 |  * @return Julian date
22 |  */
23 | double time_julian_date(const utc_t t)
24 | {
25 |   double j0 = 367 * t.year - fix(7.0 * (t.year + fix((t.month + 9.0) / 12.0)) / 4.0) + fix(275.0 * t.month / 9.0) + t.day + 1721013.5;
26 |   double ut = t.hour + t.minute / 60.0 + t.second / 3600.0;
27 | 
28 |   return j0 + ut / 24.0;
29 | }
30 | 
31 | double normalize_zero_to_360(double angle)
32 | {
33 |   while (angle < 0)
34 |   {
35 |     angle += 360.0;
36 |   }
37 | 
38 |   while (angle >= 360.0)
39 |   {
40 |     angle -= 360.0;
41 |   }
42 | 
43 |   return angle;
44 | }
45 | 
46 | // UTC to Greenwich sidereal time.
47 | double time_greenwich_sidereal(const utc_t t)
48 | {
49 |   const double j0 = 367 * t.year - fix(7.0 * (t.year + fix((t.month + 9.0) / 12.0)) / 4.0) + fix(275.0 * t.month / 9.0) + t.day + 1721013.5;
50 |   const double j = (j0 - 2451545.0) / 36525.0;
51 |   const double j_sq = j * j;
52 |   const double g0 = normalize_zero_to_360(100.4606184 + 36000.77004 * j + 0.000387933 * j_sq - 2.583e-8 * j_sq * j);
53 |   const double ut = t.hour + t.minute / 60.0 + t.second / 3600.0;
54 | 
55 |   return g0 + 360.98564724 * ut / 24.0;
56 | }
57 | 
58 | /**
59 |  * @brief UTC to local sidereal time [deg].
60 |  * @param t Time in UTC
61 |  * @param el East longitude [deg]
62 |  * @return Local sidereal time [deg]
63 |  */
64 | double time_local_sidereal_deg(const utc_t t, const double elon)
65 | {
66 |   const double lst = time_greenwich_sidereal(t) + elon;
67 |   return lst - 360.0 * fix(lst / 360.0);
68 | }
69 | 
70 | // UTC to local sidereal time [hr].
71 | double time_local_sidereal_hr(const utc_t t, const double elon)
72 | {
73 |   return time_local_sidereal_deg(t, elon) / 15.0;
74 | }
75 | 


--------------------------------------------------------------------------------
/spacetime/time.h:
--------------------------------------------------------------------------------
 1 | /**
 2 |  * @brief Standard time conversions
 3 |  * @date 2024-12-24
 4 |  * @cite [1] Curtis - Orbital mechanics for engineering students (2020)
 5 |  */
 6 | 
 7 | #ifndef _ADCS_TIME_H_
 8 | #define _ADCS_TIME_H_
 9 | 
10 | #include <inttypes.h>
11 | 
12 | #ifdef __cplusplus
13 | extern "C"
14 | {
15 | #endif
16 | 
17 | typedef struct
18 | {
19 |   uint16_t year;
20 |   uint8_t month;
21 |   uint8_t day;
22 |   uint8_t hour;
23 |   uint8_t minute;
24 |   uint8_t second;
25 | }utc_t;
26 | 
27 | double time_julian_date(const utc_t t);
28 | double time_greenwich_sidereal(const utc_t t);
29 | double time_local_sidereal_hr(const utc_t t, const double elon);
30 | double time_local_sidereal_deg(const utc_t t, const double elon);
31 | 
32 | #ifdef __cplusplus
33 | }
34 | #endif
35 | 
36 | #endif // time.h
37 | 


--------------------------------------------------------------------------------