这个图证明：

二、罗德里格旋转矩阵

我们前面推导和证明了罗德里格旋转公式，并知道罗德里格旋转公式也可以写成矩阵形式，这个矩阵就叫罗德里格旋转矩阵。矩阵 公式的推导可以参考：

http://www.cnblogs.com/xpvincent/archive/2013/02/15/2912836.html

两个向量叉乘即可得到旋转轴，然后求出旋转角度，由这两个因素便可以求出旋转矩阵。

求旋转矩阵的时候，先求得旋转轴模长，然后将旋转轴向量除以模长，便得到旋转轴的单位向量。

利用旋转轴的单位向量和旋转角度，通过公式便可以得到旋转矩阵：

三、罗德里格旋转矩阵公式在VR中的应用

Matrix.multiplyMM(mRightEye.getTransform().getEyeView(), 0,mRightEyeTranslate, 0, mHeadTransform.getHeadView(), 0);

// 获取头部姿态矩阵mHeadTracker.getLastHeadView(mHeadTransform.getHeadView(), 0);

其实现为：

      public void getLastHeadView(float[] headView, int offset){if (offset + 16 > headView.length) {throw new IllegalArgumentException("Not enough space to write the result");}//如果定义了卡尔曼滤波对象synchronized (mTracker) {//首先计算出当前时间距离上次预测时间的差值double secondsSinceLastGyroEvent = (System.nanoTime() - mLastGyroEventTimeNanos) * 1.E-09D;Log.d(TAG,"secondsSinceLastGyroEvent="+secondsSinceLastGyroEvent);//实际上有60帧每秒，30帧算的话，一帧33ms，所以这里是提前预测一帧//然后加上提前预测的时间33msdouble secondsToPredictForward = secondsSinceLastGyroEvent + 0.03333333333333333D;//这里返回的是一个double类型的数组，double[] mat = mTracker.getPredictedGLMatrix(secondsToPredictForward);for (int i = 0; i < headView.length; i++) {mTmpHeadView[i] = ((float)mat[i]);}}Matrix.multiplyMM(headView, offset, mTmpHeadView, 0, mEkfToHeadTracker, 0);}

可以看到这里计算了当前时间距离上次预测时间的差值，然后加了一个提前预测时间33ms，之后调用了getPredictedGLMatrix

      public double[] getPredictedGLMatrix(double secondsAfterLastGyroEvent){//deltaT,时间差double dT = secondsAfterLastGyroEvent;Log.d(TAG,"dT="+dT);Vector3d pmu = getPredictedGLMatrixTempV1;//应该理解为向量乘以一个常量，这个常量就是提前预测的时间，但为什么乘以一个负的呢？Log.d(TAG,"lastGyro=("+Float.toString(lastGyro[0])+","+Float.toString(lastGyro[1])+","+Float.toString(lastGyro[2])+")");pmu.set(lastGyro[0] * -dT, lastGyro[1] * -dT, lastGyro[2] * -dT);Matrix3x3d so3PredictedMotion = getPredictedGLMatrixTempM1;//Log.d(TAG,"so3PredictedMotion="+so3PredictedMotion.toString());//这里返回的so3PredictedMotion就是一个Rodrigues矩阵So3Util.sO3FromMu(pmu, so3PredictedMotion);Matrix3x3d so3PredictedState = getPredictedGLMatrixTempM2;//Log.d(TAG,"getPredictedGLMatrix,so3SensorFromWorld="+so3SensorFromWorld.toString());Matrix3x3d.mult(so3PredictedMotion,so3SensorFromWorld,so3PredictedState);//so3PredictedState还是一个3x3的矩阵，这里把它放到一个4x4矩阵中return glMatrixFromSo3(so3PredictedState);}

      //这个函数封装rodrigues算法，只不过这里根据角度的大小//来调节kA和kB的值。角度比较大的时候，和公式上比起来，KA多除了一个θ，kB多除了一个θ平方，不知道原因是什么根据。public static void sO3FromMu(Vector3d w, Matrix3x3d result){//向量点乘等于w向量模长的平方double thetaSq = Vector3d.dot(w, w);//如果这里是求theta，就要用点积再除以两者的模再求arccos//开平方，就是求得w向量的模长double theta = Math.sqrt(thetaSq);Log.d(TAG,"w=("+w.x+","+w.y+","+w.z+")"+",theta="+theta);double kA, kB;if (thetaSq < 1.0E-08D) {//Log.d(TAG,"thetaSq < 1.0E-08D");kA = 1.0D - 0.16666667163372D * thetaSq;kB = 0.5D;}else{if (thetaSq < 1.0E-06D) {//Log.d(TAG,"thetaSq < 1.0E-06D");kB = 0.5D - 0.0416666679084301D * thetaSq;kA = 1.0D - thetaSq * 0.16666667163372D * (1.0D - 0.16666667163372D * thetaSq);} else {//Log.d(TAG,"thetaSq > 1.0E-06D");double invTheta = 1.0D / theta;kA = Math.sin(theta) * invTheta;kB = (1.0D - Math.cos(theta)) * (invTheta * invTheta);}}//根据公式，w是一个单位向量，因为每一项都要除以一个模长，所以这里除以模长就放到了kA和kBrodriguesSo3Exp(w, kA, kB, result);}

函数sO3FromMu其实就是对罗德里格矩阵求法的一个封装，其中rodriguesSo3Exp函数就是罗德里格矩阵：

      private static void rodriguesSo3Exp(Vector3d w, double kA, double kB, Matrix3x3d result){//求得向量的平方double wx2 = w.x * w.x;double wy2 = w.y * w.y;double wz2 = w.z * w.z;//result.set(0, 0, 1.0D - kB * (wy2 + wz2));result.set(1, 1, 1.0D - kB * (wx2 + wz2));result.set(2, 2, 1.0D - kB * (wx2 + wy2));double a, b;a = kA * w.z;b = kB * (w.x * w.y);result.set(0, 1, b - a);result.set(1, 0, b + a);a = kA * w.y;b = kB * (w.x * w.z);result.set(0, 2, b + a);result.set(2, 0, b - a);a = kA * w.x;b = kB * (w.y * w.z);result.set(1, 2, b - a);result.set(2, 1, b + a);}

      public double[] getPredictedGLMatrix(double secondsAfterLastGyroEvent){//deltaT,时间差double dT = secondsAfterLastGyroEvent;Log.d(TAG,"dT="+dT);Vector3d pmu = getPredictedGLMatrixTempV1;//应该理解为向量乘以一个常量，这个常量就是提前预测的时间，但为什么乘以一个负的呢？Log.d(TAG,"lastGyro=("+Float.toString(lastGyro[0])+","+Float.toString(lastGyro[1])+","+Float.toString(lastGyro[2])+")");pmu.set(lastGyro[0] * -dT, lastGyro[1] * -dT, lastGyro[2] * -dT);Matrix3x3d so3PredictedMotion = getPredictedGLMatrixTempM1;//Log.d(TAG,"so3PredictedMotion="+so3PredictedMotion.toString());//这里返回的so3PredictedMotion就是一个Rodrigues矩阵So3Util.sO3FromMu(pmu, so3PredictedMotion);Matrix3x3d so3PredictedState = getPredictedGLMatrixTempM2;//Log.d(TAG,"getPredictedGLMatrix,so3SensorFromWorld="+so3SensorFromWorld.toString());Matrix3x3d.mult(so3PredictedMotion,so3SensorFromWorld,so3PredictedState);return glMatrixFromSo3(so3PredictedState);}

四、罗德里格矩阵与卡尔曼滤波的配合