PlatformKalmanFilter.h

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    ArmarX
 * @author     Mirko Waechter( mirko.waechter at kit dot edu)
 * @date       2018
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
#pragma once
 
#include <memory>
 
#include <IceUtil/Time.h>
 
#include "KalmanFilter.h"
 
namespace memoryx
{
    /**
     * @brief This class is a convenience class for a holonomic platform using a Kalman filter.
     */
    class PlatformKalmanFilter
    {
    public:
        /**
         * @brief PlatformKalmanFilter
         * @param initialPosition Initial position in mm
         * @param initialRotation Initial rotation in rad
         * @param translationSensorStdDev Standard deviation of the position sensing in mm
         * @param rotationSensorStdDev Standard deviation of the orientation sensing in rad
         * @param translationVelocitySensorStdDev Standard deviation of the translation velocity sensing in mm. This value is scaled with the current velocity in the predict step.
         * @param rotationVelocitySensorStdDev Standard deviation of the rotation velocity sensing in rad. This value is scaled with the current velocity in the predict step.
         */
        PlatformKalmanFilter(Eigen::Vector2d initialPosition,
                             double initialRotation,
                             double translationSensorStdDev = 100.0,
                             double rotationSensorStdDev = 0.1,
                             double translationVelocitySensorStdDev = 1.,
                             double rotationVelocitySensorStdDev = 1.);
        /**
         * @brief Performs the predict-step of the Kalman filter.
         * @param velX translational velocity on x-axis of the platform in **robot coordinates**
         * @param velY translational velocity on y-axis of the platform in **robot coordinates**
         * @param velTheta rotational velocity on z-axis of the platform in **robot coordinates**
         * @param deltaT timestep between last and current velocity measurement, i.e. between last and current predict() call
         */
        void predict(double velX, double velY, double velTheta, const IceUtil::Time& deltaT);
 
        /**
         * @brief Performs the update-step of the Kalman filter.
         * @param x X-position of the platform in **global coordinates**
         * @param y Y-position of the platform in **global coordinates**
         * @param alpha Rotation of the platform in **global coordinates**
         */
        void update(double x, double y, double alpha);
 
        /**
         * @brief Global pose of the holonomic platform.
         * @return x,y,rotation
         */
        const Eigen::Vector3d& getPose() const;
 
        Eigen::Vector2d getPosition() const;
        double getOrientation() const;
 
        /**
         * @brief Covariance matrix of the current belief of the state
         */
        Eigen::Matrix3d getCovariance() const;
 
        const KalmanFilter& getFilter() const;
 
        /**
         * @brief Change the internal Kalman Filter. Only use if you know what you are doing.
         */
        void setFilter(const KalmanFilter& value);
 
    protected:
        Eigen::Vector4d transformPose(const Eigen::Vector3d& pose) const;
        Eigen::Vector3d inverseTransformPose(const Eigen::Vector4d& pose) const;
 
 
        KalmanFilter filter;
        Gaussian state;
        Gaussian motionNoise;
        Eigen::Vector3d pose;
    };
 
    using PlatformKalmanFilterPtr = std::shared_ptr<PlatformKalmanFilter>;
 
} // namespace memoryx