基本原理

VR 通过分屏渲染一个虚拟的世界，让佩戴者产生沉浸感，如下图所示：

上面中屏幕中的虚拟世界画面，是由 OpenGL 渲染的 3D 世界。我们这里不讨论分屏、反畸变原理，所以只看其中的一边屏幕。我们先说下 OpenGL 中构建虚拟世界的 MVP 矩阵：

MVP 矩阵

假设 3D 空间中有一个物体，如下图所示：

假设 (0,0,0,) 为物体的中心，那么这个物体的其他顶点都相对这个中心有自己的坐标。3D 空间也有自己的坐标，这个叫做世界坐标。如果要把这个物体放到 3D 空间中（在3D空间显示，也就是在虚拟世界显示），那么需要将物体的坐标转化为世界坐标。进行这个运算的矩阵叫做模型矩阵（Model Matrix）：

在虚拟世界中创建了物体，作为人（用户），需要能够感知（看到）这个世界（中的物体），假设有台摄像机在记录这个虚拟世界，那么这个摄像机以不同的角度就能观察到虚拟世界中的不同内容，也就是存在某种转化，把虚拟世界中的内容转化到摄像机当中（虚拟世界坐标转化到摄像机坐标）。进行这个运算的叫视图矩阵（View Matrix）：

但是最后虚拟世界需要显示到一个2D的屏幕上，所以需要将摄像机中看到的内容投影到一个2D的平面上（摄像机摄影了，放映到屏幕上）。一般的这个投影是个锥形的（OpenGL 的一种投影，叫透视投影 Perspective）。进行这个投影运行的叫投影矩阵（Project Matrix）：

经过上面的一些列变化（矩阵运算），最终就能将一个3D的虚拟世界显示到一个2D的屏幕上，上面的矩阵运算一般通过矩阵的乘法，在一起运算，所以上面的三个矩阵，在 OpenGL 中就合在一起叫 MVP 矩阵。

举个形象点的例子：蓝色的是虚拟世界中的物理，红色的表示摄像机透视投影的锥形：

那么投到一个矩形的屏幕上虚拟世界中的物体就会变形成这样：

然后显示到2D窗口中最终图像是就是这样的：

头部跟踪

那么头部跟踪和前面介绍的 MVP 矩阵有什么关系呢。其实人戴着 VR 头盔转动，就是想和一个摄像机一样观察看虚拟世界不同的场景，所以头部跟踪就是将 VR 头盔的转动，反映到 View Matrix 上。那怎么反映，这里再来介绍一下另外一个概念：在3D空间中如何转动一个物体。

3D 空间有3个轴x，y，z，在 VR 领域大家更习惯用 yaw（y），pitch（x），roll（z）来表示。如果依次沿着x，y，z轴转动相应的角度，那么这个物体就有得到一个新的朝向（Orientation）：

沿着这3个轴转动的角度叫做 欧拉角（Euler Angle）。欧拉角是有顺序的，按照不同的顺序变化，角度是不同的：例如 x，y，z 和 y，x，z 。我们的 sdk 使用的顺序是 y，x，z，也就是 yaw，pitch，roll。

欧拉角可以表示一个朝向，但是欧拉角是三个独立的角度，不利于连续的插值变化运算，所以有人发明了四元数（Quaternion）。欧拉角和四元数可以相互转化，具体的数学公式这里不详细说了（可以网上查资料），sdk 里有 api 可以直接使用。

利用欧拉角可以在3D空间中进行旋转操作，也就是说摄像机的转动可以使用欧拉角来运算。头部跟踪就是要得到 VR 头盔实时转动的欧拉角，然后通过 MVP 反映到渲染的图像上。

在 android 平台上，陀螺仪（Gyroscope）上报的3轴的数据正好就是表示3个轴旋转的角速度：

假设使用一个四元数来表示当前摄像机（头盔）的朝向（O）。那么在 t0 时刻 O 初始值为 0。经过 delta 时间，在 t1 时间读取陀螺仪的角速度 s，那么这个时刻，头盔转动过的欧拉角是：

    angle = delta * s

那么在当前朝向的基础上转动这个欧拉角，就能得到头盔的新朝向 O1：

    O1 = O + angle

如果持续重复这个过程，那么摄像机（头盔）就能模拟人转动想看到的虚拟世界中的场景，这个过程就叫头部跟踪（HeadTracking）。

数据通路

