diff --git a/src/shared/algorithms/NAS/NAS.cpp b/src/shared/algorithms/NAS/NAS.cpp
index 69e1fa266f285b199ef7a0355b1132fafbaa88e7..5f745b7951ff63c6383fb20ce8268d97f04f2455 100644
--- a/src/shared/algorithms/NAS/NAS.cpp
+++ b/src/shared/algorithms/NAS/NAS.cpp
@@ -38,18 +38,18 @@ NAS::NAS(NASConfig config) : config(config)
// Covariance setup
{
// clang-format off
- Eigen::Matrix3f P_pos{
+ Matrix3f P_pos{
{config.P_POS, 0, 0},
{0, config.P_POS, 0},
{0, 0, config.P_POS_VERTICAL}
};
- Eigen::Matrix3f P_vel{
+ Matrix3f P_vel{
{config.P_VEL, 0, 0},
{0, config.P_VEL, 0},
{0, 0, config.P_VEL_VERTICAL}
};
- Eigen::Matrix3f P_att = Matrix3f::Identity() * config.P_ATT;
- Eigen::Matrix3f P_bias = Matrix3f::Identity() * config.P_BIAS;
+ Matrix3f P_att = Matrix3f::Identity() * config.P_ATT;
+ Matrix3f P_bias = Matrix3f::Identity() * config.P_BIAS;
P << P_pos, MatrixXf::Zero(3, 9),
MatrixXf::Zero(3, 3), P_vel, MatrixXf::Zero(3, 6),
MatrixXf::Zero(3, 6), P_att, MatrixXf::Zero(3, 3),
@@ -117,8 +117,8 @@ void NAS::predictAcc(const Vector3f& acceleration)
x.block<3, 1>(IDX_VEL, 0) = vel;
// Variance propagation
- Eigen::Matrix<float, 6, 6> Pl = P.block<6, 6>(0, 0);
- P.block<6, 6>(0, 0) = F_lin * Pl * F_lin_tr + Q_lin;
+ Matrix<float, 6, 6> Pl = P.block<6, 6>(0, 0);
+ P.block<6, 6>(0, 0) = F_lin * Pl * F_lin_tr + Q_lin;
}
void NAS::predictAcc(const AccelerometerData& acceleration)
@@ -175,7 +175,7 @@ void NAS::correctBaro(const float pressure)
H[2] = Constants::a * Constants::n * reference.refPressure *
powf(1 - Constants::a * x(2) / temp, -Constants::n - 1) / temp;
- Eigen::Matrix<float, 6, 6> Pl = P.block<6, 6>(0, 0);
+ Matrix<float, 6, 6> Pl = P.block<6, 6>(0, 0);
Matrix<float, 1, 1> S =
H * Pl * H.transpose() + Matrix<float, 1, 1>(config.SIGMA_BAR);
Matrix<float, 6, 1> K = Pl * H.transpose() * S.inverse();
@@ -191,13 +191,12 @@ void NAS::correctBaro(const float pressure)
void NAS::correctGPS(const Vector4f& gps)
{
- Eigen::Matrix<float, 6, 6> Pl = P.block<6, 6>(0, 0);
+ Matrix<float, 6, 6> Pl = P.block<6, 6>(0, 0);
Matrix<float, 4, 4> S = H_gps * Pl * H_gps_tr + R_gps;
Matrix<float, 6, 4> K = Pl * H_gps_tr * S.inverse();
- P.block<6, 6>(0, 0) =
- (Eigen::Matrix<float, 6, 6>::Identity() - K * H_gps) * Pl;
+ P.block<6, 6>(0, 0) = (Matrix<float, 6, 6>::Identity() - K * H_gps) * Pl;
// Current state [n e vn ve]
Matrix<float, 4, 1> H{x(0), x(1), x(3), x(4)};
@@ -259,7 +258,7 @@ void NAS::correctMag(const MagnetometerData& mag)
correctMag(magV.normalized());
}
-void NAS::correctAcc(const Eigen::Vector3f& acc)
+void NAS::correctAcc(const Vector3f& acc)
{
Vector4f q = x.block<4, 1>(IDX_QUAT, 0);
Matrix3f A = SkyQuaternion::quat2rotationMatrix(q).transpose();
@@ -328,7 +327,7 @@ void NAS::correctPitot(const float deltaP, const float staticP)
Matrix<float, 1, 6> H = Matrix<float, 1, 6>::Zero();
H.coeffRef(0, 5) = 1;
- Eigen::Matrix<float, 6, 6> Pl = P.block<6, 6>(0, 0);
+ Matrix<float, 6, 6> Pl = P.block<6, 6>(0, 0);
Matrix<float, 1, 1> S =
H * Pl * H.transpose() + Matrix<float, 1, 1>(config.SIGMA_PITOT);
@@ -343,11 +342,11 @@ NASState NAS::getState() const
return NASState(TimestampTimer::getTimestamp(), x);
}
-Eigen::Matrix<float, 13, 1> NAS::getX() const { return x; }
+Matrix<float, 13, 1> NAS::getX() const { return x; }
void NAS::setState(const NASState& state) { this->x = state.getX(); }
-void NAS::setX(const Eigen::Matrix<float, 13, 1>& x) { this->x = x; }
+void NAS::setX(const Matrix<float, 13, 1>& x) { this->x = x; }
void NAS::setReferenceValues(const ReferenceValues reference)
{
diff --git a/src/shared/algorithms/NAS/StateInitializer.cpp b/src/shared/algorithms/NAS/StateInitializer.cpp
index cfd6c08e6fa5902ea96c668adbdf15121674f5f7..528f5be25f36d2126d7474822b907a9eba8deb9a 100644
--- a/src/shared/algorithms/NAS/StateInitializer.cpp
+++ b/src/shared/algorithms/NAS/StateInitializer.cpp
@@ -22,27 +22,29 @@
#include "StateInitializer.h"
+using namespace Eigen;
+
namespace Boardcore
{
-StateInitializer::StateInitializer() { x_init << Eigen::MatrixXf::Zero(13, 1); }
-void StateInitializer::eCompass(const Eigen::Vector3f acc,
- const Eigen::Vector3f mag)
+StateInitializer::StateInitializer() { x_init << MatrixXf::Zero(13, 1); }
+
+void StateInitializer::eCompass(const Vector3f acc, const Vector3f mag)
{
// ndr: since this method runs only when the rocket is stationary, there's
// no need to add the gravity vector because the accelerometers already
// measure it. This is not true if we consider the flying rocket.
- Eigen::Vector3f am(acc.cross(mag));
+ Vector3f am(acc.cross(mag));
- Eigen::Matrix3f R;
+ Matrix3f R;
// cppcheck-suppress constStatement
R << am.cross(acc), am, acc;
R.col(0).normalize();
R.col(1).normalize();
R.col(2).normalize();
- Eigen::Vector4f x_quat = SkyQuaternion::rotationMatrix2quat(R);
+ Vector4f x_quat = SkyQuaternion::rotationMatrix2quat(R);
x_init(NAS::IDX_QUAT) = x_quat(0);
x_init(NAS::IDX_QUAT + 1) = x_quat(1);
@@ -50,40 +52,39 @@ void StateInitializer::eCompass(const Eigen::Vector3f acc,
x_init(NAS::IDX_QUAT + 3) = x_quat(3);
}
-void StateInitializer::triad(const Eigen::Vector3f& acc,
- const Eigen::Vector3f& mag,
- const Eigen::Vector3f& nedMag)
+void StateInitializer::triad(const Vector3f& acc, const Vector3f& mag,
+ const Vector3f& nedMag)
{
// The gravity vector is expected to be read inversely because
// accelerometers read the binding reaction
- Eigen::Vector3f nedAcc(0.0f, 0.0f, -1.0f);
+ Vector3f nedAcc(0.0f, 0.0f, -1.0f);
// Compute the reference triad
- Eigen::Vector3f R1 = nedAcc;
- Eigen::Vector3f R2 = nedAcc.cross(nedMag).normalized();
- Eigen::Vector3f R3 = R1.cross(R2);
+ Vector3f R1 = nedAcc;
+ Vector3f R2 = nedAcc.cross(nedMag).normalized();
+ Vector3f R3 = R1.cross(R2);
// Compute the measured triad
- Eigen::Vector3f r1 = acc;
- Eigen::Vector3f r2 = acc.cross(mag).normalized();
- Eigen::Vector3f r3 = r1.cross(r2);
+ Vector3f r1 = acc;
+ Vector3f r2 = acc.cross(mag).normalized();
+ Vector3f r3 = r1.cross(r2);
// Compose the reference and measured matrixes
- Eigen::Matrix3f M;
+ Matrix3f M;
// cppcheck-suppress constStatement
M << R1, R2, R3;
- Eigen::Matrix3f m;
+ Matrix3f m;
// cppcheck-suppress constStatement
m << r1, r2, r3;
// Compute the rotation matrix and the corresponding quaternion
- Eigen::Matrix3f A = M * m.transpose();
- Eigen::Vector4f q = SkyQuaternion::rotationMatrix2quat(A);
+ Matrix3f A = M * m.transpose();
+ Vector4f q = SkyQuaternion::rotationMatrix2quat(A);
// Save the orientation in the state
x_init.block<4, 1>(NAS::IDX_QUAT, 0) = q;
}
-Eigen::Matrix<float, 13, 1> StateInitializer::getInitX() { return x_init; }
+Matrix<float, 13, 1> StateInitializer::getInitX() { return x_init; }
} // namespace Boardcore