diff --git a/matrix/Quaternion.hpp b/matrix/Quaternion.hpp
index a0f0441db3..e1067c6bac 100644
--- a/matrix/Quaternion.hpp
+++ b/matrix/Quaternion.hpp
@@ -98,6 +98,12 @@ public:
         return r;
     }
 
+    void operator*=(const Quaternion & other)
+    {
+        Quaternion &self = *this;
+        self = self * other;
+    }
+
     Matrix41 derivative(const Matrix31 & w) const {
         const Quaternion &q = *this;
         Type dataQ[] = {
diff --git a/matrix/filter.hpp b/matrix/filter.hpp
index d075fc0f01..a9d9982b1a 100644
--- a/matrix/filter.hpp
+++ b/matrix/filter.hpp
@@ -6,12 +6,12 @@ namespace matrix {
 
 template<typename Type, size_t M, size_t N>
 int kalman_correct(
-    const SquareMatrix<Type, M> & P,
+    const Matrix<Type, M, M> & P,
     const Matrix<Type, N, M> & C,
-    const SquareMatrix<Type, N> & R,
-    const Vector<Type, N> &r,
-    Vector<Type, M> & dx,
-    SquareMatrix<Type, M> & dP,
+    const Matrix<Type, N, N> & R,
+    const Matrix<Type, N, 1> &r,
+    Matrix<Type, M, 1> & dx,
+    Matrix<Type, M, M> & dP,
     float & beta
 )
 {
diff --git a/test/filter.cpp b/test/filter.cpp
index 3470e5f4d4..f8eb01f748 100644
--- a/test/filter.cpp
+++ b/test/filter.cpp
@@ -5,16 +5,6 @@
 
 using namespace matrix;
 
-template int kalman_correct<float, 6, 5>(
-    const SquareMatrix<float, 6> & P,
-    const Matrix<float, 5, 6> & C,
-    const SquareMatrix<float, 5> & R,
-    const Vector<float, 5> &r,
-    Vector<float, 6> & dx,
-    SquareMatrix<float, 6> & dP,
-    float & beta
-);
-
 int main()
 {
     const size_t n_x = 6;