Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include "KalmanFilter.h"
00038
00039 KalmanFilter::KalmanFilter(int dim)
00040 : meas_dim_(dim), state_dim_(dim * 2), G_(dim * 2), G_small_(dim), a_max_(0) {
00041 }
00042
00043 KalmanFilter::KalmanFilter(const KalmanFilter& orig)
00044 : meas_dim_(orig.meas_dim_), state_dim_(orig.state_dim_), G_(orig.G_), G_small_(orig.G_small_), H_(orig.H_), a_max_(orig.a_max_) {
00045 }
00046
00047 KalmanFilter::~KalmanFilter() {
00048 }
00049
00050 KalmanFilter* KalmanFilter::clone() const {
00051 return new KalmanFilter(*this);
00052 }
00053
00054 void KalmanFilter::init(const pbl::Gaussian& z) {
00055 H_ = arma::eye(meas_dim_, state_dim_);
00056
00057 G_.setMean(H_.t() * z.getMean());
00058 G_.setCovariance(H_.t() * z.getCovariance() * H_);
00059
00060 G_small_.setMean(z.getMean());;
00061 G_small_.setCovariance(z.getCovariance());;
00062 }
00063
00064 void KalmanFilter::propagate(const double& dt) {
00065 if (a_max_ > 0) {
00066
00067
00068
00069
00070 pbl::Matrix F = arma::eye(state_dim_, state_dim_);
00071 for(int i = 0; i < meas_dim_; ++i) {
00072 F(i, i + meas_dim_) = dt;
00073 }
00074
00075 pbl::Vector x = G_.getMean();
00076
00077 for(int i = 0; i < meas_dim_; ++i) {
00078 x(i) += x(i + meas_dim_) * dt;
00079 }
00080
00081 G_.setMean(x);
00082
00083
00084 double q = a_max_ * a_max_ / 4;
00085 double dt2 = dt * dt;
00086 double dt4 = dt2 * dt2;
00087
00088 pbl::Matrix P = F * G_.getCovariance() * F.t();
00089 for(int i = 0; i < meas_dim_; ++i) {
00090 P(i, i) += dt4 / 4 * q;
00091 P(i, i + meas_dim_) += dt4 / 4 * q;
00092 P(i + meas_dim_, i + meas_dim_) += dt2 * q;
00093 }
00094
00095 G_.setCovariance(P);
00096
00097 G_small_.setMean(H_ * G_.getMean());
00098 G_small_.setCovariance(H_ * G_.getCovariance() * H_.t());
00099 }
00100 }
00101
00102 void KalmanFilter::update(const pbl::Gaussian& z) {
00103 const pbl::Vector& x = G_.getMean();
00104 const pbl::Matrix& P = G_.getCovariance();
00105
00106
00107 pbl::Vector y = z.getMean() - H_ * x;
00108
00109
00110 pbl::Matrix S = H_ * P * H_.t() + z.getCovariance();
00111
00112
00113 pbl::Matrix K = P * H_.t() * inv(S);
00114
00115
00116 G_.setMean(x + K * y);
00117
00118
00119 G_.setCovariance((arma::eye(state_dim_, state_dim_) - K * H_) * P);
00120
00121 G_small_.setMean(H_ * G_.getMean());
00122 G_small_.setCovariance(H_ * G_.getCovariance() * H_.t());
00123 }
00124
00125 double KalmanFilter::getLikelihood(const pbl::Gaussian& z) const {
00126 return z.getDensity(G_small_);
00127 }
00128
00129 const pbl::Gaussian& KalmanFilter::getGaussian() const {
00130 return G_small_;
00131 }
00132
00133 const pbl::Vector& KalmanFilter::getState() const {
00134 return G_.getMean();
00135 }
00136
00137 const pbl::Matrix& KalmanFilter::getStateCovariance() const {
00138 return G_.getCovariance();
00139 }
00140
00141 void KalmanFilter::setMaxAcceleration(double a_max) {
00142 a_max_ = a_max;
00143 }