37 #ifndef SE_KALMANFILTER_H_ 38 #define SE_KALMANFILTER_H_ const pbl::Matrix & getStateCovariance() const
Returns the covariance of the Kalman state.
double getLikelihood(const pbl::Gaussian &z) const
Calculates the likelihood that the given measurement originates from the estimated Kalman state...
void setMaxAcceleration(double a_max)
Set the maximum expected acceleration. This parameter is used to determine the system noise in the pr...
KalmanFilter(int dim)
Constructs a Kalman filter with specified state dimensionality.
const pbl::Vector & getState() const
Returns the Kalman state (mean of the estimated Gaussian)
Kalman filter with constant-velocity system model. The system noise is automatically calculated from ...
virtual void init(const pbl::Gaussian &G)
Initializes the Kalman filter according to the given Gaussian, i.e., the initial state and state cova...
virtual ~KalmanFilter()
Destructor.
TFSIMD_FORCE_INLINE const tfScalar & z() const
virtual void propagate(const double &dt)
Propagates the state of the Kalman filter according to the (constant- velocity) system model...
const pbl::Gaussian & getGaussian() const
Returns the Kalman state and covariance as Gaussian.
void update(const pbl::Gaussian &z)
Updates the Kalman filter with the given (Gaussian) measurement.
virtual KalmanFilter * clone() const
Clone operator.