全志的 sdk 中头部跟踪的数据通路是这样的：

上面的数据通路主要分成3块：

底层原始数据

应用通过 API 让 android system service 中的 sensor service 打开 sensor hub，sensor hub 就开始以指定的频率上报需要的数据，VR 的头部跟踪目前主流用到的是3个传感器的数据：陀螺仪（gyro）、重力加速度（accel），地磁（mag），每一个传感器有 x、y、z 3个轴的数据，将这3个传感器的数据融合（fusion）得到一个四元数的朝向，就叫9轴融合。市面上很多VR说的支持9轴传感器，就是融合陀螺仪、重力、地磁的意思。sensor service 读到数据后，就会通过 local socket 发送给上层应用。

头部跟踪模块数据融合

头部跟踪模块在软件分层中属于应用层，头部跟踪会开启一个后台采样线程将收到的 gyro、accel、mag 重复的按照下面步骤融合（fusion）成四元数（Quat）朝向（Orientation）：

1. 将 gyro、accel、mag 打包成一个数据包，具体后面说明。
2. gyro 通过累积变化，计算当前朝向（第一章说明）。
3. 重力校准（tilt correct），具体后面说明。
4. 地磁校准（yaw correct），具体后面说明。
5. 保存最后计算的朝向，等待渲染线程来取。

应用渲染线程获取朝向

一般的 VR OpenGL 应用都会有一个渲染线程，会在每一帧从头部跟踪模块获取到最新的朝向，从朝向中获取转动欧拉角，然后应用到视图矩阵。

这里的渲染线程和上面的采样线程是独立的，采样线程只是不停的融合 sensor 数据，实时更新最新朝向，等待有人来取。一般来说在 VR 上 gyro、accel 的采样率都是 1000Hz 以上（我们的目前是 800Hz），而且渲染的帧率一般是 60fps（某些高端的 VR 已经到 70fps 甚至 120fps 了）。这里就有一个疑问，既然渲染的帧率才 60，那为什么采样率需要那么高呢？这是因为采样率越高，那个计算出来的最新朝向就越能代表当前设备真实的朝向，表现在画面上的延迟感就会更低。这也就是为什么 VR 上要单独设计一个 sensor hub 的原因（高采样率的 sensor 如果通过通用 cpu 来处理，那么会对上层系统造成极大的负载，所以使用一个小 cpu 来单独处理高采样率的 sensor）。

所以我们 sdk 里的数据模型是：采样线程在后台以高频率接收 sensor 数据，并融合出最新的朝向，保存起来。前台的渲染线程按照自己的帧率从采样线程获取朝向。

简洁的来说整个数据通路各个模块的输入输出就是下面这个样子：

打包处理

接下来我们细分几块来把之前数据通路的几个地方说清楚。首选是打包处理。在头部跟踪模块接收到 sensor service 发送过来的 sensor 数据，会有一个打包的操作。这个包简单来说包含5个数据：

1. Gyro：陀螺仪，当前转动角速度，用于计算朝向增量变化
2. Accel：重力加速度，当前的位置信息，用于 tile correct
3. Mag：地磁，当前的位置信息，用于 yaw correct
4. Delta：sensor 上报的时间间隔，当前的时间戳与上一个数据包的时间戳相减得到，朝向增量变化使用这个 dt，这个是 sensor hub 带上来的时间
5. TimeStamp：收到 sensor 的时间戳，预测朝向计算使用这个 dt，这个是上层系统的时间

上面5个数据组成一个数据包（结构体 sdk 里叫 SFBodyMessageFrame）。这里先说一下，为什么需要打包。由于 sensor 的零漂误差（drift error），所以需要 tile correct 和 yaw correct（后面在说）。所以理论上来说需要 gyro、accel、mag 这3个数据一一对应（要拿当前时刻的 accel 和 mag 来校准当前时刻的 gyro 转动）。

但是这是理论情况，实际上，很有可能底层上报的数据并不是严格一一对应的（底层可以一次上报同时包括 gyro、accel、mag 三个数据的）。目前我们的平台，首先 gyro、accel 的采样率是 800Hz，mag 是 100Hz，gyro、accel 和 mag 就不可能一一对应上。然后 gyro、accel 虽然频率一样，但是上报也不是成对的，经常是先报一个 gyro，然后再 accel，或者反过来。

所以 headtracking 这边的前处理就是：

