diff --git a/libraries/AP_Math/LieGroups.cpp b/libraries/AP_Math/LieGroups.cpp
index 37226aa4d5711c..7e5d5f1e8b23e8 100644
--- a/libraries/AP_Math/LieGroups.cpp
+++ b/libraries/AP_Math/LieGroups.cpp
@@ -44,6 +44,14 @@ GL2 GL2::operator+(const GL2& rhs) const {
         _a22 + rhs._a22
     );
 }
+GL2 GL2::operator-(const GL2& rhs) const {
+    return GL2(
+        _a11 - rhs._a11,
+        _a12 - rhs._a12,
+        _a21 - rhs._a21,
+        _a22 - rhs._a22
+    );
+}
 
 Vector2F GL2::operator*(const Vector2F& rhs) const {
     ftype v1 = _a11*rhs.x + _a12*rhs.y;
diff --git a/libraries/AP_Math/LieGroups.h b/libraries/AP_Math/LieGroups.h
index 42fefa1dd679b9..083f8fb1cbb4eb 100644
--- a/libraries/AP_Math/LieGroups.h
+++ b/libraries/AP_Math/LieGroups.h
@@ -16,6 +16,7 @@ class GL2 {
     }
     GL2 operator*(const GL2& rhs) const;
     GL2 operator+(const GL2& rhs) const;
+    GL2 operator-(const GL2& rhs) const;
     Vector2F operator*(const Vector2F& rhs) const;
     GL2 inverse() const;
     GL2 transposed() const;