Merge branch 'eggert/acs' into marquardt/ptgCtrl
Some checks failed
EIVE/eive-obsw/pipeline/head There was a failure building this commit
Some checks failed
EIVE/eive-obsw/pipeline/head There was a failure building this commit
# Conflicts: # mission/controller/AcsController.cpp # mission/controller/AcsController.h # mission/controller/acs/AcsParameters.h # mission/controller/acs/ActuatorCmd.h # mission/controller/acs/Guidance.cpp # mission/controller/acs/Guidance.h # mission/controller/acs/MultiplicativeKalmanFilter.cpp # mission/controller/acs/OutputValues.h # mission/controller/acs/SensorProcessing.cpp # mission/controller/acs/SensorProcessing.h # mission/controller/acs/control/Detumble.cpp # mission/controller/acs/control/Detumble.h # mission/controller/acs/control/PtgCtrl.cpp # mission/controller/acs/util/MathOperations.h
This commit is contained in:
@ -1,107 +1,97 @@
|
||||
/*
|
||||
* MathOperations.h
|
||||
*
|
||||
* Created on: 3 Mar 2022
|
||||
* Author: rooob
|
||||
*/
|
||||
|
||||
#ifndef MATH_MATHOPERATIONS_H_
|
||||
#define MATH_MATHOPERATIONS_H_
|
||||
|
||||
#include <math.h>
|
||||
#include <sys/time.h>
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
#include <fsfw/src/fsfw/globalfunctions/constants.h>
|
||||
#include <fsfw/src/fsfw/globalfunctions/math/MatrixOperations.h>
|
||||
#include <math.h>
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
#include <sys/time.h>
|
||||
|
||||
#include <iostream>
|
||||
|
||||
using namespace Math;
|
||||
|
||||
template<typename T1, typename T2 = T1>
|
||||
template <typename T1, typename T2 = T1>
|
||||
class MathOperations {
|
||||
public:
|
||||
static void skewMatrix(const T1 vector[], T2 *result) {
|
||||
// Input Dimension [3], Output [3][3]
|
||||
result[0] = 0;
|
||||
result[1] = -vector[2];
|
||||
result[2] = vector[1];
|
||||
result[3] = vector[2];
|
||||
result[4] = 0;
|
||||
result[5] = -vector[0];
|
||||
result[6] = -vector[1];
|
||||
result[7] = vector[0];
|
||||
result[8] = 0;
|
||||
}
|
||||
static void vecTransposeVecMatrix(const T1 vector1[], const T1 transposeVector2[],
|
||||
T2 *result, uint8_t size = 3) {
|
||||
// Looks like MatrixOpertions::multiply is able to do the same thing
|
||||
for (uint8_t resultColumn = 0; resultColumn < size; resultColumn++) {
|
||||
for (uint8_t resultRow = 0; resultRow < size; resultRow++) {
|
||||
result[resultColumn + size * resultRow] = vector1[resultRow]
|
||||
* transposeVector2[resultColumn];
|
||||
public:
|
||||
static void skewMatrix(const T1 vector[], T2 *result) {
|
||||
// Input Dimension [3], Output [3][3]
|
||||
result[0] = 0;
|
||||
result[1] = -vector[2];
|
||||
result[2] = vector[1];
|
||||
result[3] = vector[2];
|
||||
result[4] = 0;
|
||||
result[5] = -vector[0];
|
||||
result[6] = -vector[1];
|
||||
result[7] = vector[0];
|
||||
result[8] = 0;
|
||||
}
|
||||
static void vecTransposeVecMatrix(const T1 vector1[], const T1 transposeVector2[], T2 *result,
|
||||
uint8_t size = 3) {
|
||||
// Looks like MatrixOpertions::multiply is able to do the same thing
|
||||
for (uint8_t resultColumn = 0; resultColumn < size; resultColumn++) {
|
||||
for (uint8_t resultRow = 0; resultRow < size; resultRow++) {
|
||||
result[resultColumn + size * resultRow] =
|
||||
vector1[resultRow] * transposeVector2[resultColumn];
|
||||
}
|
||||
}
|
||||
/*matrixSun[i][j] = sunEstB[i] * sunEstB[j];
|
||||
matrixMag[i][j] = magEstB[i] * magEstB[j];
|
||||
matrixSunMag[i][j] = sunEstB[i] * magEstB[j];
|
||||
matrixMagSun[i][j] = magEstB[i] * sunEstB[j];*/
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
/*matrixSun[i][j] = sunEstB[i] * sunEstB[j];
|
||||
matrixMag[i][j] = magEstB[i] * magEstB[j];
|
||||
matrixSunMag[i][j] = sunEstB[i] * magEstB[j];
|
||||
matrixMagSun[i][j] = magEstB[i] * sunEstB[j];*/
|
||||
}
|
||||
|
||||
static void selectionSort(const T1 *matrix, T1 *result, uint8_t rowSize,
|
||||
uint8_t colSize) {
|
||||
int min_idx;
|
||||
T1 temp;
|
||||
memcpy(result, matrix, rowSize * colSize * sizeof(*result));
|
||||
// One by one move boundary of unsorted subarray
|
||||
for (int k = 0; k < rowSize; k++) {
|
||||
for (int i = 0; i < colSize - 1; i++) {
|
||||
// Find the minimum element in unsorted array
|
||||
min_idx = i;
|
||||
for (int j = i + 1; j < colSize; j++) {
|
||||
if (result[j + k * colSize]
|
||||
< result[min_idx + k * colSize]) {
|
||||
min_idx = j;
|
||||
}
|
||||
}
|
||||
// Swap the found minimum element with the first element
|
||||
temp = result[i + k * colSize];
|
||||
result[i + k * colSize] = result[min_idx + k * colSize];
|
||||
result[min_idx + k * colSize] = temp;
|
||||
}
|
||||
}
|
||||
}
|
||||
static void selectionSort(const T1 *matrix, T1 *result, uint8_t rowSize, uint8_t colSize) {
|
||||
int min_idx;
|
||||
T1 temp;
|
||||
memcpy(result, matrix, rowSize * colSize * sizeof(*result));
|
||||
// One by one move boundary of unsorted subarray
|
||||
for (int k = 0; k < rowSize; k++) {
|
||||
for (int i = 0; i < colSize - 1; i++) {
|
||||
// Find the minimum element in unsorted array
|
||||
min_idx = i;
|
||||
for (int j = i + 1; j < colSize; j++) {
|
||||
if (result[j + k * colSize] < result[min_idx + k * colSize]) {
|
||||
min_idx = j;
|
||||
}
|
||||
}
|
||||
// Swap the found minimum element with the first element
|
||||
temp = result[i + k * colSize];
|
||||
result[i + k * colSize] = result[min_idx + k * colSize];
|
||||
result[min_idx + k * colSize] = temp;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void convertDateToJD2000(const T1 time, T2 julianDate){
|
||||
|
||||
// time = { Y, M, D, h, m,s}
|
||||
// time in sec and microsec -> The Epoch (unixtime)
|
||||
julianDate = 1721013.5 + 367*time[0]- floor(7/4*(time[0]+(time[1]+9)/12))
|
||||
+floor(275*time[1]/9)+time[2]+(60*time[3]+time[4]+(time(5)/60))/1440;
|
||||
}
|
||||
|
||||
static T1 convertUnixToJD2000(timeval time){
|
||||
//time = {{s},{us}}
|
||||
T1 julianDate2000;
|
||||
julianDate2000 = (time.tv_sec/86400.0)+2440587.5-2451545;
|
||||
return julianDate2000;
|
||||
}
|
||||
static T1 convertUnixToJD2000(timeval time) {
|
||||
// time = {{s},{us}}
|
||||
T1 julianDate2000;
|
||||
julianDate2000 = (time.tv_sec / 86400.0) + 2440587.5 - 2451545;
|
||||
return julianDate2000;
|
||||
}
|
||||
|
||||
static void dcmFromQuat(const T1 vector[], T1 *outputDcm){
|
||||
// convention q = [qx,qy,qz, qw]
|
||||
outputDcm[0] = pow(vector[0],2) - pow(vector[1],2) - pow(vector[2],2) + pow(vector[3],2);
|
||||
outputDcm[1] = 2*(vector[0]*vector[1] + vector[2]*vector[3]);
|
||||
outputDcm[2] = 2*(vector[0]*vector[2] - vector[1]*vector[3]);
|
||||
static void dcmFromQuat(const T1 vector[], T1 *outputDcm) {
|
||||
// convention q = [qx,qy,qz, qw]
|
||||
outputDcm[0] = pow(vector[0], 2) - pow(vector[1], 2) - pow(vector[2], 2) + pow(vector[3], 2);
|
||||
outputDcm[1] = 2 * (vector[0] * vector[1] + vector[2] * vector[3]);
|
||||
outputDcm[2] = 2 * (vector[0] * vector[2] - vector[1] * vector[3]);
|
||||
|
||||
outputDcm[3] = 2*(vector[1]*vector[0] - vector[2]*vector[3]);
|
||||
outputDcm[4] = -pow(vector[0],2) + pow(vector[1],2) - pow(vector[2],2) + pow(vector[3],2);
|
||||
outputDcm[5] = 2*(vector[1]*vector[2] + vector[0]*vector[3]);
|
||||
outputDcm[3] = 2 * (vector[1] * vector[0] - vector[2] * vector[3]);
|
||||
outputDcm[4] = -pow(vector[0], 2) + pow(vector[1], 2) - pow(vector[2], 2) + pow(vector[3], 2);
|
||||
outputDcm[5] = 2 * (vector[1] * vector[2] + vector[0] * vector[3]);
|
||||
|
||||
outputDcm[6] = 2*(vector[2]*vector[0] + vector[1]*vector[3]);
|
||||
outputDcm[7] = 2*(vector[2]*vector[1] - vector[0]*vector[3]);
|
||||
outputDcm[8] = -pow(vector[0],2) - pow(vector[1],2) + pow(vector[2],2) + pow(vector[3],2);
|
||||
|
||||
}
|
||||
outputDcm[6] = 2 * (vector[2] * vector[0] + vector[1] * vector[3]);
|
||||
outputDcm[7] = 2 * (vector[2] * vector[1] - vector[0] * vector[3]);
|
||||
outputDcm[8] = -pow(vector[0], 2) - pow(vector[1], 2) + pow(vector[2], 2) + pow(vector[3], 2);
|
||||
}
|
||||
|
||||
static void cartesianFromLatLongAlt(const T1 lat, const T1 longi, const T1 alt, T2 *cartesianOutput){
|
||||
/* @brief: cartesianFromLatLongAlt() - calculates cartesian coordinates in ECEF from latitude,
|
||||
@ -123,7 +113,6 @@ public:
|
||||
cartesianOutput[2] = ((1 - pow(eccentricity,2)) * auxRadius + alt) * sin(lat);
|
||||
|
||||
}
|
||||
|
||||
static void dcmEJ(timeval time, T1 * outputDcmEJ, T1 * outputDotDcmEJ){
|
||||
/* @brief: dcmEJ() - calculates the transformation matrix between ECEF and ECI frame
|
||||
* @param: time Current time
|
||||
@ -142,26 +131,26 @@ public:
|
||||
JD2000Floor += 0.5;
|
||||
}
|
||||
|
||||
double JC2000 = JD2000Floor / 36525;
|
||||
double sec = (JD2000 - JD2000Floor) * 86400;
|
||||
double gmst = 0; //greenwich mean sidereal time
|
||||
gmst = 24110.54841 + 8640184.812866 * JC2000 + 0.093104 * pow(JC2000,2) -
|
||||
0.0000062 * pow(JC2000,3) + 1.002737909350795 * sec;
|
||||
double rest = gmst / 86400;
|
||||
double FloorRest = floor(rest);
|
||||
double secOfDay = rest-FloorRest;
|
||||
secOfDay *= 86400;
|
||||
gmst = secOfDay / 240 * PI / 180;
|
||||
double JC2000 = JD2000Floor / 36525;
|
||||
double sec = (JD2000 - JD2000Floor) * 86400;
|
||||
double gmst = 0; // greenwich mean sidereal time
|
||||
gmst = 24110.54841 + 8640184.812866 * JC2000 + 0.093104 * pow(JC2000, 2) -
|
||||
0.0000062 * pow(JC2000, 3) + 1.002737909350795 * sec;
|
||||
double rest = gmst / 86400;
|
||||
double FloorRest = floor(rest);
|
||||
double secOfDay = rest - FloorRest;
|
||||
secOfDay *= 86400;
|
||||
gmst = secOfDay / 240 * PI / 180;
|
||||
|
||||
outputDcmEJ[0] = cos(gmst);
|
||||
outputDcmEJ[1] = sin(gmst);
|
||||
outputDcmEJ[2] = 0;
|
||||
outputDcmEJ[3] = -sin(gmst);
|
||||
outputDcmEJ[4] = cos(gmst);
|
||||
outputDcmEJ[5] = 0;
|
||||
outputDcmEJ[6] = 0;
|
||||
outputDcmEJ[7] = 0;
|
||||
outputDcmEJ[8] = 1;
|
||||
outputDcmEJ[0] = cos(gmst);
|
||||
outputDcmEJ[1] = sin(gmst);
|
||||
outputDcmEJ[2] = 0;
|
||||
outputDcmEJ[3] = -sin(gmst);
|
||||
outputDcmEJ[4] = cos(gmst);
|
||||
outputDcmEJ[5] = 0;
|
||||
outputDcmEJ[6] = 0;
|
||||
outputDcmEJ[7] = 0;
|
||||
outputDcmEJ[8] = 1;
|
||||
|
||||
// Derivative of dmcEJ WITHOUT PRECISSION AND NUTATION
|
||||
double dcmEJCalc[3][3] = {{outputDcmEJ[0], outputDcmEJ[1], outputDcmEJ[2]},
|
||||
@ -172,8 +161,8 @@ public:
|
||||
double dotDcmEJ[3][3] = {{0,0,0},{0,0,0},{0,0,0}};
|
||||
MatrixOperations<double>::multiply(*dcmDot, *dcmEJCalc, *dotDcmEJ, 3, 3, 3);
|
||||
MatrixOperations<double>::multiplyScalar(*dotDcmEJ, omegaEarth, outputDotDcmEJ, 3, 3);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/* @brief: ecfToEciWithNutPre() - calculates the transformation matrix between ECEF and ECI frame
|
||||
* give also the back the derivative of this matrix
|
||||
@ -259,7 +248,7 @@ public:
|
||||
double de = 9.203 * arcsecFactor *cos(Om);
|
||||
|
||||
// % true obliquity of the ecliptic eps p.71 (simplified)
|
||||
double e = 23.43929111 * PI / 180 - 46.8150 / 3600 * JC2000TT * PI / 180;;
|
||||
double e = 23.43929111 * PI / 180 - 46.8150 / 3600 * JC2000TT * PI / 180;
|
||||
|
||||
nutation[0][0]=cos(dp);
|
||||
nutation[1][0]=cos(e+de)*sin(dp);
|
||||
@ -290,9 +279,7 @@ public:
|
||||
MatrixOperations<double>::multiply(*nutationPrecession, *thetaDot, outputDotDcmEJ, 3, 3, 3);
|
||||
|
||||
}
|
||||
|
||||
static void inverseMatrixDimThree(const T1 *matrix, T1 * output){
|
||||
|
||||
int i,j;
|
||||
double determinant;
|
||||
double mat[3][3] = {{matrix[0], matrix[1], matrix[2]},{matrix[3], matrix[4], matrix[5]},
|
||||
@ -310,6 +297,113 @@ public:
|
||||
}
|
||||
}
|
||||
|
||||
static float matrixDeterminant(const T1 *inputMatrix, uint8_t size) {
|
||||
float det = 0;
|
||||
T1 matrix[size][size], submatrix[size - 1][size - 1];
|
||||
for (uint8_t row = 0; row < size; row++) {
|
||||
for (uint8_t col = 0; col < size; col++) {
|
||||
matrix[row][col] = inputMatrix[row * size + col];
|
||||
}
|
||||
}
|
||||
if (size == 2)
|
||||
return ((matrix[0][0] * matrix[1][1]) - (matrix[1][0] * matrix[0][1]));
|
||||
else {
|
||||
for (uint8_t col = 0; col < size; col++) {
|
||||
int subRow = 0;
|
||||
for (uint8_t rowIndex = 1; rowIndex < size; rowIndex++) {
|
||||
int subCol = 0;
|
||||
for (uint8_t colIndex = 0; colIndex < size; colIndex++) {
|
||||
if (colIndex == col) continue;
|
||||
submatrix[subRow][subCol] = matrix[rowIndex][colIndex];
|
||||
subCol++;
|
||||
}
|
||||
subRow++;
|
||||
}
|
||||
det += (pow(-1, col) * matrix[0][col] *
|
||||
MathOperations<T1>::matrixDeterminant(*submatrix, size - 1));
|
||||
}
|
||||
}
|
||||
return det;
|
||||
}
|
||||
|
||||
static int inverseMatrix(const T1 *inputMatrix, T1 *inverse, uint8_t size) {
|
||||
if (MathOperations<T1>::matrixDeterminant(inputMatrix, size) == 0) {
|
||||
return 1; // Matrix is singular and not invertible
|
||||
}
|
||||
T1 matrix[size][size], identity[size][size];
|
||||
// reformat array to matrix
|
||||
for (uint8_t row = 0; row < size; row++) {
|
||||
for (uint8_t col = 0; col < size; col++) {
|
||||
matrix[row][col] = inputMatrix[row * size + col];
|
||||
}
|
||||
}
|
||||
// init identity matrix
|
||||
std::memset(identity, 0.0, sizeof(identity));
|
||||
for (uint8_t diag = 0; diag < size; diag++) {
|
||||
identity[diag][diag] = 1;
|
||||
}
|
||||
// gauss-jordan algo
|
||||
// sort matrix such as no diag entry shall be 0
|
||||
// should not be needed as such a matrix has a det=0
|
||||
for (uint8_t row = 0; row < size; row++) {
|
||||
if (matrix[row][row] == 0.0) {
|
||||
bool swaped = false;
|
||||
uint8_t rowIndex = 0;
|
||||
while ((rowIndex < size) && !swaped) {
|
||||
if ((matrix[rowIndex][row] != 0.0) && (matrix[row][rowIndex] != 0.0)) {
|
||||
for (uint8_t colIndex = 0; colIndex < size; colIndex++) {
|
||||
std::swap(matrix[row][colIndex], matrix[rowIndex][colIndex]);
|
||||
std::swap(identity[row][colIndex], identity[rowIndex][colIndex]);
|
||||
}
|
||||
swaped = true;
|
||||
}
|
||||
rowIndex++;
|
||||
}
|
||||
if (!swaped) {
|
||||
return 1; // matrix not invertible
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (int row = 0; row < size; row++) {
|
||||
if (matrix[row][row] == 0.0) {
|
||||
uint8_t rowIndex;
|
||||
if (row == 0) {
|
||||
rowIndex = size - 1;
|
||||
} else {
|
||||
rowIndex = row - 1;
|
||||
}
|
||||
for (uint8_t colIndex = 0; colIndex < size; colIndex++) {
|
||||
std::swap(matrix[row][colIndex], matrix[rowIndex][colIndex]);
|
||||
std::swap(identity[row][colIndex], identity[rowIndex][colIndex]);
|
||||
}
|
||||
row--;
|
||||
if (row < 0) {
|
||||
return 1; // Matrix is not invertible
|
||||
}
|
||||
}
|
||||
}
|
||||
// remove non diag elements in matrix (jordan)
|
||||
for (int row = 0; row < size; row++) {
|
||||
for (int rowIndex = 0; rowIndex < size; rowIndex++) {
|
||||
if (row != rowIndex) {
|
||||
double ratio = matrix[rowIndex][row] / matrix[row][row];
|
||||
for (int colIndex = 0; colIndex < size; colIndex++) {
|
||||
matrix[rowIndex][colIndex] -= ratio * matrix[row][colIndex];
|
||||
identity[rowIndex][colIndex] -= ratio * identity[row][colIndex];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
// normalize rows in matrix (gauss)
|
||||
for (int row = 0; row < size; row++) {
|
||||
for (int col = 0; col < size; col++) {
|
||||
identity[row][col] = identity[row][col] / matrix[row][row];
|
||||
}
|
||||
}
|
||||
std::memcpy(inverse, identity, sizeof(identity));
|
||||
return 0; // successful inversion
|
||||
}
|
||||
};
|
||||
|
||||
#endif /* ACS_MATH_MATHOPERATIONS_H_ */
|
||||
|
||||
Reference in New Issue
Block a user