forked from dsutanto/bytetrack-counter-cpp
25 lines
600 B
C++
25 lines
600 B
C++
#pragma once
|
|
#include <Eigen/Dense>
|
|
#include <cmath>
|
|
#include <cstdint>
|
|
|
|
class KalmanFilter {
|
|
public:
|
|
static constexpr int kStateDim = 4;
|
|
static constexpr int kTotalDim = 2 * kStateDim;
|
|
|
|
KalmanFilter();
|
|
|
|
void predict();
|
|
void update(const Eigen::Vector4f& z);
|
|
|
|
Eigen::Matrix<float, kTotalDim, 1> x;
|
|
Eigen::Matrix<float, kTotalDim, kTotalDim> P;
|
|
|
|
private:
|
|
Eigen::Matrix<float, kTotalDim, kTotalDim> motion_mat_;
|
|
Eigen::Matrix<float, kStateDim, kTotalDim> update_mat_;
|
|
float std_weight_position_ = 1.0f / 20.0f;
|
|
float std_weight_velocity_ = 1.0f / 160.0f;
|
|
};
|