imu_state.h

Go to the documentation of this file.

/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2011, Willow Garage, Inc.
 *  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 Willow Garage, Inc. 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.
 *
 * Author: Sebastian Klose
 *********************************************************************/

#ifndef IMU_FILTER_IMU_STATE_H_
#define IMU_FILTER_IMU_STATE_H_

#include <iostream>

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <imu_filter/quaternion_helper.h>
#include <imu_filter/utils.h>

#define EARTH_GRAVITY   ( 9.80665 )
//#define       EARTH_GRAVITY   ( 9.79944 ) // menlo park

namespace imu_filter
{
        //* Class modelling the state of the IMU in the filter
        class ImuState
        {
                public:
                        static const size_t Dimension = 19;     
                        typedef Eigen::Matrix<double, Dimension, 1> StateVector;
                        typedef Eigen::Matrix<double, Dimension, Dimension> CovarianceMatrix;
                        ImuState() : 
                                orientation_( Eigen::Quaterniond::Identity() ), 
                                position_( Eigen::Vector3d::Zero() ), 
                                velocity_( Eigen::Vector3d::Zero() ), 
                                gyroBias_( Eigen::Vector3d::Zero() ), 
                                accBias_( Eigen::Vector3d::Zero() ),
                                gravity_( 0, 0, -EARTH_GRAVITY ),
                                covariance_( CovarianceMatrix::Zero() ),
                                gyroBiasRange_( Eigen::Vector3d( 0.01, 0.01, 0.01 ) ),
                                accBiasRange_( Eigen::Vector3d( 0.3, 0.3, 0.3 ) ),
                                body2ImuR_( Eigen::Quaterniond::Identity() ),
                                body2ImuT_( Eigen::Vector3d::Zero() ),
                                estimate_gravity_( true ),
                                estimate_gyro_bias_( true ),
                                estimate_acc_bias_( true ),
                                world2ImuPose_( Eigen::Affine3d::Identity() )
                        {
                        }

                        ImuState( const ImuState & other ) : 
                                orientation_( other.orientation_ ), 
                                position_( other.position_ ), 
                                velocity_( other.velocity_ ), 
                                gyroBias_( other.gyroBias_ ), 
                                accBias_( other.accBias_ ),
                                gravity_( other.gravity_ ),
                                covariance_( other.covariance_ ),
                                gyroBiasRange_( other.gyroBiasRange_ ),
                                accBiasRange_( other.accBiasRange_ ),
                                body2ImuR_( other.body2ImuR_ ),
                                body2ImuT_( other.body2ImuT_ )
                        {
                        }

                        ImuState & operator=( const ImuState & other )
                        {
                                orientation_    = other.orientation_;
                                position_               = other.position_;
                                velocity_               = other.velocity_;
                                gyroBias_               = other.gyroBias_;
                                accBias_                = other.accBias_;
                                gravity_                = other.gravity_;
                                covariance_             = other.covariance_;
                                gyroBiasRange_  = other.gyroBiasRange_;
                                accBiasRange_   = other.accBiasRange_;
                                body2ImuR_              = other.body2ImuR_;
                                body2ImuT_              = other.body2ImuT_;
                                return *this;
                        }

                        void update( const StateVector & updateVector )
                        {
                                orientation_.x() += updateVector[ 0 ];
                                orientation_.y() += updateVector[ 1 ];
                                orientation_.z() += updateVector[ 2 ];
                                orientation_.w() += updateVector[ 3 ];

                                orientation_.normalize();

                                position_ += updateVector.block<3, 1>( 4, 0 );
                                velocity_ += updateVector.block<3, 1>( 7, 0 );

                                if( estimate_gyro_bias_ )
                                        gyroBias_ += updateVector.block<3, 1>( 10, 0 );

                                if( estimate_acc_bias_ )
                                        accBias_  += updateVector.block<3, 1>( 13, 0 );

                                if( estimate_gravity_ )
                                        gravity_  += updateVector.block<3, 1>( 16, 0 );

                                // clamp the biases
                                gyroBias_[ 0 ] = utils::clamp( gyroBias_[ 0 ], -gyroBiasRange_[ 0 ], gyroBiasRange_[ 0 ] );
                                gyroBias_[ 1 ] = utils::clamp( gyroBias_[ 1 ], -gyroBiasRange_[ 1 ], gyroBiasRange_[ 1 ] );
                                gyroBias_[ 2 ] = utils::clamp( gyroBias_[ 2 ], -gyroBiasRange_[ 2 ], gyroBiasRange_[ 2 ] );

                                accBias_[ 0 ] = utils::clamp( accBias_[ 0 ], -accBiasRange_[ 0 ], accBiasRange_[ 0 ] );
                                accBias_[ 1 ] = utils::clamp( accBias_[ 1 ], -accBiasRange_[ 1 ], accBiasRange_[ 1 ] );
                                accBias_[ 2 ] = utils::clamp( accBias_[ 2 ], -accBiasRange_[ 2 ], accBiasRange_[ 2 ] );
                        }