1. 创建3个 fifo 分别保存收到的 gyro、accel、mag
2. 以 gyro fifo 中的数量为准（cnt 个），循环 cnt 次，分别从3个 fifo 中把 gyro，accel，mag 取出，打包到结构体中（SFBody）；将已取出打包的数据分别从 fifo 中删除。
3. 保存上一次打包的 gyro 的 timestamp（sensor hub 带上来的，lastGyroTimestamp），将当前的 gyro 的 timestampe 和 lastGyroTimestamp 相减得到 Delta。并且获取当前系统时间戳，作为 SFBody 的 Timestamp（AbsoluteTimeInSeconds）。
4. 如果2中 gyro fifo 为空，那么等待 gyro 数据。在等待过程中，如果 accel fifo 中的数据超过了一定时间没有被打包走，那么认为是过时的数据，将过时的 accel fifo 清空。

用图来表示，大概是这样的：

解释一下上面的打包处理：

对于2，打包的 SFBody 中可以没有 accel 或者 mag，但是一定会有 gyro。这是因为就算朝向变化，是依靠 gyro 来计算的，如果这次的数据没有 gyro，那么就无法计算朝向变化，accel 和 mag 只是用来校准的而已，没有只是影响朝向的精度。这样处理，是尽可能的让 gyro 和 accel 一一对应，同时放宽条件，允许底层上报数据的时候存在一定的偏差，不严格一一对应。

对于3，这里用到了2套时间系，分别是 sensor hub 上报 sensor 带上来的时间戳，和上层系统（android）的系统时间戳。前者是用来计算朝向变化的（angle x dt）。后者是用来计算预测的，预测后面再说。为什么使用2套时间系，是因为 sensor hub 带上来的时间戳，是 sensor 采样时候的时间戳，表示2个 sensor 时间之间的时间间隔，而如果 android 系统的时间戳，表示的是 headtracking 收到 sensor service 发送 sensor 的时间。这2套时间体系是有误差的，所以在计算 deltaT 的时候只能统一采用一套。所以计算朝向变化统一使用 sensor hub 上报的时间戳。预测需要应用传入时间戳，而应用只能获取到 android 系统的时间，所以预测统一使用 android 系统的时间系统。

对于4，由于2没有严格要求 gyro 和 accel 一一对应。所以这次采用一个过时机制，保证打包的 gyro、accel 在时间上相差不是特别大。而mag fifo 不处理，因为 mag 的采样率比 gyro、accel 低很多，而且在 yaw correct 的时候会处理 mag 采样率的延迟，所以不需要在打包的时候去考虑 mag 的过时。

Tile Correct

说校准之前先接着补充说一下第一章的基本原理。第一章说了 headtracking 就是让 sensor 数据表现在摄像机上，这里说一下怎么旋转摄像机的。VR 世界里面，需要构造一个虚拟世界，那么世界肯定要是正的，例如说这样的：

OpenGL 的初始状态世界坐标是正的，假设 VR 设备这个时候也是正的：

如果 VR 设备向右边歪一下，屏幕也就往右边歪了，那么世界坐标就歪了：

往如果要继续保持世界坐标系看起来是正的，这个时候摄像机就需要向反方向旋转：

头部跟踪的本质就是通过旋转视图矩阵（摄像机），让世界坐标系看上去是正的。这里就有一个概念：VR 世界里面，看上去的世界坐标是正的（下面为了方便，就直接说世界坐标了，而不特意强调是看上去的世界坐标）。

校正原理

说明白了上面的概念后，就可以来说 tile correct 了。为什么需要 tile correct，是因为 gyro 在实际的采样过程中是会存在误差的，可能每一次的误差并不是很大（sensor 厂商的 datasheet 范围内），但是由于持续的使用，就会存在累积误差，一段时间后，累积误差就会很大，从而能让使用者感知到位移误差。gyro 是有3个轴的，所以误差是在3个轴都有的，这里说的 tile correct 指的是倾斜误差，指的是 x，z 轴的误差，为什么没 y 轴的，后面会说明。

由于 gyro 的累积误差导致 x，z 轴旋转有误差，所以旋转的摄像机看到的世界坐标就是倾斜的，所以叫 tile correct。那怎样校正呢。

