diff --git a/src/drivers/uavcannode/Publishers/GnssFix2.hpp b/src/drivers/uavcannode/Publishers/GnssFix2.hpp index 4f3223ec48..4f0968c269 100644 --- a/src/drivers/uavcannode/Publishers/GnssFix2.hpp +++ b/src/drivers/uavcannode/Publishers/GnssFix2.hpp @@ -113,10 +113,10 @@ public: } // Diagonal matrix - // position variances -- Xx, Yy, Zz - fix2.covariance.push_back(gps.eph); - fix2.covariance.push_back(gps.eph); - fix2.covariance.push_back(gps.epv); + // position variances -- Xx, Yy, Zz (eph/epv are std dev in meters, must square for variance) + fix2.covariance.push_back(gps.eph * gps.eph); + fix2.covariance.push_back(gps.eph * gps.eph); + fix2.covariance.push_back(gps.epv * gps.epv); // velocity variance -- Vxx, Vyy, Vzz fix2.covariance.push_back(gps.s_variance_m_s); fix2.covariance.push_back(gps.s_variance_m_s);