KinematicModel.cpp
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 #include <labust/navigation/KinematicModel.hpp>
00035 
00036 using namespace labust::navigation;
00037 
00038 KinematicModel::KinematicModel(){this->initModel();};
00039 
00040 KinematicModel::~KinematicModel(){};
00041 
00042 void KinematicModel::initModel()
00043 {
00044   x = Base::vector::Zero(stateNum);
00045   //Setup the transition matrix
00046   A = Base::matrix::Identity(stateNum,stateNum);
00047 
00048   A(xp,Vv) = Ts*std::cos(x(psi));
00049   A(yp,Vv) = Ts*std::sin(x(psi));
00050   A(psi,r) = Ts;
00051   A(r,r) = 0.9;
00052   W = Base::matrix::Identity(stateNum,stateNum);
00053 
00054   //These are the noise variances
00055   vector q(stateNum);
00056   q<<std::pow(0.02,2), //x
00057   std::pow(0.02,2), //y
00058   std::pow(0.07,2), //u
00059   std::pow(0.01,2), //psi
00060   std::pow(0.1,2); //r
00061   Q = q.asDiagonal();
00062   H = Base::matrix::Identity(inputSize,stateNum);
00063   V = Base::matrix::Identity(inputSize,inputSize);
00064   R = 16*Base::matrix::Identity(inputSize,inputSize);
00065 }
00066 
00067 void KinematicModel::step(const input_type& input)
00068 {
00069   //This model is already discrete and we update only the interesting parts
00070   A(xp,Vv) = Ts*std::cos(x(psi));
00071   A(yp,Vv) = Ts*std::sin(x(psi));
00072   A(xp,psi) = 0;
00073   A(yp,psi) = 0;
00074 
00075   //Propagate state
00076   //x(Vv) = 0.5;
00077   //x(r) = 0;
00078   x = A*x;
00079   //x(r) = 0;
00080   //x(Vv) = 0.5;
00081 
00082   //Linearize the matrix for
00083   A(xp,psi) = -Ts*x(Vv)*sin(x(psi));
00084   A(yp,psi) = Ts*x(Vv)*cos(x(psi));
00085 };
00086 
00087 void KinematicModel::estimate_y(output_type& y)
00088 {
00089   y=H*x;
00090 }
00091 
00092 void KinematicModel::calculateXYInovationVariance(const KinematicModel::matrix& P,
00093                 double& xin,double &yin)
00094 {
00095         xin = sqrt(P(xp,xp)) + sqrt(R(xp,xp));
00096         yin = sqrt(P(yp,yp)) + sqrt(R(yp,yp));
00097 }


labust_navigation
Author(s): Gyula Nagy
autogenerated on Fri Feb 7 2014 11:36:19