1. 我们假设有一个竖直向上的向量 up (0, 1, 0)
2. 那么有 up’ = up * current orientation， up’ 就是 up 在当前朝向上的分量
3. 如果 gyro 没有误差，那么计算出来的 orientation 应该要使摄像机看到的世界坐标也是正的，那么 up’ 应该还是 (0, 1, 0)
4. 也就是说如果 up’ 和 up 存在夹角（error），那么这个夹角就是误差角，我们只要将当前朝向回正这个误差角，就做到校正了（保证世界坐标系是正的）
5. 我们知道地球的重力加速度是永远垂直向下的，在 android 上上报的方向正好相反，向上是正的，所以我们可以用 accel 来测量（measured） up’。用当前朝向和 accel 可以计算出 up’ ，然后用 up’ 和理论 up(0, 1, 0)计算夹角，就能得到误差偏移角（error）。

上面这个过程就是我们的 headtracking 中使用的 tile correct 的原理。因为为沿 y 轴旋转 accel 是没有变化的，所以 tile correct 只能校正 x，z 轴。

细节处理

下面说一下 tile correct 的几点细节：

    1. 由于 accel 也是存在误差的（只要是 sensor 都会存在误差），但是 accel 的变化没有 gyro 那么明显，所以 headtracking 中使用了一定的过滤方法来过滤一定样本的 accel。具体的过滤方法可以去看代码（大致是一定样本数量+低斯滤波）。

    2. tile correct 的 error 角度，如果太大的话，那么用户是会有明显的感知的。为了避免这个问题，有下面的处理：

        a. 第一次 tile correct （GetSize() == 1）允许完全使用 error 角度计算。 因为第一次 tile correct 一般都是刚开机，或者是休眠唤醒的时候。这个时候的设备的朝向一般都不是正的，坐标系是默认按照屏幕方向的，所以这个时候进行完全校准，能让坐标系回到正方向。但是相对的，如果这个过程被用户看到了，就会发现画面突然跳变了一下。这里就引申出一个问题，休眠唤醒的时候画面会跳一下。处理方法就是，可以尽量提高头部跟踪的启动速度，和第一次 tile correct 的速度，如果赶在渲染第一帧之前就完成第一次 tile correct 那么画面就不会有“明显”的跳动感。

        b. 如果 error 角度不大(Abs(error.w) < cos(snapThreshold / 2))，并且设备“相对静置”(Confidence > 0.75)，那么也允许完全使用 error角度。相对静置，其实就是上面的 accel 的过滤采样的样本的方差，通过一个公式计算得到一个信任分数（Confidence，和sensorhub gyro 那章判断静置的分数计算类似）。如果设备非水平转动，那么 accel 的3轴数值会变化（水平转动，accel 的变化不大），那么这个分数就会比较低。

        c. 设备“比较静置”(Confidence > 0.5)，那么对计算出的 error 角度进行一个插值（Nlerp），弱化补偿效果，让补偿转动不明显。比较静置就是上面那个信任分数稍微低一点。插值的实现自己去看代码吧，我也不是熟，反正就是弱化这个角度的作用。

        d. 如果不静置，就不进行 tile correct。也就是上面的信任分数很低。

上面的处理情况，代码就是这样的：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

Quatf correction;
if (FAccelHeadset.GetSize() == 1 ||
    ((Alg::Abs(error.w) < cos(snapThreshold / 2) 
    && FAccelHeadset.Confidence() > 0.75)))
{
    // full correction for start-up
    // or large error with high confidence
    correction = error;
}
else if (FAccelHeadset.Confidence() > 0.5)
{
    correction = error.Nlerp(Quatf(), gain * deltaT);
}
else
{
    return;
}

Yaw Correct

校正原理

上面 accel 只能校正 x，z 轴，那么下面就是要进行 y 轴校正了（yaw correct）。和 tile correct 的原理类似，对于水平方向上来说，如果存在一个水平指北的向量，并且我们可以 measured 这个方向，那么我们可以用和 tile correct 类似的原理那校正 y 轴。

我们很自然的想到地磁（mag），可以用做指南针（其实它是指北的）。但是和 accel 不同的是，地磁在不同的区域是不一样的：

如果地磁与水平面的夹角（inclination angle）过大，那么它投影到水平面上的分量就很小。可以看到芬兰附近的 inclination angle 是很大的。还有就是其实地磁也不是完美的指向北方，某些地方是也是存在偏差角度的（declination angle）。另外外界对地磁测量也存在影响，例如硬铁（hard iron）和软铁（soft iron）（具体的网上查资料吧，我也不是特别清楚）。

