创建零矩阵

打印输出矩阵

创建矩阵

矩阵乘法 C=A*B

释放堆栈

初始化堆栈

矩阵加法 A+B=C

矩阵转置

Return robot's current state

矩阵转变为单位正态随机矩阵

矩阵减法 A-B=C

Correct State: Right-Invariant Observation

创建单位矩阵

Check if empty

矩阵求行列式

矩阵求逆

矩阵赋值 C（Rs:Rs+C->row,Cs:Cs+C->column）=A

正定矩阵A的Cholesky 分解 LT*L=R

Correct state using kinematics measured between imu and contact point

InEKF Propagation - Inertial Data

---------------- Go2 point-foot preset ---------------- ---------------- Go2 点足参数预设 ---------------- * This preset describes a 3-DOF point-foot leg:

---------------- LW wheel-foot preset ---------------- ---------------- LW 轮足参数预设 ---------------- * This preset describes a 3-DOF leg plus one wheel

创建对角矩阵 返回矩阵指针

Return filter's estimated landmarks

矩阵清为零矩阵

矩阵转变为0-1均匀随机矩阵

矩阵纵向拼接 C=[A;B]

矩阵提取 C=A（Rs:Rs+C->row,Cs:Cs+C->column）

矩阵横向拼接 C=[A,B]

矩阵的迹

向量相乘得矩阵 v1*v2T=M

Set the filter's contact state