From e30c3a0e5eba587ce02ee727aa914c5b140dae7a Mon Sep 17 00:00:00 2001
From: FrancoisCarouge <francois.carouge@gmail.com>
Date: Tue, 11 Jul 2023 22:04:54 -0700
Subject: [PATCH] [filter] name filter identity value

---
 include/fcarouge/internal/kalman.hpp | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/include/fcarouge/internal/kalman.hpp b/include/fcarouge/internal/kalman.hpp
index 1f6b8d6cf..a6f35108f 100644
--- a/include/fcarouge/internal/kalman.hpp
+++ b/include/fcarouge/internal/kalman.hpp
@@ -83,6 +83,8 @@ struct kalman<State, Output, void, pack<UpdateTypes...>,
   using update_types = std::tuple<UpdateTypes...>;
   using prediction_types = std::tuple<PredictionTypes...>;
 
+  static inline const auto i{identity_v<quotient<state, state>>};
+
   state x{zero_v<state>};
   estimate_uncertainty p{identity_v<estimate_uncertainty>};
   process_uncertainty q{zero_v<process_uncertainty>};
@@ -149,9 +151,7 @@ struct kalman<State, Output, void, pack<UpdateTypes...>,
     k = p * t(h) / s;
     y = z - observation(x, update_pack...);
     x = state{x + k * y};
-    p = estimate_uncertainty{(identity_v<quotient<state, state>> - k * h) * p *
-                                 t(identity_v<quotient<state, state>> - k * h) +
-                             k * r * t(k)};
+    p = estimate_uncertainty{(i - k * h) * p * t(i - k * h) + k * r * t(k)};
   }
 
   inline constexpr void predict(const PredictionTypes &...prediction_pack) {
@@ -196,6 +196,8 @@ struct kalman<State, Output, Input, pack<UpdateTypes...>,
   using update_types = std::tuple<UpdateTypes...>;
   using prediction_types = std::tuple<PredictionTypes...>;
 
+  static inline const auto i{identity_v<quotient<state, state>>};
+
   state x{zero_v<state>};
   estimate_uncertainty p{identity_v<estimate_uncertainty>};
   process_uncertainty q{zero_v<process_uncertainty>};
@@ -267,9 +269,7 @@ struct kalman<State, Output, Input, pack<UpdateTypes...>,
     k = p * t(h) / s;
     y = z - observation(x, update_pack...);
     x = state{x + k * y};
-    p = estimate_uncertainty{(identity_v<quotient<state, state>> - k * h) * p *
-                                 t(identity_v<quotient<state, state>> - k * h) +
-                             k * r * t(k)};
+    p = estimate_uncertainty{(i - k * h) * p * t(i - k * h) + k * r * t(k)};
   }
 
   //! @todo Extended support?