                        void setParameter( const StateVector & vector )
                        {
                                orientation_.x() = vector[  0 ];
                                orientation_.y() = vector[  1 ];
                                orientation_.z() = vector[  2 ];
                                orientation_.w() = vector[  3 ];
                                position_[ 0 ]   = vector[  4 ];
                                position_[ 1 ]   = vector[  5 ];
                                position_[ 2 ]   = vector[  6 ];
                                velocity_[ 0 ]   = vector[  7 ];
                                velocity_[ 1 ]   = vector[  8 ];
                                velocity_[ 2 ]   = vector[  9 ];
                                gyroBias_[ 0 ]   = vector[ 10 ];
                                gyroBias_[ 1 ]   = vector[ 11 ];
                                gyroBias_[ 2 ]   = vector[ 12 ];
                                accBias_[ 0 ]    = vector[ 13 ];
                                accBias_[ 1 ]    = vector[ 14 ];
                                accBias_[ 2 ]    = vector[ 15 ];
                                gravity_[ 0 ]    = vector[ 16 ];
                                gravity_[ 1 ]    = vector[ 17 ];
                                gravity_[ 2 ]    = vector[ 18 ];
                        }

                        const Eigen::Quaterniond & orientation() const { return orientation_; }

                        Eigen::Quaterniond & orientation() { return orientation_; }

                        void setOrientation( const Eigen::Quaterniond & newOrientation ) 
                        { 
                                orientation_ = newOrientation; 
                        }

                        const Eigen::Vector3d & position() const { return position_; }

                        Eigen::Vector3d & position() { return position_; }

                        void setPosition( const Eigen::Vector3d & newPosition ) { position_ = newPosition; }

                        const Eigen::Vector3d & velocity() const { return velocity_; }
                        
                        Eigen::Vector3d & velocity() { return velocity_; }
                        
                        void setVelocity( const Eigen::Vector3d & newVelocity ) { velocity_ = newVelocity; }

                        const Eigen::Vector3d & gyroBias() const { return gyroBias_; }

                        Eigen::Vector3d & gyroBias() { return gyroBias_; }
                        
                        void setGyroBias( const Eigen::Vector3d & newGyroBias ) { gyroBias_ = newGyroBias; }

                        const Eigen::Vector3d & accBias() const { return accBias_; }
                        
                        Eigen::Vector3d & accBias() { return accBias_; }

                        void setAccBias( const Eigen::Vector3d & newAccBias ) { accBias_ = newAccBias; }

                        const Eigen::Vector3d & gravity() const { return gravity_; }

                        Eigen::Vector3d & gravity() { return gravity_; }

                        void setGravity( const Eigen::Vector3d & newGravity ) { gravity_ = newGravity; }

                        const CovarianceMatrix & covariance() const { return covariance_; } 
                        
                        CovarianceMatrix & covariance() { return covariance_; }

                        const Eigen::Quaterniond & body2ImuRotation() const { return body2ImuR_; }
                        
                        void setBody2ImuRotation( const Eigen::Quaterniond & rot ) { body2ImuR_ = rot; }

                        const Eigen::Vector3d & body2ImuTranslation() const { return body2ImuT_; }
                        
                        void setBody2ImuTranslation( const Eigen::Vector3d & trans ) { body2ImuT_ = trans; }

                        void setEstimateGravity( bool v ) { estimate_gravity_ = v; }
                        
                        void setEstimateGyroBias( bool v ) { estimate_gyro_bias_ = v; }
                        
                        void setEstimateAccBias( bool v ) { estimate_acc_bias_ = v; }
                        
                        void setGyroBiasRange( double rx, double ry, double rz )
                        {
                                gyroBiasRange_[ 0 ] = rx;
                                gyroBiasRange_[ 1 ] = ry;
                                gyroBiasRange_[ 2 ] = rz;
                        }

