imu_ekf.cpp

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
 *********************************************************************/
#include <imu_filter/imu_ekf.h>
#include <imu_filter/quaternion_helper.h>
#include <imu_filter/marker_visualization.h>

#include <imu_filter/debug_helper.h>
#include <imu_filter/imu_tools.h>

namespace imu_filter 
{

        ImuEkf::ImuEkf() :
                rng_( time( NULL ) ),
                F_( Eigen::Matrix<double, 19, 19>::Identity() ),
                G_( Eigen::Matrix<double, 19, 12>::Zero() ),
                Q_( Eigen::Matrix<double, 12, 12>::Zero() ),
                velocity_damping_( 1.0 )
        {
                G_.block<3, 3>(  9, 6 ) = Eigen::Matrix3d::Identity();
                G_.block<3, 3>( 12, 9 ) = Eigen::Matrix3d::Identity();
        }

        ImuEkf::~ImuEkf()
        {
        }

        void ImuEkf::propagate( const ImuMeasurement & measurement, double deltaT )
        {
                Eigen::Quaterniond qNew;
                Eigen::Vector3d posNew, velNew;

                propagateOrientation( qNew, state_, measurement, deltaT );
                propagateTranslation( posNew, qNew, state_, measurement, deltaT );
                propagateVelocity( velNew, qNew, state_, measurement, deltaT );
                state_.gyroBias() += measurement.gyroBiasPerturbation;
                state_.accBias()  += measurement.accBiasPerturbation;

                // compute the jacobians numerically
                numericalJacobians( state_, measurement, deltaT, F_, G_ );

                updateProcessNoise( measurement, deltaT );
                propagateCovariance();

                state_.setOrientation( qNew );
                state_.setPosition( posNew );
                state_.setVelocity( velNew );


                MarkerVisualization & visualizer = MarkerVisualization::instance();
                visualizer.addImuMarker( state_, measurement.linearAcceleration );
                visualizer.publish();
        }

        void ImuEkf::updateProcessNoise( const ImuMeasurement & measurement, double deltaT )
        {
                Q_.block<3, 3>( 0, 0 ) = measurement.angularRateCovariance;
                Q_.block<3, 3>( 3, 3 ) = measurement.linearAccelerationCovariance;

                // omega bias noise
                Q_.block<3, 3>( 6, 6 ) = measurement.gyroBiasCovariance * deltaT;

                // acc bias noise
                Q_.block<3, 3>( 9, 9 ) = measurement.accBiasCovariance * deltaT;
        }

        void ImuEkf::propagateCovariance()
        {
                state_.covariance() = F_ * state_.covariance() * F_.transpose() + G_ * Q_ * G_.transpose();
        }

        void ImuEkf::correction( const Observation & observation )
        {
                // predicted measurement is the attitude and position from IMU propagation
                // calculate the error vector:
                const Observation::Vector & obs = observation.vector();
                Eigen::Quaterniond qObs;
                qObs.coeffs() = obs.head<4>();

                normalizeOrientationObservation( qObs );

                measResidual_.head<4>() = qObs.coeffs() - state_.orientation().coeffs();
                measResidual_.tail<3>() = obs.tail<3>() - state_.position();

                const Observation::JacMat & measJac = observation.jacobian();

                // S = H*P^^*H^T + R
                Eigen::Matrix<double, 7, 7> S = measJac * state_.covariance() * measJac.transpose() + observation.covariance();

                Eigen::Matrix<double, 7, 7> SInv = S.inverse(); 

                kalmanGain_ = state_.covariance() * measJac.transpose() * SInv;

                // calculate the state update:
                stateUpdate_ = kalmanGain_ * measResidual_;

                if( stateUpdate_.head<4>().norm() > 0.5 ){
                        std::cout << "\n\n\nSUSPICIOUS!!!!!\n" << std::endl;
                }

                state_.update( stateUpdate_ );

                // covariance update:
                covarianceUpdate_ = kalmanGain_ * measJac; 
                state_.covariance() -= covarianceUpdate_ * state_.covariance();

                //  fix those parts which are not estimated atm.
                state_.fixCovariance();
        }


