位姿平滑方案

位姿平滑

时间控制方案

方案一：划分

• 将两帧之间的33ms左右的时间划分为更小的块（暂定10ms）
• 在收到最新一帧后，计算上一帧与新一帧之间的3个中间帧，并在下一帧到来前每10ms更新一次
• 这样在整体添加了一帧的延迟
• 但这么做不会平滑突发的噪音帧，只增加帧数，保留了异常抖动

方案二：均值滤波

新一帧显示位姿=α*上一帧位姿+β*新一帧位姿（α+β=1）

插值方式

四元数插值与均值（姿态平滑）

最终方案（结合版）

有了延时显示之后，因为我们可以手动增加更多的显示延迟，所以位姿平滑可以做的更激进一点，不仅仅做两帧之间的平滑，而是可以再加一帧，做三帧或更多帧的平滑，先以3帧为例

时间序列示意表

星号标记的为插值后数据

位姿可用时间序列	1	2	3	3-4区间	4	4-5	5	x	x-x+1
使用的位姿	1，等价于1-1插值后的1*	1，等价于1-1插值后的1-1区间	1，等价于1-2插值后的1*	1-3插值后的1-2区间	1-3插值后的2*	2*-4插值后的2*-3区间	2*-4插值后的3*	(x-3)*-(x-1)插值后的(x-2)*	(x-2)*-x插值后的(x-2)*-（x-1)区间

变量

• D3：插值起始值，即x-3
• D1：插值结束值，即x-1

步骤

1. 在x时刻位姿到来时，继续计算x时刻前使用的D3-D1的中值，并保存为新的D3，将x时刻位姿保存为D1，返回此D3作为现值
2. 在x时刻后，每次更新使用D3-D1的插值：elapsed/33*0.5

初始化

• 设D3为null,D1为null
• 收到第一帧时同时置这两者为现值

一般化

如果不限制为3帧，核心思想还是同样的：

• 永远使用插值作为返回值
• 防止突变（保持连续性）

比如5帧的情况，就一直使用x-5和x-1的插值，直到x+1时换成(x-5)-(x-1)插值后的x-2作为新的插值初值。