#include "kalmanFilter.h"

KalmanFilter::KalmanFilter()
{
    tf::Quaternion quat(0,0,0,1);
    tf::Vector3 T(0,0,0);
    m_last_pose=tf::Transform(quat,T);
}

tf::Pose KalmanFilter::filter(tf::Pose pose1,tf::Pose pose2)
{
    
    tf::Quaternion quat1=pose1.getRotation()-m_last_pose.getRotation();
    tf::Vector3 offset1=pose1.getOrigin()-m_last_pose.getOrigin();
    double sigma_th1=quat1.length2();
    double sigma_xy1=offset1.length2();
    
    tf::Quaternion quat2=pose2.getRotation()-m_last_pose.getRotation();
    tf::Vector3 offset2=pose2.getOrigin()-m_last_pose.getOrigin();
    double sigma_th2=quat2.length2();
    double sigma_xy2=offset2.length2();

    double K_th=sigma_th1/(sigma_th1+sigma_th2);
    double K_xy=sigma_xy1/(sigma_xy1+sigma_th2);

    
    

    tf::Quaternion quat_mean=pose1.getRotation()+
                            (pose2.getRotation()-pose1.getRotation())*(1.0-K_th);
    tf::Vector3 offset_mean=pose1.getOrigin()+K_xy*(pose2.getOrigin()-pose1.getOrigin());
    
    return tf::Pose(quat_mean.normalized(),offset_mean);

}