79 lines
2.2 KiB
C++
79 lines
2.2 KiB
C++
#pragma once
|
|
|
|
#include <eigen3/Eigen/Eigen>
|
|
|
|
#include <Indoor/data/Timestamp.h>
|
|
|
|
|
|
struct Kalman
|
|
{
|
|
int nucID = 0; // debug only
|
|
|
|
Eigen::Matrix<float, 2, 1> x; // predicted state
|
|
Eigen::Matrix<float, 2, 2> P; // Covariance
|
|
|
|
float R = 30; // measurement noise covariance
|
|
float processNoiseDistance; // stdDev
|
|
float processNoiseVelocity; // stdDev
|
|
|
|
float lastTimestamp = NAN; // in sec
|
|
|
|
Kalman(): nucID(0) { }
|
|
|
|
Kalman(int nucID)
|
|
: nucID(nucID)
|
|
{}
|
|
|
|
Kalman(int nucID, float measStdDev, float processNoiseDistance = 0.2, float processNoiseVelocity = 0.4)
|
|
: nucID(nucID), R(measStdDev*measStdDev), processNoiseDistance(processNoiseDistance), processNoiseVelocity(processNoiseVelocity)
|
|
{}
|
|
|
|
float predict(const Timestamp timestamp, const float measurment)
|
|
{
|
|
constexpr auto square = [](float x) { return x * x; };
|
|
const auto I = Eigen::Matrix2f::Identity();
|
|
|
|
// 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);
|
|
}
|
|
};
|
|
|
|
|
|
|