diff --git a/src/shared/algorithms/ADA/ADA.h b/src/shared/algorithms/ADA/ADA.h
index 26072b16ac3b9c09196a9993ca1dcc332f98917a..0aca225ab05894afec14c3736eca708e55e65515 100644
--- a/src/shared/algorithms/ADA/ADA.h
+++ b/src/shared/algorithms/ADA/ADA.h
@@ -33,7 +33,7 @@ namespace Boardcore
class ADA
{
public:
- using KalmanFilter = Kalman<float, 3, 1>;
+ using KalmanFilter = Kalman<float, 3, 1, 1>;
explicit ADA(const KalmanFilter::KalmanConfig kalmanConfig);
diff --git a/src/shared/algorithms/Kalman/Kalman.h b/src/shared/algorithms/Kalman/Kalman.h
index 8673baf8eaa5de97e29bc29511fb9430408d7292..3f7d4e76f4d4d0a2411725d114e3f79404288ce2 100644
--- a/src/shared/algorithms/Kalman/Kalman.h
+++ b/src/shared/algorithms/Kalman/Kalman.h
@@ -32,8 +32,12 @@ namespace Boardcore
*
* This class uses templates in order to know the size of each matrix at
* compile-time. This way we avoid Eigen to allocate memory dynamically.
+ *
+ * N_size: number of states
+ * P_size: number of measurements
+ * M_size: number of control inputs (optional)
*/
-template <typename T, int N_size, int P_size>
+template <typename T, int N_size, int P_size, int M_size>
class Kalman
{
public:
@@ -41,8 +45,10 @@ public:
using MatrixPN = Eigen::Matrix<T, P_size, N_size>;
using MatrixNP = Eigen::Matrix<T, N_size, P_size>;
using MatrixPP = Eigen::Matrix<T, P_size, P_size>;
+ using MatrixNM = Eigen::Matrix<T, N_size, M_size>;
using CVectorN = Eigen::Vector<T, N_size>;
using CVectorP = Eigen::Vector<T, P_size>;
+ using CVectorM = Eigen::Vector<T, M_size>;
/**
* @brief Configuration struct for the Kalman class.
@@ -54,6 +60,7 @@ public:
MatrixNN Q;
MatrixPP R;
MatrixNN P;
+ MatrixNM G;
CVectorN x;
};
@@ -64,8 +71,8 @@ public:
*/
Kalman(const KalmanConfig& config)
: F(config.F), H(config.H), Q(config.Q), R(config.R), P(config.P),
- S(MatrixPP::Zero(P_size, P_size)), K(MatrixNP::Zero(N_size, P_size)),
- x(config.x)
+ G(config.G), S(MatrixPP::Zero(P_size, P_size)),
+ K(MatrixNP::Zero(N_size, P_size)), x(config.x)
{
I.setIdentity();
}
@@ -84,10 +91,12 @@ public:
/**
* @brief Prediction step with previous F matrix.
+ *
+ * @param input Control inputs for the step (optional).
*/
- void predict()
+ void predict(const CVectorM& input = CVectorM::Zero())
{
- x = F * x;
+ x = F * x + G * input;
P = F * P * F.transpose() + Q;
}
@@ -95,11 +104,13 @@ public:
* @brief Prediction step.
*
* @param F_new updated F matrix.
+ * @param input Control inputs for the step (optional).
*/
- void predict(const MatrixNN& F_new)
+ void predict(const MatrixNN& F_new,
+ const CVectorM& input = CVectorM::Zero())
{
F = F_new;
- predict();
+ predict(input);
}
/**
@@ -169,6 +180,7 @@ private:
MatrixNN Q; /**< Model variance matrix (n x n) */
MatrixPP R; /**< Measurement variance (p x p) */
MatrixNN P; /**< Error covariance matrix (n x n) */
+ MatrixNM G; /**< Input matrix (n x m) */
MatrixPP S;
MatrixNP K; /**< kalman gain */
diff --git a/src/tests/algorithms/ADA/test-ada.cpp b/src/tests/algorithms/ADA/test-ada.cpp
index 646604c672fe1a756e3c465bb1df550650289540..bc268e1334a90ddad6649f22dd34b4d9536efc3e 100644
--- a/src/tests/algorithms/ADA/test-ada.cpp
+++ b/src/tests/algorithms/ADA/test-ada.cpp
@@ -71,12 +71,14 @@ ADA::KalmanFilter::KalmanConfig getADAKalmanConfig()
// cppcheck-suppress constStatement
Q_INIT << 30.0, 0.0, 0.0, 0.0, 10.0, 0.0, 0.0, 0.0, 2.5f;
ADA::KalmanFilter::MatrixPP R_INIT{4000.0f};
+ ADA::KalmanFilter::MatrixNM G_INIT = ADA::KalmanFilter::MatrixNM::Zero();
return {F_INIT,
H_INIT,
Q_INIT,
R_INIT,
P_INIT,
+ G_INIT,
ADA::KalmanFilter::CVectorN(DEFAULT_REFERENCE_PRESSURE, 0,
KALMAN_INITIAL_ACCELERATION)};
}
diff --git a/src/tests/algorithms/Kalman/test-kalman-benchmark.cpp b/src/tests/algorithms/Kalman/test-kalman-benchmark.cpp
index 52ca7e81c5ea91644604209d62d87c177f02ca6f..34ff301cbd78174057da5de8a30dec3173c288a8 100644
--- a/src/tests/algorithms/Kalman/test-kalman-benchmark.cpp
+++ b/src/tests/algorithms/Kalman/test-kalman-benchmark.cpp
@@ -45,6 +45,7 @@ int main()
const int n = 3;
const int p = 1;
+ const int m = 1;
Matrix<float, n, n> F =
(Matrix<float, n, n>(n, n) << 1, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0)
@@ -67,15 +68,18 @@ int main()
Matrix<float, p, 1> y(p); // vector with p elements (only one in this case)
- Kalman<float, n, p>::KalmanConfig config;
+ Matrix<float, n, m> G = Matrix<float, n, m>::Zero();
+
+ Kalman<float, n, p, m>::KalmanConfig config;
config.F = F;
config.H = H;
config.Q = Q;
config.R = R;
config.P = P;
+ config.G = G;
config.x = x0;
- Kalman<float, n, p> filter(config);
+ Kalman<float, n, p, m> filter(config);
float lastTime = 0.0; // Variable to save the time of the last sample
diff --git a/src/tests/catch/test-kalman.cpp b/src/tests/catch/test-kalman.cpp
index 71974bebedf31ff6790e7cc7104c9c76b5fd2538..80f7959cf52ddd02acc9763cca1b510a36b18a0b 100644
--- a/src/tests/catch/test-kalman.cpp
+++ b/src/tests/catch/test-kalman.cpp
@@ -38,6 +38,8 @@ using namespace Eigen;
static const uint8_t STATES_DIM = 3;
static const uint8_t OUTPUTS_DIM = 1;
+static const uint8_t INPUTS_DIM =
+ 1; // Actually zero, but we can't use 0 in the template
static const Matrix<float, STATES_DIM, STATES_DIM> F =
(Matrix<float, STATES_DIM, STATES_DIM>(STATES_DIM, STATES_DIM) << 1, 0.2,
@@ -61,15 +63,19 @@ static const Matrix<float, OUTPUTS_DIM, OUTPUTS_DIM> R{10};
// State vector
static const Matrix<float, STATES_DIM, 1> x0(INPUT[0], 0.0, 0.0);
-static const Kalman<float, STATES_DIM, OUTPUTS_DIM>::KalmanConfig
+static const Matrix<float, STATES_DIM, INPUTS_DIM> G =
+ Matrix<float, STATES_DIM, INPUTS_DIM>::Zero();
+
+static const Kalman<float, STATES_DIM, OUTPUTS_DIM, INPUTS_DIM>::KalmanConfig
getKalmanConfig()
{
- Kalman<float, STATES_DIM, OUTPUTS_DIM>::KalmanConfig config;
+ Kalman<float, STATES_DIM, OUTPUTS_DIM, INPUTS_DIM>::KalmanConfig config;
config.F = F;
config.H = H;
config.Q = Q;
config.R = R;
config.P = P;
+ config.G = G;
config.x = x0;
return config;
@@ -77,7 +83,8 @@ getKalmanConfig()
TEST_CASE("Update test")
{
- Kalman<float, STATES_DIM, OUTPUTS_DIM> filter(getKalmanConfig());
+ Kalman<float, STATES_DIM, OUTPUTS_DIM, INPUTS_DIM> filter(
+ getKalmanConfig());
Matrix<float, OUTPUTS_DIM, 1> y{};
float lastTime = TIME[0];