PlatformKalmanFilter.cpp

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
 */
#include "PlatformKalmanFilter.h"
 
#include <ArmarXCore/core/logging/Logging.h>
 
namespace memoryx
{
 
    PlatformKalmanFilter::PlatformKalmanFilter(Eigen::Vector2d initialPosition,
                                               double initialRotation,
                                               double translationSensorStdDev,
                                               double rotationSensorStdDev,
                                               double translationVelocitySensorStdDev,
                                               double rotationVelocitySensorStdDev)
    {
        pose.head(2) = initialPosition;
        pose(2) = initialRotation;
        Eigen::Vector4d motionNoiseCov;
        motionNoiseCov << translationVelocitySensorStdDev * translationVelocitySensorStdDev,
            translationVelocitySensorStdDev * translationVelocitySensorStdDev,
            rotationVelocitySensorStdDev * rotationVelocitySensorStdDev,
            rotationVelocitySensorStdDev * rotationVelocitySensorStdDev;
        motionNoise = Gaussian(Eigen::Vector4d::Zero(), motionNoiseCov.asDiagonal());
        filter.setMotionModel(
            Eigen::Matrix4d::Identity(), Eigen::Matrix4d::Identity(), motionNoise);
 
        Eigen::Vector4d measurementNoiseCov;
        measurementNoiseCov << translationSensorStdDev * translationSensorStdDev,
            translationSensorStdDev * translationSensorStdDev,
            rotationSensorStdDev * rotationSensorStdDev,
            rotationSensorStdDev * rotationSensorStdDev;
        Gaussian measurementNoise(Eigen::Vector4d::Zero(), measurementNoiseCov.asDiagonal());
        filter.setMeasurementModel(Eigen::Matrix4d::Identity(), measurementNoise);
 
        Eigen::Vector4d pose2d = transformPose(pose);
 
        state = Gaussian(4);
        state.setMean(pose2d);
        state.setCovariance(Eigen::Vector4d(translationSensorStdDev * translationSensorStdDev * 10,
                                            translationSensorStdDev * translationSensorStdDev * 10,
                                            rotationSensorStdDev * rotationSensorStdDev * 10,
                                            rotationSensorStdDev * rotationSensorStdDev * 10)
                                .asDiagonal());
    }
 
    void
    PlatformKalmanFilter::predict(double velX,
                                  double velY,
                                  double velTheta,
                                  const IceUtil::Time& deltaT)
    {
        Eigen::Matrix2d rotation = Eigen::Rotation2Dd(pose(2)).matrix();
        Eigen::Vector2d globalV0 = rotation * Eigen::Vector2d(velX, velY);
        Eigen::Vector4d currentVelocity;
        currentVelocity << globalV0(0), globalV0(1), -velTheta * sin(pose(2)),
            velTheta * cos(pose(2));
 
 
        auto posDelta = currentVelocity * deltaT.toSecondsDouble();
 
        // scale motion noise with velocity
        Eigen::MatrixXd adjustedMotionNoiseCov =
            motionNoise.getCovariance() * Eigen::Vector4d(std::abs(currentVelocity(0)),
                                                          std::abs(currentVelocity(1)),
                                                          std::abs(velTheta),
                                                          std::abs(velTheta))
                                              .asDiagonal();
 
        Gaussian adjustedMotionNoise(Eigen::Vector4d::Zero(), adjustedMotionNoiseCov);
        filter.setMotionModel(
            Eigen::Matrix4d::Identity(), Eigen::Matrix4d::Identity(), adjustedMotionNoise);
 
 
        state = filter.predict(state, posDelta);
        pose = inverseTransformPose(state.getMean());
    }
 
    void
    PlatformKalmanFilter::update(double x, double y, double alpha)
    {
        Eigen::Vector4d pose2d = transformPose(Eigen::Vector3d(x, y, alpha));
 
 
        state = filter.update(state, pose2d);
 
        pose = inverseTransformPose(state.getMean());
    }
 
    const Eigen::Vector3d&
    PlatformKalmanFilter::getPose() const
    {
        return pose;
    }
 
    Eigen::Vector2d
    PlatformKalmanFilter::getPosition() const
    {
        return pose.head<2>();
    }
 
    double
    PlatformKalmanFilter::getOrientation() const
    {
        return pose(2);
    }
 
    Eigen::Matrix3d
    PlatformKalmanFilter::getCovariance() const
    {
        Eigen::Matrix3d result;
        result.block<2, 2>(0, 0) = state.getCovariance().block<2, 2>(0, 0);
        result(2, 2) = (state.getCovariance()(2, 2) + state.getCovariance()(3, 3)) * 0.5;
        return result;
    }
 
    const KalmanFilter&
    PlatformKalmanFilter::getFilter() const
    {
        return filter;
    }
 
    void
    PlatformKalmanFilter::setFilter(const KalmanFilter& value)
    {
        filter = value;
    }
 
    Eigen::Vector4d
    PlatformKalmanFilter::transformPose(const Eigen::Vector3d& pose) const
    {
        Eigen::Vector4d pose2d;
        // rotation stored for Mean_of_circular_quantities
        // http://en.wikipedia.org/wiki/Mean_of_circular_quantities
        pose2d << pose(0), pose(1), cos(pose(2)), sin(pose(2));
        return pose2d;
    }
 
    Eigen::Vector3d
    PlatformKalmanFilter::inverseTransformPose(const Eigen::Vector4d& pose) const
    {
        Eigen::Matrix3d ori;
        ori.setIdentity();
 
        double angle = atan2(pose(3), pose(2));
        ori = Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitZ());
        Eigen::Vector3d result;
        result[0] = pose(0);
        result[1] = pose(1);
        result[2] = angle;
        return result;
    }
 
} // namespace memoryx