11#pragma once
22
33#include < Eigen/Dense>
4+ #include < iostream>
45#include < optional>
5- #include < vector>
66#include < stdexcept>
7- #include < iostream >
7+ #include < vector >
88
99/* *
1010 * @brief Generic linear Kalman filter (templated, no control term).
1717 * Nx = state dimension (int or Eigen::Dynamic)
1818 * Ny = measurement dimension(int or Eigen::Dynamic)
1919 */
20- template <int Nx, int Ny>
21- class KFLinear {
22- public:
20+ template <int Nx, int Ny> class KFLinear {
21+ public:
2322 using StateVec = Eigen::Matrix<double , Nx, 1 >;
2423 using StateMat = Eigen::Matrix<double , Nx, Nx>;
25- using MeasVec = Eigen::Matrix<double , Ny, 1 >;
26- using MeasMat = Eigen::Matrix<double , Ny, Ny>;
27- using ObsMat = Eigen::Matrix<double , Ny, Nx>;
24+ using MeasVec = Eigen::Matrix<double , Ny, 1 >;
25+ using MeasMat = Eigen::Matrix<double , Ny, Ny>;
26+ using ObsMat = Eigen::Matrix<double , Ny, Nx>;
2827
2928 // / Construct filter with initial condition and model matrices.
30- KFLinear (const StateVec& initial_state,
31- const StateMat& initial_covariance,
32- const StateMat& transition_matrix,
33- const ObsMat& observation_matrix,
34- const StateMat& process_covariance,
35- const MeasMat& measurement_covariance)
36- : x_(initial_state),
37- P_ (initial_covariance),
38- A_(transition_matrix),
39- H_(observation_matrix),
40- Q_(process_covariance),
41- R_(measurement_covariance)
42- {
43- std::cout << " DEBUG" << std::endl;
44- const auto n = x_.rows ();
45- if (A_.rows () != n || A_.cols () != n)
46- throw std::invalid_argument (" A must be n×n and match x dimension" );
47- if (P_.rows () != n || P_.cols () != n)
48- throw std::invalid_argument (" P must be n×n" );
49- if (Q_.rows () != n || Q_.cols () != n)
50- throw std::invalid_argument (" Q must be n×n" );
51- if (H_.cols () != n)
52- throw std::invalid_argument (" H must have n columns" );
53- const auto m = H_.rows ();
54- if (R_.rows () != m || R_.cols () != m)
55- throw std::invalid_argument (" R must be m×m with m = H.rows()" );
56- }
29+ KFLinear (const StateVec &initial_state, const StateMat &initial_covariance,
30+ const StateMat &transition_matrix, const ObsMat &observation_matrix,
31+ const StateMat &process_covariance, const MeasMat &measurement_covariance)
32+ : x_(initial_state), P_(initial_covariance), A_(transition_matrix), H_(observation_matrix),
33+ Q_ (process_covariance), R_(measurement_covariance) {
34+ std::cout << " DEBUG" << std::endl;
35+ const auto n = x_.rows ();
36+ if (A_.rows () != n || A_.cols () != n)
37+ throw std::invalid_argument (" A must be n×n and match x dimension" );
38+ if (P_.rows () != n || P_.cols () != n)
39+ throw std::invalid_argument (" P must be n×n" );
40+ if (Q_.rows () != n || Q_.cols () != n)
41+ throw std::invalid_argument (" Q must be n×n" );
42+ if (H_.cols () != n)
43+ throw std::invalid_argument (" H must have n columns" );
44+ const auto m = H_.rows ();
45+ if (R_.rows () != m || R_.cols () != m)
46+ throw std::invalid_argument (" R must be m×m with m = H.rows()" );
47+ }
5748
5849 // / Predict step (no control).
5950 void predict () {
@@ -62,7 +53,7 @@ class KFLinear {
6253 }
6354
6455 // / Update step with a measurement z.
65- void update (const MeasVec& z) {
56+ void update (const MeasVec & z) {
6657 // Innovation
6758 MeasVec nu = z - H_ * x_;
6859
@@ -76,9 +67,8 @@ class KFLinear {
7667 }
7768
7869 // K = P H^T S^{-1} via solve: S * (K^T) = (P H^T)^T
79- const auto PHt = P_ * H_.transpose (); // (Nx × Ny)
80- Eigen::Matrix<double , Nx, Ny> K =
81- ldlt.solve (PHt.transpose ()).transpose (); // (Nx × Ny)
70+ const auto PHt = P_ * H_.transpose (); // (Nx × Ny)
71+ Eigen::Matrix<double , Nx, Ny> K = ldlt.solve (PHt.transpose ()).transpose (); // (Nx × Ny)
8272
8373 // State update
8474 x_ += K * nu;
@@ -92,42 +82,42 @@ class KFLinear {
9282 }
9383
9484 // / One full step: predict then (optionally) update.
95- void step (const std::optional<MeasVec>& measurement) {
85+ void step (const std::optional<MeasVec> & measurement) {
9686 predict ();
9787 if (measurement) {
9888 update (*measurement);
9989 }
10090 }
10191
10292 // / Run over a sequence of (optional) measurements.
103- std::vector<StateVec> filter (const std::vector<std::optional<MeasVec>>& measurements) {
93+ std::vector<StateVec> filter (const std::vector<std::optional<MeasVec>> & measurements) {
10494 std::vector<StateVec> out;
10595 out.reserve (measurements.size ());
106- for (const auto & z : measurements) {
96+ for (const auto & z : measurements) {
10797 step (z);
10898 out.push_back (x_);
10999 }
110100 return out;
111101 }
112102
113103 // Accessors
114- [[nodiscard]] const StateVec& state () const { return x_; }
115- [[nodiscard]] const StateMat& covariance () const { return P_; }
104+ [[nodiscard]] const StateVec & state () const { return x_; }
105+ [[nodiscard]] const StateMat & covariance () const { return P_; }
116106
117107 // (Optional) setters if you want to tweak model online
118- void set_transition (const StateMat& A) { A_ = A; }
119- void set_observation (const ObsMat& H) { H_ = H; }
120- void set_process_noise (const StateMat& Q) { Q_ = Q; }
121- void set_measurement_noise (const MeasMat& R) { R_ = R; }
108+ void set_transition (const StateMat & A) { A_ = A; }
109+ void set_observation (const ObsMat &H) { H_ = H; }
110+ void set_process_noise (const StateMat & Q) { Q_ = Q; }
111+ void set_measurement_noise (const MeasMat & R) { R_ = R; }
122112
123- private:
113+ private:
124114 // Model
125- StateMat A_; // /< State transition
126- ObsMat H_; // /< Observation
127- StateMat Q_; // /< Process noise covariance
128- MeasMat R_; // /< Measurement noise covariance
115+ StateMat A_; // /< State transition
116+ ObsMat H_; // /< Observation
117+ StateMat Q_; // /< Process noise covariance
118+ MeasMat R_; // /< Measurement noise covariance
129119
130120 // Estimates
131- StateVec x_; // /< State mean
132- StateMat P_; // /< State covariance
121+ StateVec x_; // /< State mean
122+ StateMat P_; // /< State covariance
133123};
0 commit comments