        void ImuEkf::initialize( const Eigen::Quaterniond & rot, 
                                                         const Eigen::Vector3d & pos,
                                                         const Eigen::Quaterniond & calibRot, 
                                                         const Eigen::Vector3d & calibPos,
                                                         const Eigen::Vector3d & measuredAcceleration,
                                                         const ImuState::CovarianceMatrix & covMat,
                                                         bool constrainGravity )
        {
                // update the extrinsics
                setExtrinsicTransform( calibRot, calibPos );

                // initial rotation is the world rotation of the body * calibration rotation
                state_.setOrientation( rot * calibRot );
                state_.setPosition( rot * calibPos + pos );

                state_.covariance() = covMat;

                // constrain norm of gravity:
                Eigen::Vector3d gravity, accBias;
                if( constrainGravity ){
                        double s = sqrt( utils::square( EARTH_GRAVITY ) / measuredAcceleration.squaredNorm() );
                        gravity = -s * measuredAcceleration;
                } else {
                        gravity = -measuredAcceleration;
                }

                // bias is in imu frame: measured acc in imu frame compensated for gravity (in imu frame)
                accBias = measuredAcceleration + gravity;       
                state_.setAccBias( accBias );

                // gravity is in world frame
                gravity = state_.orientation() * gravity;
                state_.setGravity( gravity );

                state_.setGyroBias( Eigen::Vector3d::Zero() );
                state_.setVelocity( Eigen::Vector3d::Zero() );
        }


        void ImuEkf::setExtrinsicTransform( const Eigen::Quaterniond & rot, const Eigen::Vector3d & trans )
        {
                state_.setBody2ImuRotation( rot );
                state_.setBody2ImuTranslation( trans );
        }


        void ImuEkf::objectTransformation( Eigen::Quaterniond & rot, Eigen::Vector3d & trans, Eigen::Vector3d & grav )
        {
                // body orientation
                rot = state_.body2ImuRotation().inverse() * state_.orientation();

                // body position
                trans = state_.body2ImuRotation().inverse() * ( state_.position() - state_.body2ImuTranslation() );

                // gravity
                grav = state_.body2ImuRotation().inverse() * state_.gravity();
        }

        void ImuEkf::propagateOrientation( Eigen::Quaterniond & qnew,
                                                                          const ImuState & state,
                                                                          const ImuMeasurement & measurement,
                                                                          double deltaT )
        {
                Eigen::Vector3d truePerturbation;
                truePerturbation = deltaT * ( measurement.angularRate - state.gyroBias() + measurement.angularRatePerturbation );

                qnew = state.orientation();
                tools::perturb_orientation( truePerturbation, qnew );
        }

        void ImuEkf::propagateTranslation( Eigen::Vector3d & tnew,
                                                                           const Eigen::Quaterniond & rot,
                                                                           const ImuState & state, 
                                                                           const ImuMeasurement & measurement,
                                                                           double deltaT )
        {
                Eigen::Vector3d worldAcc;
                imu_filter::tools::remove_gravity( rot,
                                                                                   measurement.linearAcceleration,
                                                                           state.accBias(),
                                                                                   state.gravity(),
                                                                                   worldAcc );

                worldAcc += measurement.linearAccelerationPerturbation;

                tnew = state.position() + state.velocity() * deltaT + worldAcc * deltaT * deltaT;
        }

        void ImuEkf::propagateVelocity( Eigen::Vector3d & vnew,
                                                                        const Eigen::Quaterniond & rot,
                                                                        const ImuState & state,
                                                                        const ImuMeasurement & measurement,
                                                                        double deltaT )
        {
                Eigen::Vector3d worldAcc;

                imu_filter::tools::remove_gravity( rot,
                                                                                   measurement.linearAcceleration,
                                                                                   state.accBias(),
                                                                                   state.gravity(),
                                                                                   worldAcc );
                worldAcc += measurement.linearAccelerationPerturbation;

                // damped velocity
                vnew = velocity_damping_ * state.velocity() + worldAcc * deltaT;
        }

        void ImuEkf::propagateState( const ImuState & oldState, const ImuMeasurement & meas, double deltaT, ImuState & newState )
        {
                propagateOrientation( newState.orientation(), oldState, meas, deltaT );
                propagateTranslation( newState.position(), newState.orientation(), oldState, meas, deltaT );
                propagateVelocity( newState.velocity(), newState.orientation(), oldState, meas, deltaT );
                newState.gyroBias() = oldState.gyroBias() + meas.gyroBiasPerturbation;
                newState.accBias() = oldState.accBias() + meas.accBiasPerturbation;
                newState.gravity() = oldState.gravity();
        }

