diff --git a/matrix/filter.hpp b/matrix/filter.hpp
index e202bb8827..53fc99d92a 100644
--- a/matrix/filter.hpp
+++ b/matrix/filter.hpp
@@ -6,17 +6,20 @@ namespace matrix {
 
 template<typename Type, size_t M, size_t N>
 void kalman_correct(
-    const Matrix<Type, M, M> & P,
+    const SquareMatrix<Type, M> & P,
     const Matrix<Type, N, M> & C,
-    const Matrix<Type, N, N> & R,
+    const SquareMatrix<Type, N> & R,
     const Vector<Type, N> &r,
     Vector<Type, M> & dx,
+    SquareMatrix<Type, M> & dP,
     float & beta
 )
 {
     SquareMatrix<Type, N> S_I = SquareMatrix<Type, N>(C*P*C.T() + R).I();
-    dx = P*C.T()*S_I*r;
+    Matrix<Type, M, N> K = P*C.T()*S_I;
+    dx = K*r;
     beta = Scalar<Type>(r.T()*S_I*r);
+    dP = K*C*P*(-1);
 }
 
 }; // namespace matrix