'If my Kalman Filter ignores measurements for being old, does that make it ageist? But actually, how do you update with old measurements/Should I?
Applying code here https://github.com/JunshengFu/tracking-with-Extended-Kalman-Filter/tree/master/src (code snippet from FusionEKF.cpp) and it looks like there's an assumption of it only being updated forward in time and not backwards. Besides it being annoyingly only in 2D, Q looks wrong for negative dt's.
// compute the time elapsed between the current and previous measurements
float dt = (measurement_pack.timestamp_ - previous_timestamp_) / 1000000.0; // in seconds
previous_timestamp_ = measurement_pack.timestamp_;
float dt_2 = dt * dt;
float dt_3 = dt_2 * dt;
float dt_4 = dt_3 * dt;
// Modify the F matrix so that the time is integrated
ekf_.F_(0, 2) = dt;
ekf_.F_(1, 3) = dt;
//set the process covariance matrix Q
ekf_.Q_ = MatrixXd(4, 4);
ekf_.Q_ << dt_4/4*noise_ax, 0, dt_3/2*noise_ax, 0,
0, dt_4/4*noise_ay, 0, dt_3/2*noise_ay,
dt_3/2*noise_ax, 0, dt_2*noise_ax, 0,
0, dt_3/2*noise_ay, 0, dt_2*noise_ay;
ekf_.Predict();
/*****************************************************************************
* Update
****************************************************************************/
/**
* Use the sensor type to perform the update step.
* Update the state and covariance matrices.
*/
if (measurement_pack.sensor_type_ == MeasurementPackage::RADAR) {
// Radar updates
H_jacobian = tools.CalculateJacobian(ekf_.x_);
ekf_.H_ = H_jacobian;
ekf_.R_ = R_radar_;
ekf_.UpdateEKF(measurement_pack.raw_measurements_);
}
else {
// Laser updates
ekf_.H_ = H_laser_;
ekf_.R_ = R_laser_;
ekf_.Update(measurement_pack.raw_measurements_);
}
Sources
This article follows the attribution requirements of Stack Overflow and is licensed under CC BY-SA 3.0.
Source: Stack Overflow
| Solution | Source |
|---|