        void ImuEkf::stateJacobian( const ImuState & state, 
                                                            const ImuMeasurement & meas, 
                                                            double deltaT, 
                                                            double step,
                                                            Eigen::Matrix<double, 19, 19> & F )
        {
                ImuState newState;
                ImuState changedState( state );
                ImuState::StateVector stateVector, stateOrig;
                propagateState( state, meas, deltaT, newState );
                newState.toVector( stateOrig );

                size_t i = 0;

                // qx
                changedState.orientation().x() += step;
                propagateState( changedState, meas, deltaT, newState );
                newState.toVector( stateVector );
                F.col( i++ ) = ( stateVector - stateOrig ) / step;
                changedState.orientation() = state.orientation();

                // qy
                changedState.orientation().y() += step;
                propagateState( changedState, meas, deltaT, newState );
                newState.toVector( stateVector );
                F.col( i++ ) = ( stateVector - stateOrig ) / step;
                changedState.orientation() = state.orientation();

                // qz
                changedState.orientation().z() += step;
                propagateState( changedState, meas, deltaT, newState );
                newState.toVector( stateVector );
                F.col( i++ ) = ( stateVector - stateOrig ) / step;
                changedState.orientation() = state.orientation();

                // qw
                changedState.orientation().w() += step;
                propagateState( changedState, meas, deltaT, newState );
                newState.toVector( stateVector );
                F.col( i++ ) = ( stateVector - stateOrig ) / step;
                changedState.orientation() = state.orientation();

                // px
                changedState.position().x() += step;
                propagateState( changedState, meas, deltaT, newState );
                newState.toVector( stateVector );
                F.col( i++ ) = ( stateVector - stateOrig ) / step;
                changedState.position().x() -= step;

                // py
                changedState.position().y() += step;
                propagateState( changedState, meas, deltaT, newState );
                newState.toVector( stateVector );
                F.col( i++ ) = ( stateVector - stateOrig ) / step;
                changedState.position().y() -= step;

                // pz
                changedState.position().z() += step;
                propagateState( changedState, meas, deltaT, newState );
                newState.toVector( stateVector );
                F.col( i++ ) = ( stateVector - stateOrig ) / step;
                changedState.position().z() -= step;

                // vx
                changedState.velocity().x() += step;
                propagateState( changedState, meas, deltaT, newState );
                newState.toVector( stateVector );
                F.col( i++ ) = ( stateVector - stateOrig ) / step;
                changedState.velocity().x() -= step;

                // vy
                changedState.velocity().y() += step;
                propagateState( changedState, meas, deltaT, newState );
                newState.toVector( stateVector );
                F.col( i++ ) = ( stateVector - stateOrig ) / step;
                changedState.velocity().y() -= step;

                // vz
                changedState.velocity().z() += step;
                propagateState( changedState, meas, deltaT, newState );
                newState.toVector( stateVector );
                F.col( i++ ) = ( stateVector - stateOrig ) / step;
                changedState.velocity().z() -= step;

                // gyroBiasx
                changedState.gyroBias().x() += step;
                propagateState( changedState, meas, deltaT, newState );
                newState.toVector( stateVector );
                F.col( i++ ) = ( stateVector - stateOrig ) / step;
                changedState.gyroBias().x() -= step;

                // gyroBiasy
                changedState.gyroBias().y() += step;
                propagateState( changedState, meas, deltaT, newState );
                newState.toVector( stateVector );
                F.col( i++ ) = ( stateVector - stateOrig ) / step;
                changedState.gyroBias().y() -= step;

                // gyroBiasz
                changedState.gyroBias().z() += step;
                propagateState( changedState, meas, deltaT, newState );
                newState.toVector( stateVector );
                F.col( i++ ) = ( stateVector - stateOrig ) / step;
                changedState.gyroBias().z() -= step;

                // accBiasx
                changedState.accBias().x() += step;
                propagateState( changedState, meas, deltaT, newState );
                newState.toVector( stateVector );
                F.col( i++ ) = ( stateVector - stateOrig ) / step;
                changedState.accBias().x() -= step;

                // accBiasy
                changedState.accBias().y() += step;
                propagateState( changedState, meas, deltaT, newState );
                newState.toVector( stateVector );
                F.col( i++ ) = ( stateVector - stateOrig ) / step;
                changedState.accBias().y() -= step;

                // accBiasz
                changedState.accBias().z() += step;
                propagateState( changedState, meas, deltaT, newState );
                newState.toVector( stateVector );
                F.col( i++ ) = ( stateVector - stateOrig ) / step;
                changedState.accBias().z() -= step;

                // gravx
                changedState.gravity().x() += step;
                propagateState( changedState, meas, deltaT, newState );
                newState.toVector( stateVector );
                F.col( i++ ) = ( stateVector - stateOrig ) / step;
                changedState.gravity().x() -= step;

                // gravy
                changedState.gravity().y() += step;
                propagateState( changedState, meas, deltaT, newState );
                newState.toVector( stateVector );
                F.col( i++ ) = ( stateVector - stateOrig ) / step;
                changedState.gravity().y() -= step;

                // gravz
                changedState.gravity().z() += step;
                propagateState( changedState, meas, deltaT, newState );
                newState.toVector( stateVector );
                F.col( i ) = ( stateVector - stateOrig ) / step;
                changedState.gravity().z() -= step;
        }


