KFCore.hpp

Go to the documentation of this file.

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2010, LABUST, UNIZG-FER
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the LABUST nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/
#ifndef KFCORE_HPP_
#define KFCORE_HPP_
#include <labust/navigation/KFBase.hpp>

#include <vector>
#include <cassert>

#include <iostream>
#include <ros/ros.h>

namespace labust
{
        namespace navigation
        {
                template <class Model>
                class KFCore : public KFBase< Model >
                {
                        typedef KFBase< Model > Base;
                public:
                        KFCore(){};

                        void predict(typename Base::inputref u = typename Model::input_type())
                        {
                                assert((this->Ts) && "KFCore: Sampling time set to zero." );
                                this->step(u);
                                this->P = this->A*this->P*this->A.transpose() +
                                                //this can be optimized for constant noise models
                                                this->W*this->Q*this->W.transpose();
                        }
                        typename Base::vectorref correct(typename Base::outputref y_meas)
                        {
                                //ros::Time start(ros::Time::now());
                                typename Base::matrix PH = this->P*this->H.transpose();
                                this->innovationCov = this->H*PH;
                                typename Base::matrix VR = this->V*this->R;
                                //This can be optimized for constant R, V combinations
                                this->innovationCov += VR*this->V.transpose();
                                //this->K = PH*this->innovationCov.inverse();
                                //TODO Add option to select solvers based on size for larger matrices this should be faster
                                this->K = PH*this->innovationCov.ldlt().solve(
                                                                Base::matrix::Identity(this->innovationCov.rows(),
                                                                                                this->innovationCov.cols()));
                                /*
                                 * Correct the state estimate
                                 *
                                 * x(k) = x_(k) + K(k)*(measurement - y(k));
                                 */
                                typename Base::output_type y;
                                this->estimate_y(y);
                                this->innovation = y_meas - y;
                                this->x += this->K*this->innovation;
                                typename Base::matrix IKH = Base::matrix::Identity(this->P.rows(), this->P.cols());
                                IKH -= this->K*this->H;
                                this->P = IKH*this->P;

                                //ros::Time end(ros::Time::now());

                                //ROS_INFO("KF: Total elapsed time: %f", (start-end).toSec());

                                return this->xk_1 = this->x;
                        }

                        template <class NewMeasVector>
                        typename Base::vectorref correct(
                                        typename Base::outputref measurements,
                                        NewMeasVector& newMeas,
                                        bool reject_outliers = true)
                        {
                                //Check invariant.
                                assert(measurements.size() == Model::stateNum &&
                                                newMeas.size() == Model::stateNum);

                                std::vector<size_t> arrived;
                                std::vector<typename Model::numericprecission> dataVec;

                                for (size_t i=0; i<newMeas.size(); ++i)
                                {
                                        //Outlier rejection
                                        if (newMeas(i) && reject_outliers)
                                        {
                                                double dist=fabs(this->x(i) - measurements(i));
                                                newMeas(i) = (dist <= sqrt(this->P(i,i)) + sqrt(this->R0(i,i)));

                                                if (!newMeas(i))
                                                {
                                                        std::cerr<<"Outlier rejected: x(i)="<<this->x(i);
                                                        std::cerr<<", m(i)="<<measurements(i);
                                                        std::cerr<<", r(i)="<<sqrt(this->P(i,i)) + sqrt(this->R0(i,i));
                                                }
                                        }

                                        if (newMeas(i))
                                        {
                                                arrived.push_back(i);
                                                dataVec.push_back(measurements(i));
                                                newMeas(i) = 0;
                                        }
                                }

                                typename Base::output_type y_meas(arrived.size());
                                this->H = Model::matrix::Zero(arrived.size(),Model::stateNum);
                                this->V = Model::matrix::Zero(arrived.size(),Model::stateNum);

                                for (size_t i=0; i<arrived.size();++i)
                                {
                                        y_meas(i) = dataVec[i];
                                        this->H.row(i) = this->H0.row(arrived[i]);
                                        this->V.row(i) = this->V0.row(arrived[i]);
                                }

                                return this->correct(y_meas);
                        }
                };
        };
}
/* KFCORE_HPP_ */
#endif