diff --git a/libraries/AP_NavEKF/EKFGSF_yaw.cpp b/libraries/AP_NavEKF/EKFGSF_yaw.cpp
index 5889ab20ca..01c9227021 100644
--- a/libraries/AP_NavEKF/EKFGSF_yaw.cpp
+++ b/libraries/AP_NavEKF/EKFGSF_yaw.cpp
@@ -577,7 +577,7 @@ float EKFGSF_yaw::gaussianDensity(const uint8_t mdl_idx) const
     return normDist;
 }
 
-bool EKFGSF_yaw::getLogData(float &yaw_composite, float &yaw_composite_variance, float yaw[N_MODELS_EKFGSF], float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF])
+bool EKFGSF_yaw::getLogData(float &yaw_composite, float &yaw_composite_variance, float yaw[N_MODELS_EKFGSF], float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]) const
 {
     if (vel_fuse_running) {
         yaw_composite = GSF.yaw;
@@ -604,7 +604,7 @@ void EKFGSF_yaw::forceSymmetry(const uint8_t mdl_idx)
 }
 
 // Apply a body frame delta angle to the body to earth frame rotation matrix using a small angle approximation
-Matrix3f EKFGSF_yaw::updateRotMat(const Matrix3f &R, const Vector3f &g)
+Matrix3f EKFGSF_yaw::updateRotMat(const Matrix3f &R, const Vector3f &g) const
 {
     Matrix3f ret = R;
     ret[0][0] += R[0][1] * g[2] - R[0][2] * g[1];
@@ -632,7 +632,7 @@ Matrix3f EKFGSF_yaw::updateRotMat(const Matrix3f &R, const Vector3f &g)
     return ret;
 }
 
-bool EKFGSF_yaw::getYawData(float &yaw, float &yawVariance)
+bool EKFGSF_yaw::getYawData(float &yaw, float &yawVariance) const
 {
     if (!vel_fuse_running) {
         return false;
@@ -642,7 +642,7 @@ bool EKFGSF_yaw::getYawData(float &yaw, float &yawVariance)
     return true;
 }
 
-bool EKFGSF_yaw::getVelInnovLength(float &velInnovLength)
+bool EKFGSF_yaw::getVelInnovLength(float &velInnovLength) const
 {
     if (!vel_fuse_running) {
         return false;
@@ -659,4 +659,4 @@ void EKFGSF_yaw::setGyroBias(Vector3f &gyroBias)
     for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx++) {
         AHRS[mdl_idx].gyro_bias = gyroBias;
     }
-}
\ No newline at end of file
+}
diff --git a/libraries/AP_NavEKF/EKFGSF_yaw.h b/libraries/AP_NavEKF/EKFGSF_yaw.h
index b24ffd7598..2fee668ff5 100644
--- a/libraries/AP_NavEKF/EKFGSF_yaw.h
+++ b/libraries/AP_NavEKF/EKFGSF_yaw.h
@@ -32,15 +32,15 @@ public:
 
     // get solution data for logging
     // return false if yaw estimation is inactive
-    bool getLogData(float &yaw_composite, float &yaw_composite_variance, float yaw[N_MODELS_EKFGSF], float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]);
+    bool getLogData(float &yaw_composite, float &yaw_composite_variance, float yaw[N_MODELS_EKFGSF], float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]) const;
 
     // get yaw estimated and corresponding variance
     // return false if yaw estimation is inactive
-    bool getYawData(float &yaw, float &yawVariance);
+    bool getYawData(float &yaw, float &yawVariance) const;
 
     // get the length of the weighted average velocity innovation vector
     // return false if not available
-    bool getVelInnovLength(float &velInnovLength);
+    bool getVelInnovLength(float &velInnovLength) const;
 
 private:
 
@@ -90,7 +90,7 @@ private:
     void predictAHRS(const uint8_t mdl_idx);
 
     // Applies a body frame delta angle to a body to earth frame rotation matrix using a small angle approximation
-    Matrix3f updateRotMat(const Matrix3f &R, const Vector3f &g);
+    Matrix3f updateRotMat(const Matrix3f &R, const Vector3f &g) const;
 
     // Initialises the tilt (roll and pitch) for all AHRS using IMU acceleration data
     void alignTilt();