Spaces:
Runtime error
Runtime error
File size: 4,544 Bytes
7734d5b |
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 |
#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;
}
} |