diff --git a/geodetic_utils/include/geodetic_utils/geodetic_conv.hpp b/geodetic_utils/include/geodetic_utils/geodetic_conv.hpp index 60a5671..b7a8077 100644 --- a/geodetic_utils/include/geodetic_utils/geodetic_conv.hpp +++ b/geodetic_utils/include/geodetic_utils/geodetic_conv.hpp @@ -52,7 +52,8 @@ class GeodeticConverter // Compute ECEF to NED and NED to ECEF matrices double phiP = atan2(initial_ecef_z_, sqrt(pow(initial_ecef_x_, 2) + pow(initial_ecef_y_, 2))); - ecef_to_ned_matrix_ = nRe(phiP, initial_longitude_); + // ecef_to_ned_matrix_ = nRe(phiP, initial_longitude_); + ecef_to_ned_matrix_ = nRe(initial_latitude_, initial_longitude_); ned_to_ecef_matrix_ = nRe(initial_latitude_, initial_longitude_).transpose(); haveReference_ = true; @@ -226,4 +227,4 @@ class GeodeticConverter }; // class GeodeticConverter }; // namespace geodetic_conv -#endif // GEODETIC_CONVERTER_H_ +#endif // GEODETIC_CONVERTER_H_ \ No newline at end of file