PositionVelocityEstimator

参考论文：
[1]Camurri M, Fallon M, Bazeille S, et al. Probabilistic contact estimation and impact detection for state estimation of quadruped robots[J]. IEEE Robotics and Automation Letters, 2017, 2(2): 1023-1030.

变量：
	Rbod：机体坐标系到世界坐标系的旋转矩阵(3*3)
	p_rel：机体坐标系下的足端位置坐标(3*1)
	p_f：世界坐标系下的足端位置坐标(3*1)
	_ps：以每个足端为基准，位于机体中心的世界坐标系的位置(12*1)
	trust：速度检测的信任度
	trust_window：速度检测的阈值
		
函数：	
>>>>>>>	template <typename T>
                 void LinearKFPositionVelocityEstimator<T>::setup()
	函数功能：矩阵的初始化

>>>>>>>	template <typename T>
                 void LinearKFPositionVelocityEstimator<T>::run()
	函数功能：构建线性Kalman滤波框架：
		  从源码中的第152行开始：
			构建28*1的列向量y，按照顺序存储_ps，_vs和pzs，其中：
				_ps：以每个足端为基准，位于机体中心的世界坐标系的位置(12*1)
				_vs：每个足端对于当前机体速度的检测值，这里可以参考论文[1]中的计算方法，与此同时，与上一时刻滤波得到的最终的速度值之间进行加权，权重系数为trust，
                                     说白了也就是现在我到底信任谁的问题，是更信任上一时刻的最终滤波后的结果速度还是更信任此时通过腿部估计的速度，在二者之间进行加权，权重系数与
                                     trust相关，而trust与当前所处的一个完整步态周期的百分比有关，源码中设定速度检测的阈值trust_window为0.2,在一个完整的步态周期中，phase为估计
                                     的着地概率，phase处于0-0.2时，trust为phase/0.2;phase处于0.2-0.8时，trust为1;phase处于0.8-1的时，trust为(1-phase)/0.2。也就是说着地概率
                                     很小或者很大的时候，trust与这个概率有关，而其余时候都是1。trust越大，也就越相信此时通过腿部估计的速度。(12*1)
				pzs：p0为上一时刻滤波得到的最终的位置值，pzs中的每个元素存储一个数值，这个数值是上一时刻滤波得到的最终的位置值中的第三个元素，也就是机体高度与当
				     前时刻对应位置的足端在世界坐标系下的位置的第三个元素，也就是足端与机体中心之间在竖直方向上的高度差的差值。如果这个值大于零，意味着当前时刻
				     这个足端与机体中心的竖直方向的距离小于上一时刻滤波得到的最终的机体高度值。(4*1)
			_xhat = _A * _xhat + _B * a：动态系统的状态空间模型，表示状态的预测。其中_xhat为机器人状态向量(18*1)，前三个元素为机体的位置，之后三个元素为机体的速度，
						     最后的十二个元素分为四组。每组为每个足端在初始坐标系下的绝对位置。a为加速度向量(3*1)。
			Pm = _A * _P *  _A.transpose + Q：协方差矩阵的预测，其中_P为系统的协方差矩阵，为100倍的单位阵;Q为过程噪声矩阵。
			ey = y - _C * _xhat：得到偏差。
			S = _C * Pm * _C.transpose() + R：R为系统的观测噪声矩阵。
			状态更新以及协方差矩阵更新。