                        void setAccBiasRange( double rx, double ry, double rz )
                        {
                                accBiasRange_[ 0 ] = rx;
                                accBiasRange_[ 1 ] = ry;
                                accBiasRange_[ 2 ] = rz;
                        }
                        
                        void toVector( StateVector & vec ) const
                        {
                                vec[ 0 ] = orientation_.x();
                                vec[ 1 ] = orientation_.y();
                                vec[ 2 ] = orientation_.z();
                                vec[ 3 ] = orientation_.w();

                                vec[ 4 ] = position_.x();
                                vec[ 5 ] = position_.y();
                                vec[ 6 ] = position_.z();

                                vec[ 7 ] = velocity_.x();
                                vec[ 8 ] = velocity_.y();
                                vec[ 9 ] = velocity_.z();

                                vec[ 10 ] = gyroBias_.x();
                                vec[ 11 ] = gyroBias_.y();
                                vec[ 12 ] = gyroBias_.z();

                                vec[ 13 ] = accBias_.x();
                                vec[ 14 ] = accBias_.y();
                                vec[ 15 ] = accBias_.z();

                                vec[ 16 ] = gravity_.x();
                                vec[ 17 ] = gravity_.y();
                                vec[ 18 ] = gravity_.z();
                        }

                        void fixCovariance()
                        {
                                // gyroBias
                                if( !estimate_gyro_bias_ ){
                                        covariance_.block<3, 3>( 10, 10 ).setZero();
                                }

                                // accBias
                                if( !estimate_acc_bias_ ){
                                        covariance_.block<3, 3>( 13, 13 ).setZero();
                                }

                                // gravity
                                if( !estimate_gravity_ ){
                                        covariance_.block<3, 3>( 16, 16 ).setZero();
                                }
                        }

                        const Eigen::Affine3d & world2Imu() 
                        {
                                world2ImuPose_.linear() = orientation_.toRotationMatrix();
                                world2ImuPose_.translation() = position_;
                                return world2ImuPose_;
                        }

                private:
                        // orientation relative to base frame
                        Eigen::Quaterniond      orientation_;

                        // position relative to base frame
                        Eigen::Vector3d         position_;

                        // velocity relative to base frame
                        Eigen::Vector3d         velocity_;

                        // bias in accelerometer measurements 
                        Eigen::Vector3d         gyroBias_;

                        // bias in accelerometer measurements 
                        Eigen::Vector3d         accBias_;

                        // gravity vector 
                        Eigen::Vector3d         gravity_;

                        // the state covariance matrix
                        CovarianceMatrix        covariance_;

                        // ranges for the biases
                        Eigen::Vector3d         gyroBiasRange_;
                        Eigen::Vector3d         accBiasRange_;

                        // calibration data
                        Eigen::Quaterniond      body2ImuR_;
                        Eigen::Vector3d         body2ImuT_;

                        bool estimate_gravity_;
                        bool estimate_gyro_bias_;
                        bool estimate_acc_bias_;

                        Eigen::Affine3d         world2ImuPose_;
        };

        inline std::ostream & operator<<( std::ostream & os, const ImuState & state )
        {
                os << "ImuState:\n"
                        << "\torientation: [" 
                        << state.orientation().w() << ", " 
                        << state.orientation().x() << ", " 
                        << state.orientation().y() << ", " 
                        << state.orientation().z() << " ]\n" 
                        << "\tposition: [" 
                        << state.position().x() << ", " 
                        << state.position().y() << ", " 
                        << state.position().z() << " ]\n" 
                        << "\tvelocity: [" 
                        << state.velocity().x() << ", " 
                        << state.velocity().y() << ", " 
                        << state.velocity().z() << " ]\n" 
                        << "\tgyroBias: [" 
                        << state.gyroBias().x() << ", " 
                        << state.gyroBias().y() << ", " 
                        << state.gyroBias().z() << " ]\n" 
                        << "\taccBias: [" 
                        << state.accBias().x() << ", " 
                        << state.accBias().y() << ", " 
                        << state.accBias().z() << " ]\n" 
                        << "\tgravity: [" 
                        << state.gravity().x() << ", " 
                        << state.gravity().y() << ", " 
                        << state.gravity().z() << " ] (Norm: " << state.gravity().norm() << " )\n" << std::endl;
                return os;      
        }

}
#endif