45 lines
980 B
C
45 lines
980 B
C
#pragma once
|
|
|
|
#include <cmath>
|
|
|
|
#include <Indoor/data/Timestamp.h>
|
|
|
|
|
|
struct KalmanPrediction
|
|
{
|
|
float distance;
|
|
float speed;
|
|
|
|
float P[4]; // Covariance
|
|
};
|
|
|
|
struct Kalman
|
|
{
|
|
int nucID = 0; // debug only
|
|
|
|
float x[2] = {NAN}; // predicted state [m, m/s]
|
|
float P[4] = {NAN}; // 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 predictAndUpdate(const Timestamp timestamp, const float measurment);
|
|
KalmanPrediction predict(const Timestamp timestamp);
|
|
};
|
|
|
|
|
|
|