所以不像 accel 可以直接使用，用来作为标准来校正。那么其实有一种方法可以规避掉地磁不准的情况：就是不使用地磁的数值来校正，而且是用它作为一个参照。因为我们的目的，并不是指北，而是为了校正 y 轴旋转。

方法就是：

1. 选取一个时刻（t1）的朝向（Orientation）O 和地磁（mag） ，计算出世界坐标下的地磁向量 m，作为参考（Ref）。
2. 经过 dt 时间后，t2 时刻的朝向 O’ 和 地磁计算出此时世界坐标下的地磁向量 m’。
3. 理论上如果没有 y 轴旋转偏差，那么 m’ 和 m 应该是重合的。所以 m’ 和 m 的夹角就是偏差角（yawError）。
4. 经过一段时间后，检测作为 Ref 的 O 和当前 O’ 的夹角（偏差），如果大于某个阀值，那么用 O’ 和 m’ 作为新的 Ref。

细节处理

    1. 前面原理里用某个时刻的 Orientation 和 mag 作为 Ref。Orientation 是 gyro 计算出来的。但是 gyro 的采样率比 mag 要高很多（我们平台上 gyro 是 800MHz，mag 是 100MHz）。所以 Orientation 和 mag 无法直接一一对应上的。因此使用了一个 buffer 缓冲区来保存一定数量的 Orientation，当前时候的 mag 要获取 Orientation 的时候，根据一个回退算法获取前一个时刻的 Orientation（因为 mag 的采样率比 gyro 低，所以要往前找）：

1
2
3
4
5
6
7
8
9
10

enum
{
    MagMaxReferences = 1000,
    MagLatencyBufferSizeMax = 512,
    MagLatencyCompensationMilliseconds = 95,
};
// Determine how far to look back in buffer.
int backDist = (int) ((float) MagLatencyCompensationMilliseconds / (1000.0f * deltaT));

    2. 刚开始 yaw correct 的时候（头部跟踪模块初始化，或是休眠唤醒的时候），会等待一段时间（现在目前是 5s），等待 tile correct 将位置回正再开始 yaw correct。

    3. 如果当前的 Orientation 对应的 gyro 转动速度太快（有一个阀值），那么就不做 yaw correct，因为转动过快，yaw correct 已经没意义了（tile correct 也是一样的，运动中不校正）。

    4. 如果当前的地磁，投影到水平面的分量太小，则不校正：

1
2
3
4
5
6

// Verify that the horizontal component is sufficient.
if (magInWorldFrame.x * magInWorldFrame.x + magInWorldFrame.z * magInWorldFrame.z < minMagLengthSq)
{
    return;
}

剩下还有一些其它小细节，例如说 Ref 的一些分数取值，看下代码理解一下了。

朝向预测

朝向预测其实比较简单，原理就是通过上面的 fusion 算法，得到最后的朝向（Orientation）和转动角速度（AngularVelocity），然后给定预测的时间（pdt），在当前朝向的基础上加上 AngularVelocity * pdt 就是预测朝向了：

pose.Orientation = pose.Orientation * Quatf(angularVelocity, angularSpeed * dynamicDt)

预测算法的用途可以参看 ATW 的时序说明，简单来说就是应用渲染好的画面，送去显示，需要一段时间，如果按照实时朝向去渲染画面，那么最终在屏幕上看到的画面其实是之前一段时间的，就存在的延迟感。所以适当的预测，然后让应用使用未来的朝向来渲染，来补偿送显的延迟。

预测算法主要还是预测时间的选择上，例如说 ATW 选择的是 1.5 个 VSync。但是除了参数的自主选择，内部算法为了稳定性，还是做了一定的限制：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

const float linearCoef   = 1.0;
Vector3f angularVelocity = poseState.AngularVelocity;
float angularSpeed       = angularVelocity.Length();
// This could be tuned so that linear and angular are combined with different coefficients
float speed = angularSpeed + linearCoef * poseState.LinearVelocity.Length();
const float slope = 0.2; // The rate at which the dynamic prediction interval varies
float candidateDt = slope * speed; // TODO: Replace with smoothstep function
float dynamicDt         = predictionDt;
// Choose the candidate if it is shorter, to improve stability
// predictionDt < 0 meaning fatal error occured, never use it
if (predictionDt >= 0 && candidateDt < predictionDt) {
    dynamicDt = candidateDt;
}

