From 53c1c35c4fc438d0b2e9c32a62fe5e7c777cb08a Mon Sep 17 00:00:00 2001 From: Ivan Padilla Date: Tue, 12 Nov 2024 13:02:14 -0500 Subject: [PATCH 1/5] Initial commit of gyro-integrating function. --- qpoint/_libqpoint.py | 18 ++++++ qpoint/qpoint_class.py | 58 ++++++++++++++++++ src/qpoint.c | 132 +++++++++++++++++++++++++++++++++++++++++ src/qpoint.h | 8 +++ src/quaternion.c | 69 +++++++++++++++++++++ src/quaternion.h | 7 +++ 6 files changed, 292 insertions(+) diff --git a/qpoint/_libqpoint.py b/qpoint/_libqpoint.py index a666d30..5df566e 100644 --- a/qpoint/_libqpoint.py +++ b/qpoint/_libqpoint.py @@ -541,6 +541,24 @@ def setargs(fname, arg=None, res=None): ct.c_int, ), ) +setargs( + "qp_omega2azelpa", + arg=( + qp_memory_t_p, # params + ct.c_double, # init_az + ct.c_double, # init_el + ct.c_double, # init_roll + arr, # omega_x + arr, # omega_y + arr, # omega_z + warr, # azimuth (output) + warr, # elevation (output) + warr, # psi (output) + ct.c_double, # delta_t + ct.c_int, # n_samples + ct.c_int #fast_rot + ) +) setargs( "qp_azel2bore", arg=( diff --git a/qpoint/qpoint_class.py b/qpoint/qpoint_class.py index 8043e53..1315fed 100644 --- a/qpoint/qpoint_class.py +++ b/qpoint/qpoint_class.py @@ -1323,6 +1323,64 @@ def quat2radecpa(self, quat, **kwargs): if n == 1: return ra[0], dec[0], pa[0] return ra, dec, pa + + def omega2azelpa(self, init_az, init_el, init_roll, + omega_x, omega_y, omega_z, + delta_t, fast_rot=False, + **kwargs): + """ + Compute azimuth, elevation, and roll from angular velocity data. + + Parameters + ---------- + init_az : float + Initial azimuth in degrees. + init_el : float + Initial elevation in degrees. + init_roll : float + Initial roll (boresight rotation) in degrees. + omega_x, omega_y, omega_z : np.ndarray + Angular velocity components in the AZ/EL/ROLL frame. + delta_t : float + Time interval between samples. + fast_rot : bool, optional + If True, uses fast quaternion rotation (default: False). + + Returns + ------- + azimuth, elevation, roll : np.ndarray + Arrays of azimuth, elevation, and roll values for each sample. + """ + # self.set(**kwargs) + + omega_x = check_input("omega_x", omega_x, dtype=np.double) + omega_y = check_input("omega_y", omega_y, dtype=np.double) + omega_z = check_input("omega_z", omega_z, dtype=np.double) + + # omega_x, omega_y, omega_z = check_inputs(omega_x, omega_y, omega_z) + + n_samples = omega_x.size + + azimuth = check_output("azimuth", shape=(n_samples,), dtype=np.double) + elevation = check_output("elevation", shape=(n_samples,), dtype=np.double) + roll = check_output("roll", shape=(n_samples,), dtype=np.double) + + # print(f"Addresses in Python - Azimuth: {azimuth.__array_interface__['data'][0]}, " + # f"Elevation: {elevation.__array_interface__['data'][0]}, " + # f"Roll: {roll.__array_interface__['data'][0]}") + + qp.qp_omega2azelpa(self._memory, + init_az, init_el, init_roll, + omega_x, omega_y, omega_z, + azimuth, elevation, roll, + delta_t, int(n_samples), int(fast_rot) + ) + + # print(f"Addresses in Python - Azimuth: {hex(id(azimuth))}, " + # f"Elevation: {hex(id(elevation))}, " + # f"Roll: {hex(id(roll))}") + + return azimuth, elevation, roll def quat2pixpa(self, quat, nside=256, **kwargs): """ diff --git a/src/qpoint.c b/src/qpoint.c index 6e3482f..56db4df 100644 --- a/src/qpoint.c +++ b/src/qpoint.c @@ -1126,3 +1126,135 @@ void qp_azel2rasindec_hwp(qp_memory_t *mem, az, el, 0, pitch, roll, lon, lat, ctime, hwp, ra, sindec, sin2psi, cos2psi, n); } + + + +// #include + +// void qp_quat_azelpsi(const quat_t q, double *az, double *el, double *psi) { +// // Normalize the quaternion +// double norm = sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]); +// double q0 = q[0] / norm; +// double q1 = q[1] / norm; +// double q2 = q[2] / norm; +// double q3 = q[3] / norm; + +// // Extract elevation (el) +// double sin_el = 2.0 * (q0 * q2 - q1 * q3); +// double el_rad = asin(sin_el) + M_PI_2; // el = π/2 - asin(sin_el) + +// // Compute cos(el) for use in az and psi calculations +// double cos_el = sqrt(1.0 - sin_el * sin_el); + +// // Extract azimuth (az) +// double sin_az = (2.0 * (q0 * q1 + q2 * q3)) / cos_el; +// double cos_az = (q0 * q0 + q3 * q3 - q1 * q1 - q2 * q2) / cos_el; +// double az_rad = -atan2(sin_az, cos_az); // az = -atan2(sin_az, cos_az) + +// // Extract roll (psi) +// double sin_psi = (2.0 * (q0 * q3 + q1 * q2)) / cos_el; +// double cos_psi = (q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) / cos_el; +// double psi_rad = M_PI - atan2(sin_psi, cos_psi); // psi = π - atan2(sin_psi, cos_psi) + +// // Convert from radians to degrees +// *az = az_rad * (180.0 / M_PI); +// *el = el_rad * (180.0 / M_PI); +// *psi = psi_rad * (180.0 / M_PI); +// } + +void +qp_quat_azelpsi(const quat_t q, double *az, double *el, double *psi) { + double x, y, z, w; + x = q[1]; + y = q[2]; + z = q[3]; + w = q[0]; + + double sin_el_sq = x * x + y * y; + double cos_el_sq = w * w + z * z; + + double s = atan(z / w); + double d = atan2(x, y); + + *az = (s - d); + *psi = (s + d); + + if (cos_el_sq != 0.0) { + *el = 2.0 * atan(sqrt(sin_el_sq / cos_el_sq)); + } else { + *el = (sin_el_sq < 0.5) ? 0.0 : M_PI; + } + *el = M_PI_2 - *el; // el = π/2 - el + *psi = M_PI - *psi; // psi = π - psi + + *az = -rad2deg(*az); + *el = rad2deg(*el); + *psi = rad2deg(*psi); + + if (fabs(*az) < 1e-12) *az = 0.0; + if (fabs(*el) < 1e-12) *el = 0.0; + if (fabs(*psi) < 1e-12) *psi = 0.0; +} + +// Test Quaternion_rot_vector_fast() +// void print_vector(const char *label, const double v[3]) { +// printf("%s: [%f, %f, %f]\n", label, v[0], v[1], v[2]); +// } + +// double angle = M_PI / 4; // 90 degrees in radians +// Quaternion q_rotation; +// double axis[3] = {0, 0, 1}; // Rotation around the Z-axis +// Quaternion_rot(q_rotation, angle, axis); +// // Define the vector to rotate (unit vector along X-axis) +// double vector[3] = {1, 0, 0}; +// double rotated_vector[3]; +// // Rotate the vector +// Quaternion_rot_vector_fast(q_rotation, vector, rotated_vector); +// // Print the original and rotated vectors +// print_vector("Original vector", vector); +// print_vector("Rotated vector", rotated_vector); + +// Compute attitude quaternion from angular velocity. +void +qp_omega2azelpa(qp_memory_t *mem, double init_az, double init_el, double init_roll, + double *omega_x, double *omega_y, double *omega_z, + double *azimuth, double *elevation, double *psi, + double dt, int n_samples, int fast_rot) +{ + + + + + + // fprintf(stderr, "Addresses in C - Azimuth: %p, Elevation: %p, Psi: %p\n", (void*) azimuth, (void*) elevation, (void*) psi); + // fflush(stderr); + Quaternion attitude; + qp_azelpsi_quat(init_az, init_el, init_roll, 0.0, 0.0, attitude); + printf("Initial quaternion: [%f, %f, %f, %f]\n", attitude[0], attitude[1], attitude[2], attitude[3]); + + for (int i = 0; i < n_samples; i++) { + apply_angular_velocity(attitude, omega_x[i], omega_y[i], omega_z[i], dt); + + double omega[3] = {omega_x[i], omega_y[i], omega_z[i]}; + double rotated_vec[3]; + + // Use fast or standard quaternion rotation based on fast_rot flag + if (fast_rot) { + Quaternion_rot_vector_fast(attitude, omega, rotated_vec); + } else { + Quaternion_rot_vector(attitude, omega, rotated_vec); + } + + + printf("Final quaternion: [%f, %f, %f, %f]\n", attitude[0], attitude[1], attitude[2], attitude[3]); + + // Convert quaternion to Euler angles + double az_rad, el_rad, psi_rad; + qp_quat_azelpsi(attitude, &az_rad, &el_rad, &psi_rad); + // printf("Azimuth: %f, Elevation: %f, Psi: %f\n", az_rad, el_rad, psi_rad); + // Convert from radians to degrees + azimuth[i] = az_rad ; + elevation[i] = el_rad ; + psi[i] = psi_rad ; + } +} diff --git a/src/qpoint.h b/src/qpoint.h index 989ba12..15d55be 100644 --- a/src/qpoint.h +++ b/src/qpoint.h @@ -606,6 +606,14 @@ extern "C" { double *ra, double *sindec, double *sin2psi, double *cos2psi, int n); + /* Calculate az/el/bs roll (psi) from a starting attitude value and gyro rates. + This function integrates gyro readings to get the new attitude. + It uses Horizontal-frame quaternions internally. */ + void qp_omega2azelpa(qp_memory_t *mem, double init_az, double init_el, double init_roll, + double *omega_x, double *omega_y, double *omega_z, + double *azimuth, double *elevation, double *psi, + double dt, int n_samples, int fast_rot); + #ifndef ENABLE_LITE /* ************************************************************************* diff --git a/src/quaternion.c b/src/quaternion.c index d666f75..1a10dc1 100644 --- a/src/quaternion.c +++ b/src/quaternion.c @@ -205,3 +205,72 @@ QuaternionSlerp_interpolate(const QuaternionSlerp *slerp, double t, Quaternion q q[i] = s0*slerp->q0[i] + s1*slerp->q1[i]; } + +// Standard vector rotation using quaternions. 32 multiplications. +void +Quaternion_rot_vector(Quaternion q, const double v[3], double v_rot[3]) +{ + Quaternion vector_quat = {0, v[0], v[1], v[2]}; + Quaternion temp, rotated_quat; + + Quaternion_mul(temp, q, vector_quat); + Quaternion_conj(q); + Quaternion_mul(rotated_quat, temp, q); + Quaternion_conj(q); // Restore original quaternion + + v_rot[0] = rotated_quat[1]; + v_rot[1] = rotated_quat[2]; + v_rot[2] = rotated_quat[3]; +} + +/* Vector rotation using quaternions with fewer operations. 15 multiplcations. +this computes v' = v + q_0 x t + (q_v x t) +where t = 2 x (q_v x v_v) */ +void +Quaternion_rot_vector_fast(Quaternion q, const double v[3], double v_rot[3]) +{ + double t[3]; + t[0] = 2.0 * (q[1] * v[2] - q[3] * v[1]); + t[1] = 2.0 * (q[2] * v[0] - q[1] * v[2]); + t[2] = 2.0 * (q[3] * v[0] - q[2] * v[1]); + v_rot[0] = v[0] + q[0] * t[0] + (q[2] * t[2] - q[3] * t[1]); + v_rot[1] = v[1] + q[0] * t[1] + (q[3] * t[0] - q[1] * t[2]); + v_rot[2] = v[2] + q[0] * t[2] + (q[1] * t[1] - q[2] * t[0]); +} + + +void +apply_angular_velocity(Quaternion attitude, double omega_x, double omega_y, double omega_z, double delta_t) +{ + // Compute the magnitude of the angular velocity vector + double omega_mag = sqrt(omega_x * omega_x + omega_y * omega_y + omega_z * omega_z); + double angle = omega_mag * delta_t; + + if (omega_mag == 0.0 || angle == 0.0) { + return; // No change to attitude, so return early + } + + // Normalize the omega vector to get the rotation axis + double axis[3] = {omega_x / omega_mag, omega_y / omega_mag, omega_z / omega_mag}; + // printf("Axis: [%f, %f, %f]\n", axis[0], axis[1], axis[2]); + Quaternion q_delta; + Quaternion_rot(q_delta, angle, axis); + // printf("Quaternion delta: [%f, %f, %f, %f]\n", q_delta[0], q_delta[1], q_delta[2], q_delta[3]); + + // Corrected: Right-multiply q_delta to attitude (attitude = attitude * q_delta) + Quaternion temp; + Quaternion_mul(temp, attitude, q_delta); + attitude[0] = temp[0]; + attitude[1] = temp[1]; + attitude[2] = temp[2]; + attitude[3] = temp[3]; + // Normalize the updated attitude quaternion to prevent drift + double norm = Quaternion_norm(attitude); + attitude[0] /= norm; + attitude[1] /= norm; + attitude[2] /= norm; + attitude[3] /= norm; + // printf("Updated quaternion: [%f, %f, %f, %f]\n", attitude[0], attitude[1], attitude[2], attitude[3]); +} + + diff --git a/src/quaternion.h b/src/quaternion.h index 8f3769e..ecd3f5e 100644 --- a/src/quaternion.h +++ b/src/quaternion.h @@ -96,6 +96,9 @@ extern "C" { void Quaternion_r2_mul(double angle, Quaternion q); void Quaternion_r3_mul(double angle, Quaternion q); + void Quaternion_rot_vector(Quaternion q, const double v[3], double v_rot[3]); + void Quaternion_rot_vector_fast(Quaternion q, const double v[3], double v_rot[3]); + // q = q * scale static inline void Quaternion_scale(Quaternion q, double scale) @@ -168,6 +171,10 @@ extern "C" { void QuaternionSlerp_interpolate(const QuaternionSlerp *slerp, double t, Quaternion z); + void + apply_angular_velocity(Quaternion attitude, double omega_x, double omega_y, double omega_z, double delta_t); + + #ifdef __cplusplus } #endif From 462912b4394546d385fd2baabf84211e0cec1fdc Mon Sep 17 00:00:00 2001 From: Ivan Padilla Date: Tue, 12 Nov 2024 13:30:43 -0500 Subject: [PATCH 2/5] Running black on python files. --- qpoint/_libqpoint.py | 18 ++++++++-------- qpoint/qpoint_class.py | 47 ++++++++++++++++++++++++++++-------------- 2 files changed, 41 insertions(+), 24 deletions(-) diff --git a/qpoint/_libqpoint.py b/qpoint/_libqpoint.py index 5df566e..6c6cf6a 100644 --- a/qpoint/_libqpoint.py +++ b/qpoint/_libqpoint.py @@ -548,16 +548,16 @@ def setargs(fname, arg=None, res=None): ct.c_double, # init_az ct.c_double, # init_el ct.c_double, # init_roll - arr, # omega_x - arr, # omega_y - arr, # omega_z - warr, # azimuth (output) - warr, # elevation (output) - warr, # psi (output) + arr, # omega_x + arr, # omega_y + arr, # omega_z + warr, # azimuth (output) + warr, # elevation (output) + warr, # psi (output) ct.c_double, # delta_t - ct.c_int, # n_samples - ct.c_int #fast_rot - ) + ct.c_int, # n_samples + ct.c_int, # fast_rot + ), ) setargs( "qp_azel2bore", diff --git a/qpoint/qpoint_class.py b/qpoint/qpoint_class.py index 1315fed..ab84c21 100644 --- a/qpoint/qpoint_class.py +++ b/qpoint/qpoint_class.py @@ -1323,11 +1323,19 @@ def quat2radecpa(self, quat, **kwargs): if n == 1: return ra[0], dec[0], pa[0] return ra, dec, pa - - def omega2azelpa(self, init_az, init_el, init_roll, - omega_x, omega_y, omega_z, - delta_t, fast_rot=False, - **kwargs): + + def omega2azelpa( + self, + init_az, + init_el, + init_roll, + omega_x, + omega_y, + omega_z, + delta_t, + fast_rot=False, + **kwargs, + ): """ Compute azimuth, elevation, and roll from angular velocity data. @@ -1366,19 +1374,28 @@ def omega2azelpa(self, init_az, init_el, init_roll, roll = check_output("roll", shape=(n_samples,), dtype=np.double) # print(f"Addresses in Python - Azimuth: {azimuth.__array_interface__['data'][0]}, " - # f"Elevation: {elevation.__array_interface__['data'][0]}, " - # f"Roll: {roll.__array_interface__['data'][0]}") - - qp.qp_omega2azelpa(self._memory, - init_az, init_el, init_roll, - omega_x, omega_y, omega_z, - azimuth, elevation, roll, - delta_t, int(n_samples), int(fast_rot) + # f"Elevation: {elevation.__array_interface__['data'][0]}, " + # f"Roll: {roll.__array_interface__['data'][0]}") + + qp.qp_omega2azelpa( + self._memory, + init_az, + init_el, + init_roll, + omega_x, + omega_y, + omega_z, + azimuth, + elevation, + roll, + delta_t, + int(n_samples), + int(fast_rot), ) # print(f"Addresses in Python - Azimuth: {hex(id(azimuth))}, " - # f"Elevation: {hex(id(elevation))}, " - # f"Roll: {hex(id(roll))}") + # f"Elevation: {hex(id(elevation))}, " + # f"Roll: {hex(id(roll))}") return azimuth, elevation, roll From 819dda695fa4085af4f2570b7de4a840a1e46949 Mon Sep 17 00:00:00 2001 From: Ivan Padilla Date: Wed, 20 Nov 2024 08:59:38 -0500 Subject: [PATCH 3/5] Bug fixes in rotation code. --- src/qpoint.c | 75 ++++++++++-------------------------------------- src/quaternion.c | 5 +--- 2 files changed, 16 insertions(+), 64 deletions(-) diff --git a/src/qpoint.c b/src/qpoint.c index 56db4df..fc34b47 100644 --- a/src/qpoint.c +++ b/src/qpoint.c @@ -1127,43 +1127,9 @@ void qp_azel2rasindec_hwp(qp_memory_t *mem, hwp, ra, sindec, sin2psi, cos2psi, n); } - - -// #include - -// void qp_quat_azelpsi(const quat_t q, double *az, double *el, double *psi) { -// // Normalize the quaternion -// double norm = sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]); -// double q0 = q[0] / norm; -// double q1 = q[1] / norm; -// double q2 = q[2] / norm; -// double q3 = q[3] / norm; - -// // Extract elevation (el) -// double sin_el = 2.0 * (q0 * q2 - q1 * q3); -// double el_rad = asin(sin_el) + M_PI_2; // el = π/2 - asin(sin_el) - -// // Compute cos(el) for use in az and psi calculations -// double cos_el = sqrt(1.0 - sin_el * sin_el); - -// // Extract azimuth (az) -// double sin_az = (2.0 * (q0 * q1 + q2 * q3)) / cos_el; -// double cos_az = (q0 * q0 + q3 * q3 - q1 * q1 - q2 * q2) / cos_el; -// double az_rad = -atan2(sin_az, cos_az); // az = -atan2(sin_az, cos_az) - -// // Extract roll (psi) -// double sin_psi = (2.0 * (q0 * q3 + q1 * q2)) / cos_el; -// double cos_psi = (q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) / cos_el; -// double psi_rad = M_PI - atan2(sin_psi, cos_psi); // psi = π - atan2(sin_psi, cos_psi) - -// // Convert from radians to degrees -// *az = az_rad * (180.0 / M_PI); -// *el = el_rad * (180.0 / M_PI); -// *psi = psi_rad * (180.0 / M_PI); -// } - void -qp_quat_azelpsi(const quat_t q, double *az, double *el, double *psi) { +qp_quat_azelpsi(const quat_t q, double *az, double *el, double *psi) +{ double x, y, z, w; x = q[1]; y = q[2]; @@ -1173,13 +1139,13 @@ qp_quat_azelpsi(const quat_t q, double *az, double *el, double *psi) { double sin_el_sq = x * x + y * y; double cos_el_sq = w * w + z * z; - double s = atan(z / w); + double s = atan2(z, w); double d = atan2(x, y); *az = (s - d); *psi = (s + d); - if (cos_el_sq != 0.0) { + if (cos_el_sq > 1e-12) { *el = 2.0 * atan(sqrt(sin_el_sq / cos_el_sq)); } else { *el = (sin_el_sq < 0.5) ? 0.0 : M_PI; @@ -1214,6 +1180,7 @@ qp_quat_azelpsi(const quat_t q, double *az, double *el, double *psi) { // print_vector("Original vector", vector); // print_vector("Rotated vector", rotated_vector); + // Compute attitude quaternion from angular velocity. void qp_omega2azelpa(qp_memory_t *mem, double init_az, double init_el, double init_roll, @@ -1221,40 +1188,28 @@ qp_omega2azelpa(qp_memory_t *mem, double init_az, double init_el, double init_ro double *azimuth, double *elevation, double *psi, double dt, int n_samples, int fast_rot) { - - - - - - // fprintf(stderr, "Addresses in C - Azimuth: %p, Elevation: %p, Psi: %p\n", (void*) azimuth, (void*) elevation, (void*) psi); - // fflush(stderr); Quaternion attitude; qp_azelpsi_quat(init_az, init_el, init_roll, 0.0, 0.0, attitude); - printf("Initial quaternion: [%f, %f, %f, %f]\n", attitude[0], attitude[1], attitude[2], attitude[3]); + // printf("Initial quaternion: [%f, %f, %f, %f]\n", attitude[0], attitude[1], attitude[2], attitude[3]); for (int i = 0; i < n_samples; i++) { - apply_angular_velocity(attitude, omega_x[i], omega_y[i], omega_z[i], dt); - double omega[3] = {omega_x[i], omega_y[i], omega_z[i]}; - double rotated_vec[3]; + double rotated_omega[3]; - // Use fast or standard quaternion rotation based on fast_rot flag + // Rotate the gyro signal into the new frame of the instrument if (fast_rot) { - Quaternion_rot_vector_fast(attitude, omega, rotated_vec); + Quaternion_rot_vector_fast(attitude, omega, rotated_omega); } else { - Quaternion_rot_vector(attitude, omega, rotated_vec); + Quaternion_rot_vector(attitude, omega, rotated_omega); } + apply_angular_velocity(attitude, rotated_omega[2], rotated_omega[1], rotated_omega[0], dt); - printf("Final quaternion: [%f, %f, %f, %f]\n", attitude[0], attitude[1], attitude[2], attitude[3]); - - // Convert quaternion to Euler angles double az_rad, el_rad, psi_rad; qp_quat_azelpsi(attitude, &az_rad, &el_rad, &psi_rad); - // printf("Azimuth: %f, Elevation: %f, Psi: %f\n", az_rad, el_rad, psi_rad); - // Convert from radians to degrees - azimuth[i] = az_rad ; - elevation[i] = el_rad ; - psi[i] = psi_rad ; + + azimuth[i] = az_rad; + elevation[i] = el_rad; + psi[i] = psi_rad; } } diff --git a/src/quaternion.c b/src/quaternion.c index 1a10dc1..d542a3d 100644 --- a/src/quaternion.c +++ b/src/quaternion.c @@ -242,7 +242,6 @@ Quaternion_rot_vector_fast(Quaternion q, const double v[3], double v_rot[3]) void apply_angular_velocity(Quaternion attitude, double omega_x, double omega_y, double omega_z, double delta_t) { - // Compute the magnitude of the angular velocity vector double omega_mag = sqrt(omega_x * omega_x + omega_y * omega_y + omega_z * omega_z); double angle = omega_mag * delta_t; @@ -250,21 +249,19 @@ apply_angular_velocity(Quaternion attitude, double omega_x, double omega_y, doub return; // No change to attitude, so return early } - // Normalize the omega vector to get the rotation axis double axis[3] = {omega_x / omega_mag, omega_y / omega_mag, omega_z / omega_mag}; // printf("Axis: [%f, %f, %f]\n", axis[0], axis[1], axis[2]); Quaternion q_delta; Quaternion_rot(q_delta, angle, axis); // printf("Quaternion delta: [%f, %f, %f, %f]\n", q_delta[0], q_delta[1], q_delta[2], q_delta[3]); - // Corrected: Right-multiply q_delta to attitude (attitude = attitude * q_delta) Quaternion temp; Quaternion_mul(temp, attitude, q_delta); attitude[0] = temp[0]; attitude[1] = temp[1]; attitude[2] = temp[2]; attitude[3] = temp[3]; - // Normalize the updated attitude quaternion to prevent drift + double norm = Quaternion_norm(attitude); attitude[0] /= norm; attitude[1] /= norm; From 13c771cbb0d6d980bd645ccb9c1245b3a12df020 Mon Sep 17 00:00:00 2001 From: Ivan Padilla Date: Tue, 3 Dec 2024 08:45:45 -0500 Subject: [PATCH 4/5] Putting psi in the [-180,180] range. --- src/qpoint.c | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/qpoint.c b/src/qpoint.c index fc34b47..7d55921 100644 --- a/src/qpoint.c +++ b/src/qpoint.c @@ -1151,12 +1151,18 @@ qp_quat_azelpsi(const quat_t q, double *az, double *el, double *psi) *el = (sin_el_sq < 0.5) ? 0.0 : M_PI; } *el = M_PI_2 - *el; // el = π/2 - el - *psi = M_PI - *psi; // psi = π - psi + *psi = M_PI - *psi; // psi = π - psi // psi = π - psi *az = -rad2deg(*az); *el = rad2deg(*el); *psi = rad2deg(*psi); + if (*psi > 180.0) { + *psi -= 360.0; + } else if (*psi < -180.0) { + *psi += 360.0; + } + if (fabs(*az) < 1e-12) *az = 0.0; if (fabs(*el) < 1e-12) *el = 0.0; if (fabs(*psi) < 1e-12) *psi = 0.0; From 9082bb709abcac3425f04016251fc28be1c31788 Mon Sep 17 00:00:00 2001 From: Ivan Padilla Date: Sat, 15 Mar 2025 14:35:46 -0400 Subject: [PATCH 5/5] Removing frame rotation from omega2azelpa. The integration happens directly in the sensor frame, so the rotation to inertial frame is not needed. --- src/qpoint.c | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/src/qpoint.c b/src/qpoint.c index 7d55921..2e7ef9a 100644 --- a/src/qpoint.c +++ b/src/qpoint.c @@ -1203,13 +1203,15 @@ qp_omega2azelpa(qp_memory_t *mem, double init_az, double init_el, double init_ro double rotated_omega[3]; // Rotate the gyro signal into the new frame of the instrument - if (fast_rot) { - Quaternion_rot_vector_fast(attitude, omega, rotated_omega); - } else { - Quaternion_rot_vector(attitude, omega, rotated_omega); - } - - apply_angular_velocity(attitude, rotated_omega[2], rotated_omega[1], rotated_omega[0], dt); + // if (fast_rot) { + // Quaternion_rot_vector_fast(attitude, omega, rotated_omega); + // } else { + // Quaternion_rot_vector(attitude, omega, rotated_omega); + // } + + // apply_angular_velocity(attitude, rotated_omega[2], rotated_omega[1], rotated_omega[0], dt); + apply_angular_velocity(attitude, omega[0], omega[1], omega[2], dt); + // apply_angular_velocity(attitude, rotated_omega[0], rotated_omega[1], rotated_omega[2], dt); double az_rad, el_rad, psi_rad; qp_quat_azelpsi(attitude, &az_rad, &el_rad, &psi_rad);