4

如何在CMRotationMatrix上应用过滤器？也许是卡尔曼滤波器。我需要修复 CMRotationMatrix (transformFromCMRotationMatrix) 的噪声，以获得结果矩阵的线性值

该矩阵值将转换为 XYZ，在我的情况下，我在 2D 屏幕上模拟 3D，如下所示：

// 将矩阵转换为 x, y

vec4f_t v;
multiplyMatrixAndVector(v, projectionCameraTransform, boxMatrix);

float x = (v[0] / v[3] + 1.0f) * 0.5f;
float y = (v[1] / v[3] + 1.0f) * 0.5f;

CGPointMake(x * self.bounds.size.width, self.bounds.size.height - (y * self.bounds.size.height));

代码：

// 定义变量

mat4f_t cameraTransform;

// 启动显示链接循环

- (void)startDisplayLink
{
    displayLink = [CADisplayLink displayLinkWithTarget:self selector:@selector(onDisplayLink:)];
    [displayLink setFrameInterval:1];
    [displayLink addToRunLoop:[NSRunLoop currentRunLoop] forMode:NSDefaultRunLoopMode];
}

// 停止显示链接循环

- (void)stopDisplayLink
{
    [displayLink invalidate];
    displayLink = nil;      
}

// 显示链接事件

- (void)onDisplayLink:(id)sender
{
    CMDeviceMotion *d = motionManager.deviceMotion;

    if (d != nil) {
        CMRotationMatrix r = d.attitude.rotationMatrix;

        transformFromCMRotationMatrix(cameraTransform, &r);
        [self setNeedsDisplay];
    }
}

// [self setNeedDisplay]之前的函数触发；

void transformFromCMRotationMatrix(vec4f_t mout, const CMRotationMatrix *m)
{    
    mout[0] = (float)m->m11;
    mout[1] = (float)m->m21;
    mout[2] = (float)m->m31;
    mout[3] = 0.0f;

    mout[4] = (float)m->m12;
    mout[5] = (float)m->m22;
    mout[6] = (float)m->m32;
    mout[7] = 0.0f;

    mout[8] = (float)m->m13;
    mout[9] = (float)m->m23;
    mout[10] = (float)m->m33;
    mout[11] = 0.0f;

    mout[12] = 0.0f;
    mout[13] = 0.0f;
    mout[14] = 0.0f;
    mout[15] = 1.0f;
}

// 矩阵向量和矩阵矩阵乘法例程

void multiplyMatrixAndVector(vec4f_t vout, const mat4f_t m, const vec4f_t v)
{
    vout[0] = m[0]*v[0] + m[4]*v[1] + m[8]*v[2] + m[12]*v[3];
    vout[1] = m[1]*v[0] + m[5]*v[1] + m[9]*v[2] + m[13]*v[3];
    vout[2] = m[2]*v[0] + m[6]*v[1] + m[10]*v[2] + m[14]*v[3];
    vout[3] = m[3]*v[0] + m[7]*v[1] + m[11]*v[2] + m[15]*v[3];
}
4

1 回答 1

1

一般来说，我会区分提高信噪比和平滑信号。

信号改善

如果您真的想比已经实施传感器融合算法的 Apple Core Motion 做得更好，请为结果不确定的长期项目做好准备。在这种情况下，您最好采用原始加速度计和陀螺仪信号来构建您自己的传感器融合算法，但您必须关心很多问题，例如漂移、不同 iPhone 版本的硬件依赖性、同一传感器的硬件差异一代，...所以我的建议：尽一切努力避免它。

平滑

这只是意味着插入两个或更多信号并建立一种平均值。我不知道有什么合适的方法可以直接用于旋转矩阵（也许有一个），但您可以使用四元数（更多资源：OpenGL Tutorial Using Quaternions to representation rotation或Quaternion FAQ）。

这种插值的结果四元数可以与您的向量相乘以获得类似于矩阵方式的投影（您可以查看为 iOS 设备查找法线向量以获取更多信息）。

表示旋转的两个单位四元数之间的插值可以用Slerp完成。在实践中，您将使用 Wikipedia 中描述为 Geometric Slerp 的内容。如果有两个时间点t1和t2以及对应的四元数q1和q2以及它们之间的角距离omega，则公式为：

q'(q1, q2, t) = sin((1- t) * omega) / sin(omega) * q0 + sin(t * omega) / sin(omega) * q1

t 应该是 0.5，因为你想要两个旋转之间的平均值。Omega 可以通过点积计算：

cos(omega) = q1.q2 = w1*w2 + x1*x2 + y1*y2 + z1*z2

If this approach using two quaternions still doesn't match your needs, you can repeat this by using slerp (slerp (q1, q2), slerp (q3, q4)). Some notes:

  • From a performance point fo view it's not that cheap to perform three sin and one arccos call in your run loop 1/frequency times per second. Thus you should avoid using too many points
  • In your case all signals are close to each other especially when using high sensor frequencies. You have to take care about angles that are very small and let 1/sin(omega) explode. In this case set sin(x) ≈ x
  • Like in other filters like low-pass filter the more points in time you use the more time delay you get. So if you have frequency f you will get about 0.5/f sec delay when using two points and 1.5/f for the double slerp.
  • If something appears weird, check that your resulting quaternions are unit quaternions i.e. ||q|| = 1
  • If you are running into performance issues you might have a look at Hacking Quaternions

The C++ project pbrt at github contains a quaternion class to get some inspiration from.

于 2012-07-17T18:13:42.663 回答