52 TrackerKalman::TrackerKalman(
const string& name,
const StatePosVel& sysnoise):
60 tracker_initialized_(false)
64 for (
unsigned int i = 1; i <= 3; i++)
69 ColumnVector sys_mu(6);
73 for (
unsigned int i = 0; i < 3; i++)
84 Matrix meas_matrix(3, 6);
86 for (
unsigned int i = 1; i <= 3; i++)
87 meas_matrix(i, i) = 1;
89 ColumnVector meas_mu(3);
91 SymmetricMatrix meas_sigma(3);
93 for (
unsigned int i = 0; i < 3; i++)
94 meas_sigma(i + 1, i + 1) = 0;
95 Gaussian meas_noise(meas_mu, meas_sigma);
96 meas_pdf_ =
new LinearAnalyticConditionalGaussian(meas_matrix, meas_noise);
117 ColumnVector mu_vec(6);
118 SymmetricMatrix sigma_vec(6);
120 for (
unsigned int i = 0; i < 3; i++)
122 mu_vec(i + 1) = mu.
pos_[i];
123 mu_vec(i + 4) = mu.
vel_[i];
124 sigma_vec(i + 1, i + 1) = pow(sigma.
pos_[i], 2);
125 sigma_vec(i + 4, i + 4) = pow(sigma.
vel_[i], 2);
127 prior_ = Gaussian(mu_vec, sigma_vec);
147 for (
unsigned int i = 1; i <= 3; i++)
168 assert(cov.columns() == 3);
171 ColumnVector meas_vec(3);
172 for (
unsigned int i = 0; i < 3; i++)
173 meas_vec(i + 1) = meas[i];
176 ((LinearAnalyticConditionalGaussian*)(
meas_model_->MeasurementPdfGet()))->AdditiveNoiseSigmaSet(cov);
189 ColumnVector tmp =
filter_->PostGet()->ExpectedValueGet();
190 for (
unsigned int i = 0; i < 3; i++)
192 est.
pos_[i] = tmp(i + 1);
193 est.
vel_[i] = tmp(i + 4);
200 ColumnVector tmp =
filter_->PostGet()->ExpectedValueGet();
215 double sigma_max = 0;
216 SymmetricMatrix cov =
filter_->PostGet()->CovarianceGet();
217 for (
unsigned int i = 1; i <= 2; i++)
218 sigma_max = max(sigma_max, sqrt(cov(i, i)));
220 return 1.0 -
min(1.0, sigma_max / 1.5);
virtual void getEstimate(BFL::StatePosVel &est) const
get filter posterior
const std::string & getName() const
return the name of the tracker
virtual double getTime() const
return the time of the tracker
virtual double getLifetime() const
return the lifetime of the tracker
bool tracker_initialized_
BFL::ExtendedKalmanFilter * filter_
Class representing state with pos and vel.
BFL::LinearAnalyticMeasurementModelGaussianUncertainty * meas_model_
static const double damping_velocity
BFL::LinearAnalyticSystemModelGaussianUncertainty * sys_model_
double calculateQuality()
virtual ~TrackerKalman()
destructor
virtual void initialize(const BFL::StatePosVel &mu, const BFL::StatePosVel &sigma, const double time)
initialize tracker
MatrixWrapper::Matrix sys_matrix_
BFL::LinearAnalyticConditionalGaussian * meas_pdf_
BFL::LinearAnalyticConditionalGaussian * sys_pdf_
virtual bool updateCorrection(const tf::Vector3 &meas, const MatrixWrapper::SymmetricMatrix &cov)
MatrixWrapper::SymmetricMatrix sys_sigma_
virtual bool updatePrediction(const double time)
update tracker