ModelSpace
•All Classes Namespaces Functions Variables Enumerations Pages
DCM.hpp
1/******************************************************************************
2* Copyright (c) ATTX LLC 2024. All Rights Reserved.
3*
4* This software and associated documentation (the "Software") are the
5* proprietary and confidential information of ATTX, LLC. The Software is
6* furnished under a license agreement between ATTX and the user organization
7* and may be used or copied only in accordance with the terms of the agreement.
8* Refer to 'license/attx_license.adoc' for standard license terms.
9*
10* EXPORT CONTROL NOTICE: THIS SOFTWARE MAY INCLUDE CONTENT CONTROLLED UNDER THE
11* INTERNATIONAL TRAFFIC IN ARMS REGULATIONS (ITAR) OR THE EXPORT ADMINISTRATION
12* REGULATIONS (EAR99). No part of the Software may be used, reproduced, or
13* transmitted in any form or by any means, for any purpose, without the express
14* written permission of ATTX, LLC.
15******************************************************************************/
16/*
17DCM header file
18
19Author: Alex Reynolds
20*/
21#ifndef DCM_HPP
22#define DCM_HPP
23
24#include <cmath>
25
26#include "core/Matrix.hpp"
27#include "core/CartesianVector.hpp"
28#include "core/safemath.hpp"
29#include "core/clockwerkerrors.h"
30
31namespace clockwerk {
32
33 /// Forward declarations for the compiler to resolve circular references
34 template<typename T>
35 class Euler321;
36 template<typename T>
37 class Quaternion;
38 template<typename T>
39 class MRP;
40
41 /// @brief Class defining a direction cosine matrix inherited from Matrix
42
43 /// This file defines a simple DCM attitude representation for cartesian
44 /// coordinate systems. It is largely the same as the base matrix class,
45 /// with the following exceptions:
46 /// - Set to be square, 3x3 (that's where it will primarily be used)
47 /// - Default constructor is identity matrix, rather than zeroes
48 /// - Inverse is set to account for orthogonal matrix
49 /// - Functions defined to convert to other attitude
50 /// - Function defined to calculate rate of change as a function of omega
51
52 /// Naming conventions
53 /// ------------------
54 /// - All naming conventions and equations are per Analytical Mechanics of
55 /// Space Systems by Schaub and Junkins
56 /// - The omega vector is sometimes denoted by w and assumed frame consistent with
57 /// the attitude representation. For example, if a DCM represents the
58 /// relative orientation between two frames [BN] the omega vector is
59 /// assumed to be w_(B/N)
60 /// - The naming convention for all vectors is:
61 /// variablename_obj1_obj2__frameN (note the double underscore preceding frame)
62 /// and reads back as:
63 /// "variable variablename representing the relationship between obj1 and obj2
64 /// represented in frameN"
65 /// - Unless otherwise noted angles are in RADIANS
66
67 /// NOTE: All attitude representations, including DCMs, are assumed to represent a
68 /// three dimensional, cartesian coordinate system because that is what they
69 /// are used, and in many cases defined, for.
70 template<typename T>
71 class DCM : public Matrix<T, 3, 3> {
72 public:
73 /// @brief Default constructor generates DCM as an identity matrix
74 DCM<T>() : Matrix<T, 3, 3>({{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}) {};
75
76 /// Constructor for Matrix class with initialization.
77 /// Initializes matrix to values passed in via array
78 DCM<T>(const T(&initial)[3][3]) : Matrix<T, 3, 3>(initial) {};
79
80 /// Copy constructor for Matrix class. Copies data from matrix
81 /// object to the current instance
82 DCM<T>(const DCM<T> &initial) : Matrix<T, 3, 3>(initial) {};
83
84 /// Constructor for Matrix class with initialization.
85 /// Initializes matrix to values passed in via array
86 DCM<T>(const std::array<std::array<T, 3>, 3> &initial) : Matrix<T, 3, 3>(initial) {};
87
88 /// Destructor -- doesn't do anything because we don't dynamically
89 /// allocate
90 ~DCM<T>() {}
91
92 /// @brief Function to return the inverse of the matrix
93 /// @param result PBR return of matrix inverse
94 /// @return Integer value corresponding to error codes in clockwerkerrrors.h
95 /// @note Overloaded for value return
96 int inverse(DCM<T> &result) const;
97 DCM<T> inverse() const {DCM<T> tmp; inverse(tmp); return tmp;}
98
99 /// ---------------------------------------------------------------------------
100 /// Dynamics and kinematics functions
101 /// ---------------------------------------------------------------------------
102 /// @brief Function to calculate the rate of change in the current
103 /// representation based on the omega vector
104 /// @param omega_f1_f2__f1 Angular velocity vector for the attitude
105 /// representation - omega of frame 1 wrt frame 2
106 /// expressed in frame 1
107 /// @param dcmdot_f1_f2 Rate of change in the current DCM
108 void rate(const CartesianVector<T, 3> &omega_f1_f2__f1, Matrix<T, 3, 3> &dcmdot_f1_f2);
109
110 /// ---------------------------------------------------------------------------
111 /// Conversions to other attitude representations
112 /// ---------------------------------------------------------------------------
113 /// @brief Function to convert current attitude to 321 Euler sequence
114 /// @param PBR return of 321 Euler sequence in PBR case
115 /// @return Integer error code corresponding to errors in clockwerkerrors.h
116 /// @note Cannot overload for return by value because conversion isn't guaranteed
117 int toEuler321(Euler321<T> &euler_f1_f2) const;
118 Euler321<T> toEuler321();
119
120 /// @brief Overloaded functions to convert current attitude to quaternion
121 /// @param PBR return of quaternion in PBR case
122 /// @return Quaternion in return by value case
123 int toQuaternion(Quaternion<T> &q_f1_f2) const;
124 Quaternion<T> toQuaternion();
125
126 /// @brief Overloaded functions to convert current attitude to MRP
127 /// @param PBR return of MRP in PBR case
128 /// @return MRP in return by value case
129 int toMRP(MRP<T> &mrp_f1_f2) const;
130 MRP<T> toMRP();
131 };
132
133 template <typename T>
134 int DCM<T>::inverse(DCM<T> &result) const {
135 /// Because we're taking a simple transpose, operation is guaranteed safe
136 Matrix<T, 3, 3>::transpose(result);
137 return NO_ERROR;
138 }
139
140 template <typename T>
141 void DCM<T>::rate(const CartesianVector<T, 3> &omega_f1_f2__f1, Matrix<T, 3, 3> &dcmdot_f1_f2) {
142 /// Assign output to output vector
143 dcmdot_f1_f2.values[0][0] = omega_f1_f2__f1.values[2][0]*Matrix<T, 3, 3>::values[1][0] - omega_f1_f2__f1.values[1][0]*Matrix<T, 3, 3>::values[2][0];
144 dcmdot_f1_f2.values[0][1] = omega_f1_f2__f1.values[2][0]*Matrix<T, 3, 3>::values[1][1] - omega_f1_f2__f1.values[1][0]*Matrix<T, 3, 3>::values[2][1];
145 dcmdot_f1_f2.values[0][2] = omega_f1_f2__f1.values[2][0]*Matrix<T, 3, 3>::values[1][2] - omega_f1_f2__f1.values[1][0]*Matrix<T, 3, 3>::values[2][2];
146 dcmdot_f1_f2.values[1][0] = omega_f1_f2__f1.values[0][0]*Matrix<T, 3, 3>::values[2][0] - omega_f1_f2__f1.values[2][0]*Matrix<T, 3, 3>::values[0][0];
147 dcmdot_f1_f2.values[1][1] = omega_f1_f2__f1.values[0][0]*Matrix<T, 3, 3>::values[2][1] - omega_f1_f2__f1.values[2][0]*Matrix<T, 3, 3>::values[0][1];
148 dcmdot_f1_f2.values[1][2] = omega_f1_f2__f1.values[0][0]*Matrix<T, 3, 3>::values[2][2] - omega_f1_f2__f1.values[2][0]*Matrix<T, 3, 3>::values[0][2];
149 dcmdot_f1_f2.values[2][0] = omega_f1_f2__f1.values[1][0]*Matrix<T, 3, 3>::values[0][0] - omega_f1_f2__f1.values[0][0]*Matrix<T, 3, 3>::values[1][0];
150 dcmdot_f1_f2.values[2][1] = omega_f1_f2__f1.values[1][0]*Matrix<T, 3, 3>::values[0][1] - omega_f1_f2__f1.values[0][0]*Matrix<T, 3, 3>::values[1][1];
151 dcmdot_f1_f2.values[2][2] = omega_f1_f2__f1.values[1][0]*Matrix<T, 3, 3>::values[0][2] - omega_f1_f2__f1.values[0][0]*Matrix<T, 3, 3>::values[1][2];
152 }
153
154 template <typename T>
155 int DCM<T>::toEuler321(Euler321<T> &euler_f1_f2) const {
156 /// Note: The easiest way to take the 3-2-1 Euler conversion from DCM to Euler angles
157 /// involves dividing values, so we invoke safedivide here
158
159 /// Here we implement our equations
160 /// psi = atan(C_12/C_11)
161 /// theta = -asin(C_13)
162 /// phi = atan(C_23/C_33)
163 if(Matrix<T, 3, 3>::values[0][0] == 0) {
164 return ERROR_DIVIDE_BY_ZERO;
165 }
166 euler_f1_f2.values[0][0] = atan2(Matrix<T, 3, 3>::values[0][1], Matrix<T, 3, 3>::values[0][0]);
167 if(Matrix<T, 3, 3>::values[0][2] > 1 || Matrix<T, 3, 3>::values[0][2] < -1) {
168 return ERROR_INVALID_RANGE;
169 }
170 euler_f1_f2.values[1][0] = -asin(Matrix<T, 3, 3>::values[0][2]);
171 if(Matrix<T, 3, 3>::values[2][2] == 0) {
172 return ERROR_DIVIDE_BY_ZERO;
173 }
174 euler_f1_f2.values[2][0] = atan2(Matrix<T, 3, 3>::values[1][2], Matrix<T, 3, 3>::values[2][2]);
175
176 return NO_ERROR;
177 }
178
179 template<typename T>
180 Euler321<T> DCM<T>::toEuler321() {
181 Euler321<T> tmp;
182 toEuler321(tmp);
183 return tmp;
184 }
185
186 template <typename T>
187 int DCM<T>::toQuaternion(Quaternion<T> &quat_f1_f2) const {
188 /// Apply Shepperd's method
189 /// First calculate our scaled beta squared values
190 /// Note that we
191 int err;
192 T tmpTrace = T();
193 Matrix<T, 3, 3>::trace(tmpTrace);
194 quat_f1_f2.values[0][0] = 1 + tmpTrace;
195 quat_f1_f2.values[1][0] = 1 + 2*Matrix<T, 3, 3>::values[0][0] - tmpTrace;
196 quat_f1_f2.values[2][0] = 1 + 2*Matrix<T, 3, 3>::values[1][1] - tmpTrace;
197 quat_f1_f2.values[3][0] = 1 + 2*Matrix<T, 3, 3>::values[2][2] - tmpTrace;
198
199 /// Now determine index of the greatest beta squared
200 std::pair<unsigned int, unsigned int> indices;
201 T value;
202 quat_f1_f2.max(value, indices);
203
204 /// Now calculate other three values based on greatest element
205 switch(indices.first) {
206 case 0:
207 err = safeSqrt(0.25*quat_f1_f2.values[0][0], quat_f1_f2.values[0][0]);
208 if(err) {return err;}
209 err = safeDivide(0.25*(Matrix<T, 3, 3>::values[1][2] - Matrix<T, 3, 3>::values[2][1]),
210 quat_f1_f2.values[0][0],
211 quat_f1_f2.values[1][0]);
212 if(err) {return err;}
213 err = safeDivide(0.25*(Matrix<T, 3, 3>::values[2][0] - Matrix<T, 3, 3>::values[0][2]),
214 quat_f1_f2.values[0][0],
215 quat_f1_f2.values[2][0]);
216 if(err) {return err;}
217 err = safeDivide(0.25*(Matrix<T, 3, 3>::values[0][1] - Matrix<T, 3, 3>::values[1][0]),
218 quat_f1_f2.values[0][0],
219 quat_f1_f2.values[3][0]);
220 if(err) {return err;}
221 quat_f1_f2.normalize();
222 break;
223 case 1:
224 err = safeSqrt(0.25*quat_f1_f2.values[1][0], quat_f1_f2.values[1][0]);
225 if(err) {return err;}
226 err = safeDivide(0.25*(Matrix<T, 3, 3>::values[1][2] - Matrix<T, 3, 3>::values[2][1]),
227 quat_f1_f2.values[1][0],
228 quat_f1_f2.values[0][0]);
229 if(err) {return err;}
230 err = safeDivide(0.25*(Matrix<T, 3, 3>::values[0][1] + Matrix<T, 3, 3>::values[1][0]),
231 quat_f1_f2.values[1][0],
232 quat_f1_f2.values[2][0]);
233 if(err) {return err;}
234 err = safeDivide(0.25*(Matrix<T, 3, 3>::values[2][0] + Matrix<T, 3, 3>::values[0][2]),
235 quat_f1_f2.values[1][0],
236 quat_f1_f2.values[3][0]);
237 if(err) {return err;}
238 quat_f1_f2.normalize();
239 break;
240 case 2:
241 err = safeSqrt(0.25*quat_f1_f2.values[2][0], quat_f1_f2.values[2][0]);
242 if(err) {return err;}
243 err = safeDivide(0.25*(Matrix<T, 3, 3>::values[2][0] - Matrix<T, 3, 3>::values[0][2]),
244 quat_f1_f2.values[2][0],
245 quat_f1_f2.values[0][0]);
246 if(err) {return err;}
247 err = safeDivide(0.25*(Matrix<T, 3, 3>::values[0][1] + Matrix<T, 3, 3>::values[1][0]),
248 quat_f1_f2.values[2][0],
249 quat_f1_f2.values[1][0]);
250 err = safeDivide(0.25*(Matrix<T, 3, 3>::values[1][2] + Matrix<T, 3, 3>::values[2][1]),
251 quat_f1_f2.values[2][0],
252 quat_f1_f2.values[3][0]);
253 if(err) {return err;}
254 quat_f1_f2.normalize();
255 break;
256 case 3:
257 err = safeSqrt(0.25*quat_f1_f2.values[3][0], quat_f1_f2.values[3][0]);
258 if(err) {return err;}
259 err = safeDivide(0.25*(Matrix<T, 3, 3>::values[0][1] - Matrix<T, 3, 3>::values[1][0]),
260 quat_f1_f2.values[3][0],
261 quat_f1_f2.values[0][0]);
262 if(err) {return err;}
263 err = safeDivide(0.25*(Matrix<T, 3, 3>::values[2][0] + Matrix<T, 3, 3>::values[0][2]),
264 quat_f1_f2.values[3][0],
265 quat_f1_f2.values[1][0]);
266 if(err) {return err;}
267 err = safeDivide(0.25*(Matrix<T, 3, 3>::values[1][2] + Matrix<T, 3, 3>::values[2][1]),
268 quat_f1_f2.values[3][0],
269 quat_f1_f2.values[2][0]);
270 if(err) {return err;}
271 quat_f1_f2.normalize();
272 break;
273 default:
274 /// We shouldn't be here
275 return ERROR_DIMENSIONS;
276 break;
277 }
278
279 return NO_ERROR;
280 }
281
282 template<typename T>
283 Quaternion<T> DCM<T>::toQuaternion() {
284 Quaternion<T> tmp;
285 toQuaternion(tmp);
286 return tmp;
287 }
288
289 template <typename T>
290 int DCM<T>::toMRP(MRP<T> &mrp_f1_f2) const {
291 // First convert our MRP to a quaternion
292 int err;
293 T trace = T();
294 T tau;
295
296 /// Calculate the tau parameter for MRP calculation
297 Matrix<T, 3, 3>::trace(trace);
298 err = safeSqrt(trace + 1, tau);
299
300 /// Now calculate our parameters
301 err = safeDivide(1, tau*(tau+2), tau);
302 if(err) {return err;}
303 mrp_f1_f2.values[0][0] = tau*(Matrix<T, 3, 3>::values[1][2] - Matrix<T, 3, 3>::values[2][1]);
304 mrp_f1_f2.values[1][0] = tau*(Matrix<T, 3, 3>::values[2][0] - Matrix<T, 3, 3>::values[0][2]);
305 mrp_f1_f2.values[2][0] = tau*(Matrix<T, 3, 3>::values[0][1] - Matrix<T, 3, 3>::values[1][0]);
306
307 return NO_ERROR;
308 }
309
310 template<typename T>
311 MRP<T> DCM<T>::toMRP() {
312 MRP<T> tmp;
313 toMRP(tmp);
314 return tmp;
315 }
316
317}
318
319#endif
clockwerk::CartesianVector
Standard vector class derived from Matrix.
Definition CartesianVector.hpp:39
clockwerk::DCM
Class defining a direction cosine matrix inherited from Matrix.
Definition DCM.hpp:71
clockwerk::DCM::inverse
int inverse(DCM< T > &result) const
Function to return the inverse of the matrix.
Definition DCM.hpp:134
clockwerk::DCM::rate
void rate(const CartesianVector< T, 3 > &omega_f1_f2__f1, Matrix< T, 3, 3 > &dcmdot_f1_f2)
Function to calculate the rate of change in the current representation based on the omega vector.
Definition DCM.hpp:141
clockwerk::DCM::toMRP
int toMRP(MRP< T > &mrp_f1_f2) const
Overloaded functions to convert current attitude to MRP.
Definition DCM.hpp:290
clockwerk::DCM::toQuaternion
int toQuaternion(Quaternion< T > &q_f1_f2) const
Overloaded functions to convert current attitude to quaternion.
Definition DCM.hpp:187
clockwerk::DCM::toEuler321
int toEuler321(Euler321< T > &euler_f1_f2) const
Function to convert current attitude to 321 Euler sequence.
Definition DCM.hpp:155
clockwerk::Matrix
Matrix math implementation.
Definition Matrix.hpp:54
NO_ERROR
#define NO_ERROR
Definition clockwerkerrors.h:31
ERROR_DIMENSIONS
#define ERROR_DIMENSIONS
Definition clockwerkerrors.h:38
ERROR_INVALID_RANGE
#define ERROR_INVALID_RANGE
Definition clockwerkerrors.h:50
ERROR_DIVIDE_BY_ZERO
#define ERROR_DIVIDE_BY_ZERO
Definition clockwerkerrors.h:46