卡尔曼滤波的理解以及参数调整
http://blog.csdn.net/u013453604/article/details/50301477
四元数
YPR和四元数转换
http://wiki.ros.org/kdl/Tutorials/Frame%20transformations%20%28Python%29
计算两个朝向的夹角
import PyKDL
a = PyKDL.Rotation.Quaternion(x, y, z, w)
b = PyKDL.Rotation.Quaternion(x, y, z, w)
c = a * b.Inverse()
c