构造运动学方程
输出位姿估计
读取span class=\"equation-text\" contenteditable=\"false\" data-index=\"0\" data-equation=\
开始
对span class=\"equation-text\" contenteditable=\"false\" data-index=\"0\" data-equation=\
求解
输出速度估计
结束
求解span class=\"equation-text\" contenteditable=\"false\" data-index=\"0\" data-equation=\
联立求解span class=\"equation-text\" contenteditable=\"false\" data-index=\"0\" data-equation=\
分解