限制了预测的时间范围（不允许过大），根据当前速度和加速进行限制。不过我们现在加速度没有计算，所以限制只和速度有关。

Sensor hub gyro 偏移校准

一般来说如果把设备静止放置，理想情况下，gyro 上报的数据应该是 0，但是真实的情况是会存在误差的。一般来说在某些外界因素下，底层采集的原始数据有可能存在一定情况的偏移，例如说下面这种情况（静置情况下）：

可以看到 gyro 底层采集到的数据很明显的偏移了一个数值（均值大概 0.02 rad/s 转化为角度大概是 1.14 deg/s），偏移很明显，这种情况下，头部跟踪融合出来的朝向数据，是会慢慢的超一个方向转动的。前面的 tile correct 和 yaw correct 均无法消除这种级别的偏差。

这种情况下，就需要底层采样上报的时候进行偏移校准。虽然这部分不属于上次头部跟踪，但是会影响到最后头部跟踪的最终效果，所以这里也调试分析了一下。google 提供的 contexthub（sensorhub）自带了一个动态的校准算法。

从上面的图可以看出来，其实校准的核心在于补偿静置时候的偏移误差。 就是计算出静置时候的 gyro 的数值（理论上来说这个时候 gyro 的值应该是0），然后在上报的时候减去这个补偿值就可以了。

校准流程

google contexthub gyro 动态校准流程是：

大概流程是：

1. 采集 gyro, accel, mag
2. 每次更新 gyro, accel, mag 到静置检测器（stillness detect），判断当前是否处于静置状态
3. 如果处于静置状态并且维持一段时间，那么就将这段时间内 gyro 的平均值作为校准补偿值。

上述流程是一直在都在运行，也就是说在不停的检测是否处于静置状态，如果处于静置状态一段时间，那么就更新校准补偿值。从上面的流程图可以看到，处于静置状态持续的时间有2个阀值判断，一个是 min_stillness_duration，一个是 max_stillness_duration。这个2个值的区别是：

min_stillness_duration:
之前处于静置状态，当前不处于静置状态了，那么从开始处于静置状态的时间开始算，到当前不处于静置状态，如果这段时间大于 min_stillness_duration ，那么触发更新校准补偿值操作。

max_stillness_duration:
之前处于静置状态，现在也是静置状态（一直处于静置状态），那么从开始处于静置状态时间开始，到当前的时间，大于 max_stillness_duration，那么触发更新校准补偿值操作。

每次更新校准值后，都会重置静置状态检测器的状态，然后重新判断。当第一次进入静置状态（通过判断 prev_still 这个 flag）的时候会记录开始静置的时间戳。

min_stillness_duration、max_stillness_duration 这2个值都是可以配置，我调试的 VR 平台根据调试的数据，选择的是 3s 和 4s。

静置检测流程

接下来看一下，这个算法里面，关于静置检测的算法（上面流程中的 stillness detect）。静置检测的算法流程大致如下：

检测流程为：

1. 采集 gyro、accel、mag
2. 更新 gyro、accle、mag 数据，确定一个窗口时间和超时时间（一段采样时间）
3. 在这个窗口时间（window_time_duration）内（不超时），对采样的数据计算方差（还可以计算平均值，用作后面的校准值）
4. 通过方差计算得到一个可信度得分（confidence），如果得分高于设定的阀值，那么认为是静置的

首先是采样窗口的确定。window_time_duration 这个值也是可以配置的，VR9 上配置的是 1.5s。当检测器状态被重置，会拿第一个采样到的数据的时间戳当做窗口开始时间（window_start_time），然后 window_start_time + window_time_duration 就是 window_end_time。超时是 2* window_time_duration。超时是什么意思呢？这里的静置检测使用到了 gyro、accel、mag 3个 sensor 的数据来判断。开始的时间是第一个采集到 gyro 的时间，如果这段时间内 accel、mag 一直没采集到数据（num_acc_samples < 1），那么就认为是超时，重置状态，重新开始一个新采样窗口。

上面提到了最后是通过一个分数来判断的。这个分数是通过方差计算出来的。这里方差公式不贴了，网上很多，有比较简单（平均值也是），贴一下分数的计算公式：

    1.设定一个方差的阀值，var_threshold，可以配置，VR9 配置为 0.00018 rad/s，还有一个方差的阀值变化范围值，threshold_delta，可以配置，VR9 配置为 0.00001 rad/s。根据这2个值可以得到2个范围上下限：

