$search
00001 /************************************************************************ 00002 * Copyright (C) 2012 Eindhoven University of Technology (TU/e). * 00003 * All rights reserved. * 00004 ************************************************************************ 00005 * Redistribution and use in source and binary forms, with or without * 00006 * modification, are permitted provided that the following conditions * 00007 * are met: * 00008 * * 00009 * 1. Redistributions of source code must retain the above * 00010 * copyright notice, this list of conditions and the following * 00011 * disclaimer. * 00012 * * 00013 * 2. Redistributions in binary form must reproduce the above * 00014 * copyright notice, this list of conditions and the following * 00015 * disclaimer in the documentation and/or other materials * 00016 * provided with the distribution. * 00017 * * 00018 * THIS SOFTWARE IS PROVIDED BY TU/e "AS IS" AND ANY EXPRESS OR * 00019 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * 00020 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * 00021 * ARE DISCLAIMED. IN NO EVENT SHALL TU/e OR CONTRIBUTORS BE LIABLE * 00022 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * 00023 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT * 00024 * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; * 00025 * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF * 00026 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * 00027 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE * 00028 * USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH * 00029 * DAMAGE. * 00030 * * 00031 * The views and conclusions contained in the software and * 00032 * documentation are those of the authors and should not be * 00033 * interpreted as representing official policies, either expressed or * 00034 * implied, of TU/e. * 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 // const pbl::Vector& x = G_.getMean(); 00067 // const pbl::Matrix& P = G_.getCovariance(); 00068 00069 // set state transition matrix 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 // // set system noise 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; // cov pos 00091 P(i, i + meas_dim_) += dt4 / 4 * q; // cov pos~vel 00092 P(i + meas_dim_, i + meas_dim_) += dt2 * q; // cov vel 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 // determine innovation 00107 pbl::Vector y = z.getMean() - H_ * x; 00108 00109 // determine innovation covariance 00110 pbl::Matrix S = H_ * P * H_.t() + z.getCovariance(); 00111 00112 // calculate optimal Kalman gain 00113 pbl::Matrix K = P * H_.t() * inv(S); 00114 00115 // update state 00116 G_.setMean(x + K * y); 00117 00118 // update state covariance 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 }