第三篇 坐标转换关系T平滑

一 背景

在SLAM系统中或者自动驾驶定位系统中，需要视觉感知元素和地图元素进行匹配，然后将感知元素旋转到地图上或者将地图元素旋转到感知元素表达的坐标系。这种转换关系使用T表达，每一帧感知都会对应一个T，帧与帧之间存在着抖动，需要进行平滑。
载体坐标和感知元素在 smooth DR坐标下表达，地图元素在地图坐标下表达，需要平滑的地图坐标到smooth坐标之间的转换关系T如下表达。
$$T_{\_smooth\_map}$$

二 位姿矩阵平滑定义变量

不能直接平滑T_smooth_map，因为位置和姿态的计算是相互关联的，并不是相互独立的过程。可以将载体在地图中的位置和姿态设置为状态量。姿态的计算是一个非线性过程，所以采用误差卡尔曼滤波。

2.1 状态量

载体在地图中的位置状态量：
$$T_{\_map\_rig}$$
状态误差量定义如下:
$$\begin{gathered} R_{true}=R\ast \delta R \\ {t}_{true}=t-\delta t \end{gathered}$$
状态误差向量定义如下
$$st\_vec\_err=[\delta x\delta y\delta z\delta {\varphi }_x\delta {\varphi }_y\delta {\varphi }_z]$$
后三维是姿态误差的等效旋转矢量

2.2 状态量更新

T_map_rig(k+1)(update)=T_map_rig(k)∗T_rig(k)_rig(k+1)=T_map_rig(k)∗T_rig(k)_smooth∗T_smooth_rig(k+1)=T_map_rig(k)∗T_smooth_rig(k).inv()∗T_smooth_rig(k+1) \begin{aligned} T_{\_map\_rig(k+1)}(update) &= T_{\_map\_rig(k)} *T_{\_rig(k)\_rig(k+1)} \\ &= T_{\_map\_rig(k)} *T_{\_rig(k)\_smooth} *T_{\_smooth\_rig(k+1)}\\ &= T_{\_map\_rig(k)} *T_{\_smooth\_rig(k)} .inv()*T_{\_smooth\_rig(k+1)} \end{aligned} T_map_rig(k+1)​(update)​=T_map_rig(k)​∗T_rig(k)_rig(k+1)​=T