diff --git a/src/shared/algorithms/NAS/NAS.cpp b/src/shared/algorithms/NAS/NAS.cpp
index 099e557b7632b2caabd3e31a2253eaa724fadd16..692a618ff4d0153b1622993706b7140752d30d1b 100644
--- a/src/shared/algorithms/NAS/NAS.cpp
+++ b/src/shared/algorithms/NAS/NAS.cpp
@@ -57,17 +57,6 @@ NAS::NAS(NASConfig config) : config(config)
// clang-format on
}
- // GPS
- {
- H_gps = Matrix<float, 4, 6>::Zero();
- H_gps.coeffRef(0, 0) = 1;
- H_gps.coeffRef(1, 1) = 1;
- H_gps.coeffRef(2, 3) = 1;
- H_gps.coeffRef(3, 4) = 1;
- H_gps_tr = H_gps.transpose();
- R_gps << config.SIGMA_GPS * Matrix<float, 4, 4>::Identity();
- }
-
// Utility matrixes
R_acc << config.SIGMA_ACC * Matrix3f::Identity();
R_mag << config.SIGMA_MAG * Matrix3f::Identity();
@@ -190,31 +179,70 @@ void NAS::correctBaro(const float pressure)
void NAS::correctGPS(const Vector4f& gps)
{
+ // GPS
+ H_gps = Matrix<float, 4, 6>::Zero();
+
+ //
+ float angle_rad = (reference.refLatitude + x(0) / Constants::gpsLatConst) *
+ Constants::DEGREES_TO_RADIANS;
+
+ float H11 = 1 / Constants::gpsLatConst;
+ float H12 = 0;
+ float H21 = (x(1) * sin(angle_rad)) /
+ (Constants::gpsLatConst * Constants::gpsLonConst *
+ pow(cos(angle_rad), 2));
+ float H22 = 1 / (Constants::gpsLonConst * cos(angle_rad));
+
+ // Convert to millideg
+ H_gps.coeffRef(0, 0) = H11 * 1000;
+ H_gps.coeffRef(0, 1) = H12 * 1000;
+ H_gps.coeffRef(1, 0) = H21 * 1000;
+ H_gps.coeffRef(1, 1) = H22 * 1000;
+
+ H_gps.coeffRef(2, 3) = 1;
+ H_gps.coeffRef(3, 4) = 1;
+
+ H_gps_tr = H_gps.transpose();
+
+ Matrix<float, 1, 4> R_molt = {1, 1, std::max(std::abs(gps(2)), 30.0f),
+ std::max(std::abs(gps(3)), 30.0f)};
+ Matrix<float, 4, 4> R_gps = config.SIGMA_GPS.asDiagonal().toDenseMatrix() *
+ R_molt.asDiagonal().toDenseMatrix();
+
+ // Current state [n e vn ve]
+ float lat = x(0) / Constants::gpsLatConst + reference.refLatitude;
+ float lon = x(1) / (Constants::gpsLonConst *
+ cos(lat * Constants::DEGREES_TO_RADIANS)) +
+ reference.refLongitude;
+ Matrix<float, 4, 1> z{lat, lon, x(3), x(4)};
+ Vector4f e{(gps(0) - z(0)) * 1000.0f, (gps(1) - z(1)) * 1000.0f,
+ gps(2) - z(2), gps(3) - z(3)};
+
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) = (Matrix<float, 6, 6>::Identity() - K * H_gps) * Pl;
+ // If not invertible, don't do the correction and return
+ if (S.determinant() < 1e-3)
+ {
+ return;
+ }
- // Current state [n e vn ve]
- Matrix<float, 4, 1> H{x(0), x(1), x(3), x(4)};
+ Matrix<float, 6, 4> K = Pl * H_gps_tr * S.inverse();
// Update the state
- x.head<6>() = x.head<6>() + K * (gps - H);
+ x.head<6>() = x.head<6>() + K * e;
+
+ P.block<6, 6>(0, 0) = (Matrix<float, 6, 6>::Identity() - K * H_gps) * Pl;
}
void NAS::correctGPS(const GPSData& gps)
{
- if (!gps.fix)
+ if (gps.fix != 3)
return;
- auto gpsPos = Aeroutils::geodetic2NED(
- {gps.latitude, gps.longitude},
- {reference.refLatitude, reference.refLongitude});
-
- correctGPS(
- Vector4f{gpsPos(0), gpsPos(1), gps.velocityNorth, gps.velocityEast});
+ correctGPS(Vector4f{gps.latitude, gps.longitude, gps.velocityNorth,
+ gps.velocityEast});
}
void NAS::correctMag(const Vector3f& mag)
diff --git a/src/shared/algorithms/NAS/NASConfig.h b/src/shared/algorithms/NAS/NASConfig.h
index f82d12112c7cbd22ec027bc56ebb7671e1362bc8..9c3338907fd49529b5a535f0f06213c65e662707 100644
--- a/src/shared/algorithms/NAS/NASConfig.h
+++ b/src/shared/algorithms/NAS/NASConfig.h
@@ -29,13 +29,15 @@ namespace Boardcore
struct NASConfig
{
- float T; ///< [s] Sample period
- float SIGMA_BETA; ///< [rad/s^2] Estimated gyroscope bias variance
- float SIGMA_W; ///< [rad^2] Estimated gyroscope variance
- float SIGMA_ACC; ///< [uT^2] Estimated accelerometer variance
- float SIGMA_MAG; ///< [uT^2] Estimated magnetometer variance
- float SIGMA_GPS; ///< [m^2] Estimated GPS variance
- float SIGMA_BAR; ///< [m^2] Estimated altitude variance
+ float T; ///< [s] Sample period
+ float SIGMA_BETA; ///< [rad/s^2] Estimated gyroscope bias variance
+ float SIGMA_W; ///< [rad^2] Estimated gyroscope variance
+ float SIGMA_ACC; ///< [uT^2] Estimated accelerometer variance
+ float SIGMA_MAG; ///< [uT^2] Estimated magnetometer variance
+ Eigen::Vector4f SIGMA_GPS; ///< [millideg^2, millideg^2, m^2/s^2, m^2/s^2]
+ ///< estimated GPS variance. position from test,
+ ///< velocity from datasheet
+ float SIGMA_BAR; ///< [m^2] Estimated altitude variance
float SIGMA_POS; ///< [m^2] Estimated variance of the position noise
float SIGMA_VEL; ///< [(m/s)^2] Estimated variance of the velocity noise
float SIGMA_PITOT; ///< [Pa^2] Estimated variance of the pitot velocity
diff --git a/src/shared/utils/Constants.h b/src/shared/utils/Constants.h
index 87481cb6a6df7fa7a73cbbdf89a22167fa6d01e3..5caf126047883890839c97eeb829c49aa69767b0 100644
--- a/src/shared/utils/Constants.h
+++ b/src/shared/utils/Constants.h
@@ -49,6 +49,11 @@ static constexpr float Hn = 10400.0; // Scale height [m]
static constexpr float MSL_PRESSURE = 101325.0f; // [Pa]
static constexpr float MSL_TEMPERATURE = 288.15f; // [Kelvin]
+static constexpr float gpsLatConst =
+ 111132.95225; ///< Constants used in GPS correction(lat)
+static constexpr float gpsLonConst =
+ 111412.87733; ///< Constants used in GPS correction(lon)
+
static constexpr float B21_LATITUDE = 45.501141;
static constexpr float B21_LONGITUDE = 9.156281;
} // namespace Constants
diff --git a/src/tests/algorithms/NAS/test-attitude-parafoil.cpp b/src/tests/algorithms/NAS/test-attitude-parafoil.cpp
index 3af6f6222da312a1521abc54ccce120e1e205236..0cc72ffbfbbf908cbff26200b5fbfcc9f5cc8262 100644
--- a/src/tests/algorithms/NAS/test-attitude-parafoil.cpp
+++ b/src/tests/algorithms/NAS/test-attitude-parafoil.cpp
@@ -69,7 +69,7 @@ NASConfig getEKConfig()
config.SIGMA_W = 0.3f;
config.SIGMA_ACC = 0.1f;
config.SIGMA_MAG = 0.1f;
- config.SIGMA_GPS = 10.0f;
+ config.SIGMA_GPS = {0.002f, 0.002f, 0.01f / 30.0f, 0.01f / 30.0f};
config.SIGMA_BAR = 4.3f;
config.SIGMA_POS = 10.0;
config.SIGMA_VEL = 10.0;
diff --git a/src/tests/algorithms/NAS/test-attitude-stack.cpp b/src/tests/algorithms/NAS/test-attitude-stack.cpp
index 00d0145f7737a20a708c2a65fb5d725ce292d214..6326820e7479855a1220021b7aa3ac16591eeb0b 100644
--- a/src/tests/algorithms/NAS/test-attitude-stack.cpp
+++ b/src/tests/algorithms/NAS/test-attitude-stack.cpp
@@ -69,7 +69,7 @@ NASConfig getEKConfig()
config.SIGMA_W = 0.3f;
config.SIGMA_ACC = 0.1f;
config.SIGMA_MAG = 0.1f;
- config.SIGMA_GPS = 10.0f;
+ config.SIGMA_GPS = {0.002f, 0.002f, 0.01f / 30.0f, 0.01f / 30.0f};
config.SIGMA_BAR = 4.3f;
config.SIGMA_POS = 10.0;
config.SIGMA_VEL = 10.0;
diff --git a/src/tests/algorithms/NAS/test-nas-parafoil.cpp b/src/tests/algorithms/NAS/test-nas-parafoil.cpp
index 12d106ca45c05d91eeb3e112d1df31e249ae12fc..e89dc6ce8c0d296a82f10b0ad772fae944944fa3 100644
--- a/src/tests/algorithms/NAS/test-nas-parafoil.cpp
+++ b/src/tests/algorithms/NAS/test-nas-parafoil.cpp
@@ -77,7 +77,7 @@ NASConfig getEKConfig()
config.SIGMA_W = 0.3f;
config.SIGMA_ACC = 0.1f;
config.SIGMA_MAG = 0.1f;
- config.SIGMA_GPS = 10.0f;
+ config.SIGMA_GPS = {0.002f, 0.002f, 0.01f / 30.0f, 0.01f / 30.0f};
config.SIGMA_BAR = 4.3f;
config.SIGMA_POS = 10.0;
config.SIGMA_VEL = 10.0;
diff --git a/src/tests/algorithms/NAS/test-nas-with-triad.cpp b/src/tests/algorithms/NAS/test-nas-with-triad.cpp
index 372a4137bc17282e334bbd50751234f7ef59d82f..3a304dc696557ef1ba2709a53ae778c7fc84baba 100644
--- a/src/tests/algorithms/NAS/test-nas-with-triad.cpp
+++ b/src/tests/algorithms/NAS/test-nas-with-triad.cpp
@@ -74,7 +74,7 @@ NASConfig getEKConfig()
config.SIGMA_W = 0.3f;
config.SIGMA_ACC = 0.1f;
config.SIGMA_MAG = 0.1f;
- config.SIGMA_GPS = 10.0f;
+ config.SIGMA_GPS = {0.002f, 0.002f, 0.01f / 30.0f, 0.01f / 30.0f};
config.SIGMA_BAR = 4.3f;
config.SIGMA_POS = 10.0;
config.SIGMA_VEL = 10.0;
diff --git a/src/tests/algorithms/NAS/test-nas.cpp b/src/tests/algorithms/NAS/test-nas.cpp
index c62f60ffcca4cee4d86c86551254a606174f8321..8cc90303ba04abbcb569ebe983bf3503a0a51e53 100644
--- a/src/tests/algorithms/NAS/test-nas.cpp
+++ b/src/tests/algorithms/NAS/test-nas.cpp
@@ -81,7 +81,7 @@ NASConfig getEKConfig()
config.SIGMA_W = 0.3f;
config.SIGMA_ACC = 0.1f;
config.SIGMA_MAG = 0.1f;
- config.SIGMA_GPS = 10.0f;
+ config.SIGMA_GPS = {0.002f, 0.002f, 0.01f / 30.0f, 0.01f / 30.0f};
config.SIGMA_BAR = 4.3f;
config.SIGMA_POS = 10.0;
config.SIGMA_VEL = 10.0;