51 lines
1.5 KiB
C++
51 lines
1.5 KiB
C++
#include "FtmKalman.h"
|
|
|
|
#include <eigen3/Eigen/Eigen>
|
|
|
|
float Kalman::predict(const Timestamp timestamp, const float measurment)
|
|
{
|
|
constexpr auto square = [](float x) { return x * x; };
|
|
const auto I = Eigen::Matrix2f::Identity();
|
|
|
|
Eigen::Map<Eigen::Matrix<float, 2, 1>> x(this->x);
|
|
Eigen::Map<Eigen::Matrix<float, 2, 2>> P(this->P);
|
|
|
|
// init kalman filter
|
|
if (isnan(lastTimestamp))
|
|
{
|
|
P << 10, 0,
|
|
0, 10; // Initial Uncertainty
|
|
|
|
x << measurment,
|
|
0;
|
|
}
|
|
|
|
const float dt = isnan(lastTimestamp) ? 1 : timestamp.sec() - lastTimestamp;
|
|
lastTimestamp = timestamp.sec();
|
|
|
|
Eigen::Matrix<float, 1, 2> H; // Measurement function
|
|
H << 1, 0;
|
|
|
|
Eigen::Matrix2f A; // Transition Matrix
|
|
A << 1, dt,
|
|
0, 1;
|
|
|
|
Eigen::Matrix2f Q; // Process Noise Covariance
|
|
Q << square(processNoiseDistance), 0,
|
|
0, square(processNoiseVelocity);
|
|
|
|
// Prediction
|
|
x = A * x; // Prädizierter Zustand aus Bisherigem und System
|
|
P = A * P*A.transpose() + Q; // Prädizieren der Kovarianz
|
|
|
|
// Correction
|
|
float Z = measurment;
|
|
auto y = Z - (H*x); // Innovation aus Messwertdifferenz
|
|
auto S = (H*P*H.transpose() + R); // Innovationskovarianz
|
|
auto K = P * H.transpose()* (1 / S); //Filter-Matrix (Kalman-Gain)
|
|
|
|
x = x + (K*y); // aktualisieren des Systemzustands
|
|
P = (I - (K*H))*P; // aktualisieren der Kovarianz
|
|
|
|
return x(0);
|
|
} |