diff --git a/book/src/introduction.md b/book/src/introduction.md index b18d8ff..6d3fe9f 100644 --- a/book/src/introduction.md +++ b/book/src/introduction.md @@ -1,7 +1,42 @@ # cppXplorers : Explorations in C++ for applied sciences -`cppXplorers` is a collection of experiments and examples in modern C++, with a focus on applied sciences and engineering. The project is organized as a mono-repository, where each crate is independent and self-contained. +`cppXplorers` is a collection of experiments and examples in modern C++, with a focus on applied sciences and engineering. The project is organized as a mono-repository, where each crate is independent and self-contained. All the sources can be found in the folder `crates` of the [repository](https://github.com/bstaber/cppxplorers/). The purpose of this repository is to provide a space for exploring numerical methods, algorithms, and patterns in C++ in a practical and modular way. It is not intended as a tutorial or comprehensive learning resource, but rather as a set of working examples and references. -Contributions are welcome. If you spot an issue, find an area that could be improved, or want to add your own example, feel free to open an issue or submit a pull request. \ No newline at end of file +Contributions are welcome. If you spot an issue, find an area that could be improved, or want to add your own example, feel free to open an issue or submit a pull request. + +## Organisation and layout + +This project has the following layout: + +```bash +├── book +│ └── src +└── crates + ├── kf_linear + │ ├── include + │ ├── src + │ └── tests + ├── another_example + │ ├── include + │ ├── src + │ └── tests + ├── another_example + │ ├── include + │ ├── src + │ └── tests + ├── ... + │ ├── include + │ ├── src + │ └── tests + └── simple_optimizers + ├── include + ├── src + └── tests +``` + +* The `crates` folder contains all the examples. I apologize, I've been doing some Rust lately ([rustineers](https://github.com/bstaber/rustineers)). +* Each example has its own headers, sources, and tests. +* The book folder simply contains the sources for generating this book with `mdBook`. +* Each chapter in the book will explain what's implemented in each example. \ No newline at end of file diff --git a/book/src/kf_linear.md b/book/src/kf_linear.md index c4d593f..0fccc8b 100644 --- a/book/src/kf_linear.md +++ b/book/src/kf_linear.md @@ -62,4 +62,85 @@ $$ K_t = P_t^{t-1} A_t^T(A_t P_t^{t-1}A_t^T + S)^{-1}\,. $$ -Implementing the Kalman filter boils down to implement these few equations! \ No newline at end of file +Implementing the Kalman filter boils down to implement these few equations! + +## C++ implementation + +The following code provides a generic templated class `KFLinear` supporting both fixed-size and dynamic-size state and measurement vectors, using the [Eigen](https://eigen.tuxfamily.org/) linear algebra library. + +
+Click here to view the full implementation: include/kf_linear.hpp. We break into down in the sequel of this section. + +```cpp +{{#include ../../crates/kf_linear/include/kf_linear.hpp}} +``` +
+ + +Here's the header file without the inlined implementations. + +```cpp +#pragma once + +#include +#include +#include +#include +#include + +/** + * @brief Generic linear Kalman filter (templated, no control term). + * + * State-space model: + * x_k = A x_{k-1} + w_{k-1}, w ~ N(0, Q) + * z_k = H x_k + v_k, v ~ N(0, R) + * + * Template parameters: + * Nx = state dimension (int or Eigen::Dynamic) + * Ny = measurement dimension(int or Eigen::Dynamic) + */ +template class KFLinear { + public: + using StateVec = Eigen::Matrix; + using StateMat = Eigen::Matrix; + using MeasVec = Eigen::Matrix; + using MeasMat = Eigen::Matrix; + using ObsMat = Eigen::Matrix; + + KFLinear(const StateVec &initial_state, const StateMat &initial_covariance, + const StateMat &transition_matrix, const ObsMat &observation_matrix, + const StateMat &process_covariance, const MeasMat &measurement_covariance); + + void predict(); + void update(const MeasVec &z); + void step(const std::optional &measurement); + std::vector filter(const std::vector> &measurements); + + [[nodiscard]] const StateVec &state() const { return x_; } + [[nodiscard]] const StateMat &covariance() const { return P_; } + + void set_transition(const StateMat &A) { A_ = A; } + void set_observation(const ObsMat &H) { H_ = H; } + void set_process_noise(const StateMat &Q) { Q_ = Q; } + void set_measurement_noise(const MeasMat &R){ R_ = R; } + + private: + StateMat A_, Q_, P_; + ObsMat H_; + MeasMat R_; + StateVec x_; +}; +``` + +A few comments: + +- **Predict step**: The method `predict()` propagates the state and covariance using the transition matrix A and process noise covariance Q. + +- **Update step**: The method `update(z)` corrects the prediction using a new measurement z. It computes the Kalman gain K efficiently by solving a linear system with LDLT factorization instead of forming the matrix inverse explicitly. The covariance update uses the Joseph form to ensure numerical stability and positive semi-definiteness. + +- **Step and filter**: The `step()` method combines prediction with an optional update (useful when some measurements are missing). The `filter()` method processes an entire sequence of measurements, returning the sequence of estimated states. + +- **Flexibility**: + - Works with both fixed-size and dynamic-size Eigen matrices. + - Provides setters to update system matrices online (e.g. if the model changes over time). + - Uses `std::optional` to naturally handle missing observations.