        void ImuEkf::noiseJacobian( const ImuState & state, 
                                                            const ImuMeasurement & meas,
                                                            double deltaT,
                                                            double step,        
                                                            Eigen::Matrix<double, 19,12> & G )
        {
                ImuState newState;
                ImuState changedState( state );

                ImuState::StateVector stateVector, stateOrig;
                propagateState( state, meas, deltaT, newState );
                newState.toVector( stateOrig );
                
                ImuMeasurement measCopy( meas );
                
                size_t i = 0;

                // angular rate noise
                measCopy.angularRatePerturbation.x() += step;
                propagateState( state, measCopy, deltaT, newState );
                newState.toVector( stateVector );
                G.col( i++ ) = ( stateVector - stateOrig ) / step;
                measCopy.angularRatePerturbation.x() -= step;

                measCopy.angularRatePerturbation.y() += step;
                propagateState( state, measCopy, deltaT, newState );
                newState.toVector( stateVector );
                G.col( i++ ) = ( stateVector - stateOrig ) / step;
                measCopy.angularRatePerturbation.y() -= step;

                measCopy.angularRatePerturbation.z() += step;
                propagateState( state, measCopy, deltaT, newState );
                newState.toVector( stateVector );
                G.col( i++ ) = ( stateVector - stateOrig ) / step;
                measCopy.angularRatePerturbation.z() -= step;

                // acceleration rate noise
                measCopy.linearAccelerationPerturbation.x() += step;
                propagateState( state, measCopy, deltaT, newState );
                newState.toVector( stateVector );
                G.col( i++ ) = ( stateVector - stateOrig ) / step;
                measCopy.linearAccelerationPerturbation.x() -= step;

                measCopy.linearAccelerationPerturbation.y() += step;
                propagateState( state, measCopy, deltaT, newState );
                newState.toVector( stateVector );
                G.col( i++ ) = ( stateVector - stateOrig ) / step;
                measCopy.linearAccelerationPerturbation.y() -= step;

                measCopy.linearAccelerationPerturbation.z() += step;
                propagateState( state, measCopy, deltaT, newState );
                newState.toVector( stateVector );
                G.col( i++ ) = ( stateVector - stateOrig ) / step;
                measCopy.linearAccelerationPerturbation.z() -= step;

                // gyro bias noise 
                measCopy.gyroBiasPerturbation.x() += step;
                propagateState( state, measCopy, deltaT, newState );
                newState.toVector( stateVector );
                G.col( i++ ) = ( stateVector - stateOrig ) / step;
                measCopy.gyroBiasPerturbation.x() -= step;

                measCopy.gyroBiasPerturbation.y() += step;
                propagateState( state, measCopy, deltaT, newState );
                newState.toVector( stateVector );
                G.col( i++ ) = ( stateVector - stateOrig ) / step;
                measCopy.gyroBiasPerturbation.y() -= step;

                measCopy.gyroBiasPerturbation.z() += step;
                propagateState( state, measCopy, deltaT, newState );
                newState.toVector( stateVector );
                G.col( i++ ) = ( stateVector - stateOrig ) / step;
                measCopy.gyroBiasPerturbation.z() -= step;

                // acc Bias noise
                measCopy.accBiasPerturbation.x() += step;
                propagateState( state, measCopy, deltaT, newState );
                newState.toVector( stateVector );
                G.col( i++ ) = ( stateVector - stateOrig ) / step;
                measCopy.accBiasPerturbation.x() -= step;

                measCopy.accBiasPerturbation.y() += step;
                propagateState( state, measCopy, deltaT, newState );
                newState.toVector( stateVector );
                G.col( i++ ) = ( stateVector - stateOrig ) / step;
                measCopy.accBiasPerturbation.y() -= step;

                measCopy.accBiasPerturbation.z() += step;
                propagateState( state, measCopy, deltaT, newState );
                newState.toVector( stateVector );
                G.col( i++ ) = ( stateVector - stateOrig ) / step;
                measCopy.accBiasPerturbation.z() -= step;

        }
        

        void ImuEkf::numericalJacobians( const ImuState & state,
                                                                         const ImuMeasurement & meas,
                                                                         double deltaT,
                                                                         Eigen::Matrix<double, 19, 19> & F,
                                                                         Eigen::Matrix<double, 19, 12> & G )
        {
                ImuState newState;
                ImuState changedState( state );

                ImuState::StateVector stateVector, stateOrig;
                propagateState( state, meas, deltaT, newState );
                newState.toVector( stateOrig );

                double step = 1e-9;

                stateJacobian( state, meas, deltaT, step, F );
                noiseJacobian( state, meas, deltaT, step, G );
        }

}