lower_var_thresh = var_threshold - threshold_delta
upper_var_thresh = var_threshold + threshold_delta

    2. 如果当前窗口的3轴方差（win_var_x, win_var_y, win_var_z）其中有一个轴高于 upper_var_thresh 那么 confidence 为 0。方差越小，代表变化越小，也就越说明静置不动，confidence 的取值为 [0, 1]，越接近1得分越高，就越代表可信（静置不动）。如果3轴方差全部小于 lower_var_thresh 则 confidence 为 1.

    3.如果不是上面2种情况，那么按照下面的公式来计算 confidence:

1
2
3
4
5
6
7
8
9
10
11

// Compute the stillness confidence score.
// Each axis score is limited [0,1].
tmp_denom = 1.f / (upper_var_thresh - lower_var_thresh);
gyro_still_det->stillness_confidence =
    gyroStillDetLimit(
        0.5 - (gyro_still_det->win_var_x - var_thresh) * tmp_denom) *
    gyroStillDetLimit(
        0.5 - (gyro_still_det->win_var_y - var_thresh) * tmp_denom) *
    gyroStillDetLimit(
        0.5 - (gyro_still_det->win_var_z - var_thresh) * tmp_denom);

上面的 gyroStillDetLimit 的作用就是限制输入参数的范围为 0 ~ 1.0f，即 < 0 的为0，> 1 的为 1.0f。

简单总结一下，上面的算法的核心就是通过一段时间的采样，然后计算方差（变化幅度），根据阀值来判断是否处于静置；处于静置，就使用这段时间的平均值作为新的补偿偏移（bias）；上报的数据减去这个偏移值完成校准。

参数配置

这里一共有好几个参数可以配置：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29

void gyroCalInit(struct gyroCal_t* gyro_cal, uint64_t min_still_duration,
             uint64_t max_still_duration,
             float bias_x, float bias_y, float bias_z,
             uint64_t calibration_time,
             uint64_t window_time_duration,
             float gyro_var_threshold,
             float gyro_confidence_delta,
             float accel_var_threshold,
             float accel_confidence_delta,
             float mag_var_threshold,
             float mag_confidence_delta,
             float stillness_threshold,
             int remove_bias_enable) 
gyroCalInit(&mTask.gyro_cal,
      3e9,     // min stillness period = 3 seconds
      4e9,     // max stillness period = 4 seconds
      0, 0, 0,   // initial bias offset calibration
      0,       // time stamp of initial bias calibration
      1.5e9,    // analysis window length = 1.5 seconds
      18e-5f,   // gyroscope variance threshold [rad/sec]^2 (0.00018)
      1e-5f,    // gyroscope confidence delta [rad/sec]^2   (0.00001)
      8e-3f,    // accelerometer variance threshold [m/sec^2]^2
      1.6e-3f,  // accelerometer confidence delta [m/sec^2]^2
      1.4f,     // magnetometer variance threshold [uT]^2
      0.25,     // magnetometer confidence delta [uT]^2
      0.88f,    // stillness threshold [0,1]
      1);       // 1=gyro calibrations will be applied

我调试的 VR 平台主要是调节了这几个参数：

min_stillness_duration, max_stillness_duration,
window_time_duration
gyro_var_threshold, gyro_confidence_delta
stillness_threshold

默认的参数，我调试的 VR 平台 gyro 静置的时候变化相对来说比较大，原来默认的参数，在某些机器上经常无法判断静置的条件（静置情况下）。所以经常无法更新校准值，会导致缓慢的偏移。所以我根据实际调试情况，适当的调整了阀值范围，让这套算法适应我调试的 VR 平台。

google 的 contexthub 其实 accel，mag 也是有校准，不过由于并没有出什么问题，所以就没去研究。而且 google 的 contexthub 也带有 fusion 算法（其实就是 framework sensorservice 的 fusion 算法移过去了），以后也可以研究一下。

参考资料

(1). HeadTrackingforOculusRift.pdf
(2). Vrbookbig.pdf: Tracking 章节
(3). 有关 openGL 矩阵变化的原理
(4). oculus ovr headtracking source code: ovr/VRLib/jni/hmd/sensor （这个是 oculus 早期的一个开源版本的 ovr sdk，后面这部分好像就不开源了）
(5). aosp context hub gyro calibrate source code: android/device/google/contexthub/firmware/src/algos