Estimating the vehicle location.
Estimating the vehicle location.
In this example, we would like to estimate the location of the vehicle in the XY plane. The vehicle has an onboard location sensor that reports X and Y coordinates of the system. We assume constant acceleration dynamics. In this example we don't have a control variable u since we don't have control input. Let us assume a vehicle moving in a straight line in the X direction with a constant velocity. After traveling 400 meters the vehicle turns right, with a turning radius of 300 meters. During the turning maneuver, the vehicle experiences acceleration due to the circular motion (an angular acceleration). The measurements period: Δt = 1s (constant). The random acceleration standard deviation: σa = 0.2 m.s^-2.
#include "fcarouge/linalg.hpp"
#include <cassert>
#include <cmath>
template <typename Numerator, fcarouge::algebraic Denominator>
return rhs.transpose()
.fullPivHouseholderQr()
.solve(lhs.transpose())
.transpose();
}
namespace {
kalman filter;
filter.x(0., 0., 0., 0., 0., 0.);
{0., 500., 0., 0., 0., 0.},
{0., 0., 500., 0., 0., 0.},
{0., 0., 0., 500., 0., 0.},
{0., 0., 0., 0., 500., 0.},
{0., 0., 0., 0., 0., 500.}});
{0.25, 0.5, 0.5, 0., 0., 0.}, {0.5, 1., 1., 0., 0., 0.},
{0.5, 1., 1., 0., 0., 0.}, {0., 0., 0., 0.25, 0.5, 0.5},
{0., 0., 0., 0.5, 1., 1.}, {0., 0., 0., 0.5, 1., 1.}};
q *= 0.2 * 0.2;
filter.q(q);
{0., 1., 1., 0., 0., 0.},
{0., 0., 1., 0., 0., 0.},
{0., 0., 0., 1., 1., 0.5},
{0., 0., 0., 0., 1., 1.},
{0., 0., 0., 0., 0., 1.}});
filter.predict();
filter.h(
kalman::output_model{{1., 0., 0., 0., 0., 0.}, {0., 0., 0., 1., 0., 0.}});
filter.update(-393.66, 300.4);
filter.predict();
filter.predict();
}};
assert(std::abs(1 - filter.x()[0] / -277.8) < 0.001 &&
std::abs(1 - filter.x()[1] / 148.3) < 0.001 &&
std::abs(1 - filter.x()[2] / 94.5) < 0.001 &&
std::abs(1 - filter.x()[3] / 249.8) < 0.001 &&
std::abs(1 - filter.x()[4] / -85.9) < 0.001 &&
std::abs(1 - filter.x()[5] / -63.6) < 0.001 &&
"The state estimates expected at 0.1% accuracy.");
assert(std::abs(1 - filter.x()[0] / 298.5) < 0.006 &&
std::abs(1 - filter.x()[1] / -1.65) < 0.006 &&
std::abs(1 - filter.x()[2] / -1.9) < 0.006 &&
std::abs(1 - filter.x()[3] / -22.5) < 0.006 &&
std::abs(1 - filter.x()[4] / -26.1) < 0.006 &&
std::abs(1 - filter.x()[5] / -0.64) < 0.006 &&
"The state estimates expected at 0.6% accuracy.");
assert(std::abs(1 - filter.p()(0, 0) / 11.25) < 0.001 &&
std::abs(1 - filter.p()(0, 1) / 4.5) < 0.001 &&
std::abs(1 - filter.p()(0, 2) / 0.9) < 0.001 &&
std::abs(1 - filter.p()(1, 1) / 2.4) < 0.001 &&
std::abs(1 - filter.p()(2, 2) / 0.2) < 0.001 &&
std::abs(1 - filter.p()(3, 3) / 11.25) < 0.001 &&
"The estimate uncertainty expected at 0.1% accuracy."
"At this point, the position uncertainty px = py = 5, which means "
"that the standard deviation of the prediction is square root of 5m.");
return 0;
}()};
}
}
implementation::process_uncertainty process_uncertainty
Type of the process noise correlated variance matrix Q.
implementation::estimate_uncertainty estimate_uncertainty
Type of the estimated correlated variance matrix P.
implementation::output_uncertainty output_uncertainty
Type of the observation noise correlated variance matrix R.
implementation::state_transition state_transition
Type of the state transition matrix F.
The Kalman filter class and library top-level header.
Examples, tutorials, demonstrators of the library.
Eigen::Vector< Type, Row > column_vector
Compile-time sized Eigen3 column vector.
constexpr auto operator/(const Numerator &lhs, const Denominator &rhs) -> quotient< Numerator, Denominator >
A user-definable algebraic division solution.