재시민

배경

스마트워치의 가속도 데이터는 필터링이 거쳐지지 않은 데이터다. 따라서, 노이즈 및 조그마한 변화에도 값이 확 튀게 된다. 이를 위해서 필터링 과정이 필요하다. 필터링을 위해 칼만필터를 사용하기로 했다.

칼만필터는 쉽게 말하면 이전 데이터들을 토대로 다음 데이터를 예측하는 필터다. 데이터가 추가될 수록 칼만필터의 필터링 변수들이 최신화되고 최신화된 변수들의 값을 통해 다음 데이터를 예측한다.

class KalmanFilter(q: Float,r: Float,p: Float,  x: Float, k: Float) {

    private var q: Float = q // process noise covariance
    private var r: Float = r // measurement noise covariance
    private var x: Float = x // estimated value
    private var p: Float = p // estimation error covariance
    private var k: Float = k // kalman gain

    fun update(z: Float): Float {
        // prediction update
        p = p + q

        // measurement update
        k = p / (p + r)
        x = x + k * (z - x)
        p = (1 - k) * p

        return x
    }
}

자세한 칼만필터 공식은 인터넷 검색하면 잘나온다 ~ ㅎㅎ 나는 이렇게 클래스를 만들어 사용했다.

하지만, 스마트워치의 가속도 센서가 안좋은지는 몰라도 움직임을 트래킹하는데 있어 정확한 자세가 측정되지 않았다.

문제 : 적분 드리프트

가속도 데이터를 이용해 트래킹을 하려면 이동거리 데이터로 이중적분을 해야한다. 하지만, 적분의 기초를 알게되면 적분시 적분상수값이 생성되는 것을 알 수 있다.

두번의 적분을 하게된다면 두개의 적분상수가 생성되므로 이게 누적되어 드리프트 (원래 값보다 값이 상승 혹은 하락) 현상이 발생한다.

이를 해결하기 위해 여러 방법을 고안했다.

첫번째 방법 : ZUPT 알고리즘

ZUPT(Zero velocity UPdaTe) 영속도 보정 알고리즘을 이용해보았다. 대충 설명해보자면 일정 임계값내의 가속도 데이터 값은 정지상태임을 가정하고 속도를 0으로 계산하는 방법이다.

필터링연구를 하면서 ‘정지 상태’를 인지해야함이 중요하다는 것을 알게되었다. 가만히 있어도 가속도데이터는 측정되기 때문에 오차가 발생할 확률이 증가한다.

따라서, ZUPT 알고리즘을 도입했고 아래 코드와 같이 제작해보았다.

if ((accX < threshold && accX > -threshold) && (accY < threshold && accY > -threshold) && (accZ < threshold && accZ > -threshold))
        {
            x += veloX * dT;
            y += veloY * dT;
            z += veloZ * dT;

            veloX = 0f;
            veloY = 0f;
            veloZ = 0f;
        }

두번째 방법 : 모델의 움직임 제한

위의 방법들을 통해서도 드리프트 현상은 해결하지 못했다. 나의 능력 부족이라 생각했다. 하지만, 전시회에서 시연을 해야하므로 일단 해결은 해야했다. 따라서, 모델의 움직임을 제한하고 운동기구가 손에서 벗어나지 않는 범위내에서 움직이게 한다면 그럴싸하게 보일 것 이라 생각했다.

따라서, 어깨축을 중심으로 구형태의 범위로 제한하다면 좋은 결과물이 나올것이라고 생각이 들었다.

if (distance > radius)
{
      // 오브젝트를 구의 표면에 놓아서 원점에서 지정된 반지름만큼 떨어뜨립니다.
            // moveObeject 는 바벨
      Vector3 fromOriginToObject = moveObject.transform.position - initPos;
      Vector3 fromOriginToObject2 = moveObject2.transform.position - initPos2;
      fromOriginToObject *= radius / distance; // 거리를 반지름으로 정규화
      fromOriginToObject2 *= radius / distance; // 거리를 반지름으로 정규화
      moveObject.transform.position = initPos + fromOriginToObject;
      moveObject2.transform.position = initPos2 + fromOriginToObject2;

            //범위가 radius 값을 초과하면 값을 끝값에 초기화
            x = moveObject.transform.position.x;
            y = moveObject.transform.position.y;
            z = moveObject.transform.position.z;
}

움직인 범위를 빨간공으로 나타내어 봤는데 딱 구형태로 잘 움직인는 것을 보니 뿌듯했다.

하지만

위의 해결책은 잠시나마 꼼수로 한 것이다. 너무 너무 아쉽지만 나중에 IMU 센서 혹은 MPU6050 을 이용해 나의 해결책이 틀린건지 센서가 이상한건지 교차검증 해보고는 싶다.