123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152 |
- #include "kalmanFilter.h"
- #include <Eigen/Cholesky>
-
- namespace byte_kalman
- {
- const double KalmanFilter::chi2inv95[10] = {
- 0,
- 3.8415,
- 5.9915,
- 7.8147,
- 9.4877,
- 11.070,
- 12.592,
- 14.067,
- 15.507,
- 16.919
- };
- KalmanFilter::KalmanFilter()
- {
- int ndim = 4;
- double dt = 1.;
-
- _motion_mat = Eigen::MatrixXf::Identity(8, 8);
- for (int i = 0; i < ndim; i++) {
- _motion_mat(i, ndim + i) = dt;
- }
- _update_mat = Eigen::MatrixXf::Identity(4, 8);
-
- this->_std_weight_position = 1. / 20;
- this->_std_weight_velocity = 1. / 160;
- }
-
- KAL_DATA KalmanFilter::initiate(const DETECTBOX &measurement)
- {
- DETECTBOX mean_pos = measurement;
- DETECTBOX mean_vel;
- for (int i = 0; i < 4; i++) mean_vel(i) = 0;
-
- KAL_MEAN mean;
- for (int i = 0; i < 8; i++) {
- if (i < 4) mean(i) = mean_pos(i);
- else mean(i) = mean_vel(i - 4);
- }
-
- KAL_MEAN std;
- std(0) = 2 * _std_weight_position * measurement[3];
- std(1) = 2 * _std_weight_position * measurement[3];
- std(2) = 1e-2;
- std(3) = 2 * _std_weight_position * measurement[3];
- std(4) = 10 * _std_weight_velocity * measurement[3];
- std(5) = 10 * _std_weight_velocity * measurement[3];
- std(6) = 1e-5;
- std(7) = 10 * _std_weight_velocity * measurement[3];
-
- KAL_MEAN tmp = std.array().square();
- KAL_COVA var = tmp.asDiagonal();
- return std::make_pair(mean, var);
- }
-
- void KalmanFilter::predict(KAL_MEAN &mean, KAL_COVA &covariance)
- {
- //revise the data;
- DETECTBOX std_pos;
- std_pos << _std_weight_position * mean(3),
- _std_weight_position * mean(3),
- 1e-2,
- _std_weight_position * mean(3);
- DETECTBOX std_vel;
- std_vel << _std_weight_velocity * mean(3),
- _std_weight_velocity * mean(3),
- 1e-5,
- _std_weight_velocity * mean(3);
- KAL_MEAN tmp;
- tmp.block<1, 4>(0, 0) = std_pos;
- tmp.block<1, 4>(0, 4) = std_vel;
- tmp = tmp.array().square();
- KAL_COVA motion_cov = tmp.asDiagonal();
- KAL_MEAN mean1 = this->_motion_mat * mean.transpose();
- KAL_COVA covariance1 = this->_motion_mat * covariance *(_motion_mat.transpose());
- covariance1 += motion_cov;
-
- mean = mean1;
- covariance = covariance1;
- }
-
- KAL_HDATA KalmanFilter::project(const KAL_MEAN &mean, const KAL_COVA &covariance)
- {
- DETECTBOX std;
- std << _std_weight_position * mean(3), _std_weight_position * mean(3),
- 1e-1, _std_weight_position * mean(3);
- KAL_HMEAN mean1 = _update_mat * mean.transpose();
- KAL_HCOVA covariance1 = _update_mat * covariance * (_update_mat.transpose());
- Eigen::Matrix<float, 4, 4> diag = std.asDiagonal();
- diag = diag.array().square().matrix();
- covariance1 += diag;
- // covariance1.diagonal() << diag;
- return std::make_pair(mean1, covariance1);
- }
-
- KAL_DATA
- KalmanFilter::update(
- const KAL_MEAN &mean,
- const KAL_COVA &covariance,
- const DETECTBOX &measurement)
- {
- KAL_HDATA pa = project(mean, covariance);
- KAL_HMEAN projected_mean = pa.first;
- KAL_HCOVA projected_cov = pa.second;
-
- //chol_factor, lower =
- //scipy.linalg.cho_factor(projected_cov, lower=True, check_finite=False)
- //kalmain_gain =
- //scipy.linalg.cho_solve((cho_factor, lower),
- //np.dot(covariance, self._upadte_mat.T).T,
- //check_finite=False).T
- Eigen::Matrix<float, 4, 8> B = (covariance * (_update_mat.transpose())).transpose();
- Eigen::Matrix<float, 8, 4> kalman_gain = (projected_cov.llt().solve(B)).transpose(); // eg.8x4
- Eigen::Matrix<float, 1, 4> innovation = measurement - projected_mean; //eg.1x4
- auto tmp = innovation * (kalman_gain.transpose());
- KAL_MEAN new_mean = (mean.array() + tmp.array()).matrix();
- KAL_COVA new_covariance = covariance - kalman_gain * projected_cov*(kalman_gain.transpose());
- return std::make_pair(new_mean, new_covariance);
- }
-
- Eigen::Matrix<float, 1, -1>
- KalmanFilter::gating_distance(
- const KAL_MEAN &mean,
- const KAL_COVA &covariance,
- const std::vector<DETECTBOX> &measurements,
- bool only_position)
- {
- KAL_HDATA pa = this->project(mean, covariance);
- if (only_position) {
- printf("not implement!");
- exit(0);
- }
- KAL_HMEAN mean1 = pa.first;
- KAL_HCOVA covariance1 = pa.second;
-
- // Eigen::Matrix<float, -1, 4, Eigen::RowMajor> d(size, 4);
- DETECTBOXSS d(measurements.size(), 4);
- int pos = 0;
- for (DETECTBOX box : measurements) {
- d.row(pos++) = box - mean1;
- }
- Eigen::Matrix<float, -1, -1, Eigen::RowMajor> factor = covariance1.llt().matrixL();
- Eigen::Matrix<float, -1, -1> z = factor.triangularView<Eigen::Lower>().solve<Eigen::OnTheRight>(d).transpose();
- auto zz = ((z.array())*(z.array())).matrix();
- auto square_maha = zz.colwise().sum();
- return square_maha;
- }
- }
|