KFBase.hpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, LABUST, UNIZG-FER
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * 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 provided
00016  *     with the distribution.
00017  *   * Neither the name of the LABUST nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 #ifndef KFBASE_HPP_
00035 #define KFBASE_HPP_
00036 #include <stdexcept>
00037 
00038 namespace labust
00039 {
00040         namespace navigation
00041         {
00045                 template <class Model>
00046                 class KFBase : public Model
00047                 {
00048                 public:
00049                         typedef const typename Model::matrix& matrixref;
00050                         typedef const typename Model::vector& vectorref;
00051                         //For MIMO systems
00052                         typedef const typename Model::input_type& inputref;
00053                         typedef const typename Model::output_type& outputref;
00054 
00058                         KFBase(): P(Model::matrix::Identity(Model::stateNum, Model::stateNum)),
00059                                         outlierR(1.0){};
00060 
00066                         inline void setStateCovariance(matrixref P){this->P = P;};
00072                         inline matrixref getStateCovariance(){return this->P;};
00078                         inline matrixref getInovationCovariance(){return this->innovationCov;};
00084                         inline void setState(vectorref x){this->xk_1 = x; this->x = x;};
00088       void setMeasurementParameters(const typename Model::matrix& V,
00089                 const typename Model::matrix& R)
00090       {
00091         this->V = V; this->R = R;
00092       }
00096       void setStateParameters(const typename Model::matrix& W,
00097                 const typename Model::matrix& Q)
00098       {
00099         this->W = W; this->Q = Q;
00100       }
00104       void setOutlierR(double outlierR)
00105       {
00106         this->outlierR = outlierR;
00107       }
00113                         inline vectorref getState(){return this->x;};
00119                         inline void setTs(double Ts)
00120                         {
00121                                 if (!Ts) throw std::invalid_argument("Cannot set zero sampling time.");
00122                                 this->Ts = Ts;
00123                                 this->initModel();
00124                         };
00125 
00131                         inline double traceP()
00132                         {
00133                                 return this->P.trace();
00134                         }
00135 
00136                 protected:
00140                         typename Model::matrix K, P, innovationCov;
00144                         typename Model::vector innovation;
00148                         double outlierR;
00149                 };
00150         }
00151 }
00152 
00153 /* KFBASE_HPP_ */
00154 #endif


labust_navigation
Author(s): Gyula Nagy
autogenerated on Fri Aug 28 2015 11:23:33