卡尔曼滤波算法,姿态检测算法,控制算法,以及相互之间的关系