Drone's DIYer

가속도계(Accelerometer)는 움직임이 있을 때마다 값이 튈 수 밖에 없는데 이는 움직임에 따른 가속도와 중력가속도가 합해져 마치 중력가속도 방향이 변한 것처럼 보이기 때문입니다. 이러한 고주파 잡음을 제거하기 위해서는 저역 통과필터(Low pass filter; LPF)를 사용해야 합니다. 반면에 자이로(Gyroscope)는 잡음이 긴 시간에 대해서 누적(적분)되어 드리프트(혹은 바이어스)되므로 짧은 시간인 고역 통과필터(High pass filter; HPF)를 사용해야 합니다. 이러한 아이디어를 구체화한 것이 아래의 블록선도와 같으며 이를 IMU(Inertia Measurement Unit) 데이터를 융합하는데 가장 간단한 방법인 보상필터(Complementary filter) 혹은 상보필러라 부릅니다.

여기서 과 는 각각 1차 저역 통과필터, 1차 고역 통과필터로 그 합은 안정성의 문제로 '1'이 성립되어야만 합니다. 2차 필터도 구현할 수 있지만 편의상 1차 필터로만 제한합니다. 위 그림으로부터 수식을 정리하면 다음과 같습니다.

여기서 는 차단주파수(cut off frequency)를 결정합니다. 위 식을 정리하면 다음과 같습니다.

위 수식으로 블록선도를 다시 그리면 다음과 같습니다.

위 수식을 MCU로 처리하기 위해서는 연속시간에 대한 이산화가 필요하고 하드웨어 상에 아날로그 혹은 디지털 필터(예를 들어 IIR, FIR 등)를 사용하지 않는 이상 코드로 구현해야 합니다. MCU 성능 개선으로 외장 디지털 필터나 디지털 필터 알고리즘을 사용하지 않고 수식을 이산화하여 구현하였습니다.

동일한 연속 시간 전달 함수(continous time transfer function) G(s)를 이산화하는 방법에는 여러가지가 있으며 안정도와 고주파 잡음 등과 같은 성능이 동일하지는 않습니다. 전형적으로 연속 시간 전달 함수(continous time transfer function) G(s)를 이산(불연속 시간 전달 함수(discrete time transfer function) H(z)로 변환하는 방법은 전형적으로 다음과 같은 방법이 있습니다.

1) 후방 차분법(backward difference) : 

2) Bilinear transformation, expansion of ln(z) : 

3) Impulse invariance transformation : 

가장 기본적인 후방 차분법(backward difference)을 적용하면 이므로 이를 대입하여 다시 정리하면 추정 각도 는 다음과 같이 나타낼 수 있습니다.

여기서 편의상 가속계인 accelerometer의 첫글자 , 자이로 측정치로부터의 각속도(rotation rate)라 놓았습니다.

위 식에 z 변환인 은 이산 시스템에서 이므로 위 식을 이산 신호 시스템으로 변경하면 다음과 같습니다. 다른 말로 는 단위 시간 지연(unit time delay)과도 같기 때문입니다.

위 식에서 이면 다음과 같습니다.

정성적으로 위 수식에서 만일 가속도계에 측정치가 튄다면 즉, 고주파 잡음이 들어갔다면, 0.01이 곱하여져 새로운 각도가 계산됩니다. 그러나 0.01이 곱하여지므로 기여도는 작아 각도는 프로그램 루프(loop)가 상당히 반복되어도 매우 천천히 증가하게 됩니다. 이는 가속도계 입력이 추정 각도 계산에 실시간으로 반영되기 어렵고 이는 출력이 반응하지 않는다는 의미로 고주파 잡음은 제거되고 서서히 증감되는 저주파 측정치만 반영되게 된다는 의미입니다.

는 weight factor라 부르며 통상 0.95~0.98 범위의 값을 사용합니다.

Appendix A.

Reference:

http://d1.amobbs.com/bbs_upload782111/files_44/ourdev_665531S2JZG6.pdf

http://www.olliw.eu/2013/imu-data-fusing/#chapter21

http://www.dummies.com/education/science/signals-and-systems-working-with-transform-theorems-and-pairs/(z-transform)

아두이노(Arduino) 환경에서 전형적인 MPU6050 센서 입력에 사용되는 칼만 필터(Kalman filter)의 예제입니다.

http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/#comment-57783

또한 이 칼만 필터는 가속도계(accelerometer) 혹은 지자계(magnetometer)와 그리고 자이로(gyroscope)로부터 각도(angle), 각속도(rate) 그리고 bias를 계산하는데 사용될 수 있습니다(C++ version).

https://github.com/TKJElectronics/KalmanFilter

다음은 C version입니다.

http://www.cnblogs.com/zjutlitao/p/3915786.html

Kalman.h

Kalman.h

아두이노(Arduino) 환경에서 전형적인 MPU6050 센서 입력에 사용되는 칼만 필터(Kalman filter)의 예제입니다.

http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/#comment-57783

Step 1)

위 식에서 각도(angle)의 추정치 는 이전 상태의 추정치 에 bias 되지 않은 각속도(rate)에 곱하기 미소 시간 를 더한 것과 같습니다. bias 되지 않은 각속도(rate)에 곱하기 미소 시간 은 결국 드리프트 되지 않은 각도의 증분이 됩니다. 또한 좌측항은 아직 보정되지 않았음을 기억합니다.

게다가 우리는 bias를 직접 측정할 수 없기 때문에 bias의 추정치는 이전 것을 사용합니다.

이는 다음과 같이 C 언어로 쓸 수 있습니다.

Step 2)

위 식은 다음과 같이 C 언어로 쓸 수 있습니다.

Step 3)

참고로 우측에 현재 상태변수는 보정되지 않았기 때문에 angle 변수를 그대로 사용합니다.

위 식은 다음과 같이 C 언어로 쓸 수 있습니다.

Step 4)

위 식은 다음과 같이 C 언어로 쓸 수 있습니다.

Step 5)

다른 경우에 S는 행렬이 될 수 있습니다. 그 경우에는 여러분은 간단히 S로 P를 나눌 수 없으며 역행렬을 구해서 곱해야 합니다.

위 식은 다음과 같이 C 언어로 쓸 수 있습니다.

Step 6)

위 식은 다음과 같이 C 언어로 쓸 수 있습니다.

Step 7)

상태 추정 오차가 감소되었기 때문에 오차 공분산 행렬을 다시 감소시킴을 기억하세요.

C 코드는 다음과 같습니다.

참고로 대부분의 IMU에 대해서 다음의 변수들이 완벽하게 동작합니다.

초기치로서 각도를 설정하는 것을 기억하세요 왜냐하면 필터가 안정화되는데 시간이 걸리기 때문입니다. 반대로 칼만 필터가 안정화되기 전까지는 상태의 추정치를 믿을 수 없습니다.

아두이노(Arduino) 환경에서 전형적인 MPU6050 센서 입력에 사용되는 칼만 필터(Kalman filter)의 예제입니다.

http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/#comment-57783

한편 의 관측 혹은 측정을 위한 식은 다음과 같이 주어집니다.

위 식에서와 같이 는 현재 상태 가  행렬과 곱하여지고 측정 잡음 와 합하여 집니다.

는 관측 모델로 불려지며 실제 상태 공간을 관측된 상태의 공간으로 매핑하기 위해서 사용되어집니다. 실제 상태는 관측될 수 없는데 이는 측정이 단지 가속도계로부터의 측정에 불과하기 때문입니다.

는 다음과 같습니다.

측정 잡음은 게다가 '0'의 평균과 다음과 같이 의 공분산을 가져야 합니다. 

그러나 은 행렬이 아니기 때문에 측정 잡음은 단지 측정값의 분산과 같습니다. 왜냐하면 같은 변수의 공분산은 분산과 같기 때문입니다. 그러므로 우리는 R을 다음과 같이 쓸 수 있습니다.

우리는 측정 잡음은 동일하고 시간 k에 의존하지 않는다고 가정합니다.

만일 여러분이 측정 잡음 분산 을 너무 높게 설정하면 필터는 너무 늦게 응답하는데 이는 새로운 측정값을 덜 믿기 때문이며, 반면에 너무 낮게 설정한다면 값은 오버슈트(overshoot)가 발생하고 잡음이 많은데 이는 우리가 가속도계 측정값을 너무 많이 믿기 때문임을 참고하세요.

그래서 여러분은 프로세스 잡음 분산 와  그리고 측정 잡음 의 측정 분산을 찾아야만 합니다.

Time Update("Predict")

처음 2개의 방정식에서 시간 k에서 우리는 현재 상태와 오차 공분산 행렬을 예측할 것입니다. 우선 필터는 모든 이전 상태들과 자이로 측정으로부터 근거 된 현재 상태를 추정할 것입니다.

위 식의 우측 두번째 항을 제어 입력이라고 부르는 이유는 우리가 현재 시간 k에서 상태를 추정하기 위해 이를 추가적인 입력으로 사용했기 때문입니다. 다음은 우리가 이전 오차 공부산 행렬 에 근거하여 다음에 정의된 것처럼 오차 공분산 행렬 을 추정할 것입니다.

이 행렬은 추정된 상태의 현재 값을 얼마나 많이 신뢰하는가를 추정하기 위해서 사용되어집니다. 작을수록 우리는 좀더 현재 추정된 상태를 신뢰하게 됩니다. 위 방정식의 원리는 실제로 이해하기에 매우 쉽습니다. 이는 우리가 상태의 추정을 마지막 갱신한 후에 오차 공분산은 증가하는 것이 매우 분명하기 때문입니다. 그러므로 우리는 오차 공분산 행렬을 상태 전이 모델 과 그 행렬의 전치행렬 을 곱하고 시간 k에서 현재 프로세스 잡음 를 더합니다.

오차 공분산 행렬 는 2x2 행렬입니다.

Measurement Update("Correct")

우리가 계산해야 할 첫번째 일은 측정치 와 보정되지 않은 이전 상태로부터 예측된 상태 변수 사이에 차를 계산하는 것입니다.

여기서 관측 모델 는 이전 상태 추정치를 관측된 공간과 매핑하게 위해서 사용되어집니다. 관측 공간이라 함은 가속도계로부터의 측정치로 그러므로 위 식은 행렬이 아닙니다.

다음에 할 것은 를 계산하는 것입니다.

이전 오차 공분산 행렬 와 측정 공분산 행렬 에 근거한 측정을 얼마나 믿을 수 있는지 예측하는 것입니다. 관측 모델 는 이전 오차 공분산 행렬 를 관측 공간으로 매핑하기 위해서 사용되어집니다.

큰 측정 잡음은 값을 크게 합니다. 이는 입력되는 측정값을 그렇게 많이 신뢰할 수 없음을 의미합니다. 이 경우 는 행렬이 아니고 다음과 같이 쓸 수 있습니다.

다음 단계는 칼만 이득(Kalman gain)을 계산하는 것입니다. 칼만 게인은 측정치 와 이전 상태 변수와의 차이를 얼마나 신뢰할 수 있는지를 나타내는데 사용되어집니다.

만일 우리가 그 차이를 도저히 신뢰하지 못한다면, 차이에 대한 공분산 는 높게 될 것입니다. 그리고 만일 우리가 추정된 상태를 신뢰한다면 오차 공분산 행렬 는 작게 될 것이고 칼만 이득도 작아질 것입니다. 반대로 차이를 신뢰하지만 현재 상태의 추정치를 믿지 못한다면 반대가 될 것입니다.

여기서 관측 모델 H의 전치행렬이 오차 공분산 행렬 P를 관측 공간으로 매핑하는데 사용되어짐을 알 수 있습니다. 그다음 우리는 공분산 S로 나누어 오차 공분산 행렬을 비교합니다.

이는 관측 모델 H를 상태 오차 공분산을 빼기 위해서 그리고 S의 현재 추정치와 비교하기 때문에 의미가 있다는 것입니다. 만일 여러분이 초기에 상태를 모른다면 오차 공분산 행렬을 다음과 같이 설정할 수 있다.

여기서 은 큰 숫자를 대표하며 초기에 상태가 알려진다면 오차 공분산 행렬을 다음과 같습니다.

칼만 이득은 2x1 행렬입니다.

우리는 추정된 현재 상태를 보상합니다.

이는 yk와 칼만 이득을 곱하여 보상되지 않은 현재 상태에 더하는 것입니다.

는 측정된 와 보상되지 않은 추정된 현재 상태의 차이로 양수나 음수가 될 수 있습니다.

가장 간단화된 방정식은 보상되지 않은 현재 추정 상태 를 가속도계 측정값으로 간단하게 바로잡는 것으로 이해할 수 있습니다.

여기서 는 이전 상태와 자이로 측정값으로부터 계산되어진 것입니다.

마지막으로 우리는 보상되지 않은 오차 공분산 행렬을 갱신하는 것입니다.

여기서 I는 항등행렬입니다.

필터가 하는 일은 기본적으로 얼마나 많이 우리가 추정치를 바로잡았는가에 근거하여 오차 공분산 행렬을 스스로 바로잡는 것입니다. 이것은 우리가 이전의 오차 공분산 과 공분산 에 근거된 상태를 바로잡았기 때문에 의미가 있다는 것입니다. 

아두이노(Arduino) 환경에서 전형적인 MPU6050 센서 입력에 사용되는 칼만 필터(Kalman filter)의 예제입니다.

http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/#comment-57783

MPU6050은 가속도와 자이로센서가 1개의 센서에 모두 포함하고 있는 6DOF(Degrees of Freedom) 센서로, I2C(Inter Integrated Circuit) 통신 프로토콜을 통해서 데이터를 추출 할 수 있습니다. 위 모듈은 MPU6050을 포함하며 사용하기 쉽게 만든 모듈입니다. 

MPU6050의 간단한 스펙입니다.

가속도계(accelerometer)는 움직임이 있을때 일반적으로 많은 잡음(noise)을 포함합니다. 자이로(gyroscope)의 문제는 시간이 누적되면 DC 성분(혹은 이를 bias라 표현)이 누적된다는 것입니다. 결론적으로 자이로는 짧은 시간에 믿을 수 있지만, 반면 가속도계는 긴 시간에 더 신뢰성이 있다는 것입니다.

그렇기 때문에 상보 필터(complimentary filter, 혹은 보상 필터)의 적용이 쉽습니다. 기본적으로 가속도계에 대해서 디지털 low pass filter(LPF), 자이로에 대해서는 디지털 high pass filter(HPF)를 적용하는 것입니다. 그러나 칼만 필터(Kalman filter) 만큼 정확하지는 않다는 것입니다. 근래에 아두이노를 이용하여 처리가 가능하지만 칼만 필터는 아두이노가 주로 제공하는 MCU에서는 무겁다는 단점이 있습니다.

k 시간에 시스템의 상태는 다음과 같습니다.

여기서 는 상태 행렬로 다음과 같습니다.

는 각도 의 시간에 따라 누적된 양으로 bias라고 하며 자이로에서 측정된 값에서 이 bias를 빼는 것에 의하여 현재의 각도를 얻을 수 있습니다.

그러므로 이전 상태 로부터의 전이 행렬 는 다음과 같이 쓸 수 있습니다.

는 제어 입력으로 k 시점에서 자이로의 측정값으로 [˚/s]의 단위를 가지며 이 로 상태방정식을 다음과 같이 다시 쓸 수 있습니다.

여기서 는 제어 입력 행렬로 다음과 같이 나타낼 수 있습니다.

는 프로세스 잡음으로, 주어진 k 시간에서 '0'의 평균과 의 공분산(covariance)을 갖는 가우시안 분포(Gaussian distribution)(혹은 정규분포)를 갖습니다.

는 프로세스 잡음 공분산 행렬이고, 이 경우에 가속도계와 bias의 상태 추정치 공분산 행렬이 됩니다. 우리는 bias와 가속도계의 추정이 서로 독립적임을 가정합니다.

위 식에서 처럼  공분산 행렬은 현재 시간 k에 의존합니다. 따라서 가속도계 분산 와 bias의 분산 은 미소 시간 에 곱하여집니다.

프로세스 잡음은 시간이 길어질수록 커지게 됨을 의미합니다. 이는 상태의 마지막 업데이트이기 때문입니다. 예를 들어 자이로는 드리프트(drift) 할 수 있습니다. 우리는 칼만 필터의 동작을 위해서 이들 상수들을 알아야 합니다. 만일 여러분이 큰 값을 설정한다면 상태 추정에 잡음이 커진다는 것을 기억해야 합니다. 예를 들어, 추정 각도가 드리프트 하기 시작하면 여러분은 의 값을 증가시켜야 합니다. 반면에 추정이 여러분이 생각하는 것보다 너무 많이 느려진다면 반응이 빠르도록 의 값을 감소시켜 볼 수 있습니다.

루프만 칼만(Rudolf E. Kalman)이 1960년대 초 개발한 필터(filter)로 과거의 정보와 새로운 측정값을 사용하여 측정값에 포함된 잡음(noise)를 제거시켜 추정(estimate)(최적의 값을 추적)하는데 사용하는 알고리즘(algorithm)입니다. 선형적(linear) 움직임을 갖는 대상에 재귀적(reculsive)으로 동작시킵니다. 이는 누적된 과거 데이터와 현재 얻을 수 있는 최선의 측정치로 현상태를 추정하고자 함으로 결국, 자연계의 움직임은 어느 정도 예측가능하고 일반적 움직임 물성을 갖기 때문입니다.

여기서 선형시스템은 행렬연산을 가능하게 만들기 때문이며, 잡음은 시간에 영향이 없는 백색잡음(white noise)이고 Gaussian 분포(정규분포; normal distribution)를 따라야 하는데 이는 평균과 공분산으로 정확히 모델링이 가능하기 때문입니다. 여기서 LPF(low pass filter)나 HPF(high pass filter)처럼 물리적 필터가 아닌데도 불구하고 필터로 불리는 것은 컴퓨터로 이산신호처리시 시간영역에서 필터링과 같기 때문입니다. 참고로 선형시스템이란 특정 시점에서의 상태는 이전 시점의 상태와 선형적인 관계를 가지고 있는 경우를 말합니다.

만일 대부분의 자연계의 속성처럼 시스템이 비선형이고 잡음도 Gaussian 분포를 따르지 않는 경우가 많으므로 칼만필터의 변형이 요구되는데 이때 확장 칼만필터(Extended Kalman Filter; EKF)가 오류는 있지만 가장 많이 사용되고 있으며, EKF는 선형화 칼만필터(Linearized KF)와 유사하나 선형화하는 기준점을 계속 갱신해 나간다는 특징이 있습니다. UKF 참고

재귀적 자료 처리(recursive) 방식은 과거 데이터의 평균을 알기 위해서 엄청난 양의 데이터를 저장하고 갱신할 때마다 모두 계산하는 방법이 아닌 과거값과 현재의 값만 기억하여 전체 평균을 알고자 함입니다. 예를 들어, k개의 값이 입력되었다면 현재까지의 평균 Y(k)=Y(k-1)*((k-1)/k) + (1/k)*Vk로 표현할 수 있습니다. 여기서 Vk는 k번째 입력이고 k-1번째의 평균값 Y(k-1)만 알면 전체 평균을 알 수 있다는 것입니다.

만일 입력된 2개의 데이터, V1, V2의 평균은 (V1+V2)/2인데 이 경우 두 데이터의 중요도는 같습니다. 그러나 한 데이터가 잡음에 가까워 튀었다면 평균을 계산하는데 가중치가 달라져야 할 것입니다. 평균에서 얼마나 벗어났는가를 나타내는 지표는 표준편차이고 각 데이터의 중요도는 표준편차의 제곱에 반비례합니다. 그러므로 표준편차가 크면 클수록 그 값의 중요도는 떨어지고 평균에 대한 기여도도 적어져야 할 것입니다.

이와 같이 연속적인 이전의 출력값과 새로운 입력값을 중요도를 감안하여 과거와 현재의 2개 데이터로 평균을 구할 수 있고 새로운 최적값(optimized state variables)을 계산하는 측정갱신 알고리즘(measurement update algorithm)의 형태가 만들어지는데 이것이 칼만필터이고 기본식은 일반적으로 행렬(Matrix) 혹은 벡터(Vector) 형태로 나타냅니다.

선형시스템 기술할 때 일반적으로 아래의 2개의 간단한 수식으로 표현됩니다. 첫번째는 상태방정식(state equation)으로 시스템의 신호 파형을 표현하며 'Process Model' 혹은 'Plant Model'라고도 부릅니다. 두번째는 출력방정식으로(output equation) 시스템 신호 중 측정 가능한 값들을 표현하며, 'Measurement Model', 혹은 'Sensor Model'라고 부릅니다.

A는 이전 상태에 기반한 상태 전이 행렬이고 H는 해당 시간에 측정에 관계된 행렬이며, k는 시간, x는 시스템의 상태를 나타내는 벡터(Vector)입니다. 는 센서 등을 이용해서 측정된 값을 의미하며, w, v는 noise 혹은 error입니다. 특히 w는 프로세스 혹은 시스템 잡음(Process Noise), v를 측정 잡음(Measurement Noise)라고 부르며 평균이 '0'인 정규분포입니다. x, z, w, v는 시스템을 설명하는 대부분 다변수이기 때문에 벡터입니다. 여기서 초기 상태와 각 잡음은 상호 독립이어야 합니다.

시스템의 상태벡터 x는 시스템의 현재 상태에 대한 모든 정보, 예를 들어 비행체의 위치 정보와 각도, 가속도 등을 나타냅니다. 하지만 중요한 것은 이들을 직접적으로 측정할 수가 없어서 대신에 측정 잡음 v를 포함하는 센서의 출력을 이용하는 것입니다. 다음은 시스템의 현재 상태 대를 구하는 위한 칼만 필터의 알고리즘 입니다.

위 그림에서 'Time Update'는 상태방정식으로 이전 데이터를 근거고 예측하는 부분이고, 'Measurement Update'는 측정 모델(Measurement model)로써 새로운 측정값으로 교정하는 부분입니다.

x와 P는 초기예측이 필요하고 K는 칼만 이득, P는 오차 공분산 행렬(error covariance matrix), Q는 시스템 잡음(, 위 그림에서 )의 공분산 행렬이고 상태 의 크기가 n*1이면 n*n 크기의 대각행렬이고, R은 상수값으로 측정 잡음()의 공분산 행렬로 센서 제조사가 제공하기도 합니다. x위의 ^(hat)의 의미는 추정된 'estimated(priori state)'이라는 의미이며, 첨자 '-'는 아직 보정되지 않음을 뜻합니다. 

, 의 초기치를 결정합니다.

Time Update:

(1) 으로부터 현재 상태변수를 예측합니다.

(2) 오차 공분산을 예측합니다.

추정 오차(estimation error)는 다음과 같이 유도할 수 있습니다.

그러므로 공분산 행렬은 다음과 같습니다. 여기서 , 입니다.

위 식에서 두번째와 세번째 항은 시스템 잡음()과 추정 오차의 공분산인데 시스템 잡음은 백색잡음으로 추정 오차와 상관성이 없으므로(uncorrelated) '0'이 됩니다.

Measurement Update:

(1) 칼만 이득을 구합니다.

시스템에 대한 추정으로부터 오차 공분산을 구했으면 추정에 대한 불확실성을 표현할 수 있는 가장 간단하면서 강력한 방법 중에 하나는 Gaussian 분포로 표현하는 것입니다. 는 의 추정에 대한 평균과 분산을 가지는 Gaussian 확률 밀도 함수(Probability Density Function, PDF)로 정의할 수 있습니다. 만약 Gaussian PDF로 추정을 한다면, 가 r 에 위치할 확률은 다음과 같습니다.

Gaussian PDF를 이용하여 프로세스 오차 및 측정 오차를 표현하며, 이 Gaussian PDF는 매 state 마다 를 추정할 때와 측정할 때 각각 따로 존재하고, 최종 추정 값은 이 두 Gaussian PDF의 합성(fusion)을 이용해야 한다는 것입니다. 여기서 Gaussian PDF의 중요한 속성을 이용하는데, 서로 다른 두 개의 Gaussian PDF를 합성하면 또 다른 형태의 Gaussian PDF가 만들어진다는 것이다. 우리는 이러한 특성을 이용해 복잡도를 증가시키지 않고 매 state에 대한 Gaussian PDF를 계산할 수 있다는 것입니다.

위 그림에서와 같이 추정에 대한 Gaussian PDF의 식을 다음과 같이 정리하고,

또한 측정에 대한 Gaussian PDF의 식을 다음과 같이 정리할 수 있습니다.

위 두 개의 Gaussian PDF의 곱을 통해 PDF가 합성됨으로 인해 새로운 Gaussian PDF를 다음과 같이 얻을 수 있습니다.

  

                       

위 함수를 다시 정리하면,

여기서 , 는 다음과 같습니다. 

위의 두 식은 시스템 잡음 및 측정 잡음이 같은 도메인 혹은 단위에 있다고 가정하였지만 실제로는 다르기 때문에 한쪽의 Gaussian PDF를 다른 한 쪽의 Gaussian PDF에 곱하기 위해서는 도메인을 맞추어 주어야 합니다. 이를 위해서 시스템 잡음 쪽을 c로 나누어 스케일링을 통해 측정 잡음 도메인으로 맞추었습니다.

 

위 식을 이용하여 합성 평균과 분산을 다시 구하면 다음과 같습니다.

위의 식에서  그리고 으로 치환하면 아래와 같은 식으로 다시 쓸 수 있습니다.

한편 분산은 다음과 같습니다.

위 식을 칼만 필터 알고리즘에 적용을 위해 정리하면 다음과 같습니다.

이와 같이 칼만 필터 알고리즘의 모든 식을 유도할 수 있습니다. 한편 칼만 이득은 오차 공분산의 기대값 을 최소화하는 값을 찾는 것으로도 유도할 수 있는데, 이는 최소 자승법(Least mean square; LMS)으로 오차 공분산의 도함수가 '0'일 때가 최소이며 이 경우에 칼만 이득이 결정되게 됩니다.

위 칼만이득의 식에서 측정잡음(Measurement noise)의 공분산 R이 매우 크면 칼만이득이 작아지고 다음 단계의 추정치를 계산할 때 현재의 측정값이 무시되는데, 이는 측정잡음의 공분산이 크다는 것은 신뢰성이 떨어짐을 의미하므로 현재의 측정치를 무시하게 된다는 것입니다. 또한 예측오차 공분산(Process noise) P가 매우 크면 칼만이득은 '1'에 근접하고 측정치를 그대로 사용하게 되는데 이는 오차가 커져 오직 측정치에 의존함을 보입니다.

(2) 보정된 상태변수를 갱신합니다.

만일 H=I(Identity matrix), 위 식은 다음 식과 같이 변형이 가능합니다.

칼만 이득인 K가 '1'에 가깝다는 것은 센서로부터 측정된 값을 전적으로 신뢰할 수 있슴을 의미합니다. 그러므로 상태변수로부터 예측된 를 무시하고 100% 전적으로 신뢰할 수 있는 측정된 값 를 상태변수로 간주합니다. 하지만 K가 '0'에 가깝다는 말은 센서로부터 측정된 상태변수 값을 전혀 믿지 못하고 이전 k-1 스텝의 상태변수 값을 사용하여 예측된 상태변수를 사용하겠다는 것입니다.

(3) 보정된 오차 공분산을 갱신합니다.

평균(mean, average), 표준편차(standard deviation)와 분산(variance)은 각 데이터가 평균을 중심으로 얼마나 퍼져있는가를 나타내는 척도입니다. 평균은 , 분산과 표준편차 각각 , 로 나타내면 다음과 같습니다. 또한 공분산(covariance)은 2개의 변수간에 상관관계를 나타내는 것으로, Cov로 표시합니다. 단, 는 데이터의 갯수입니다.

공분산이 '0'보다 크면 x가 증가할때 y도 증가하고 '0'보다 작으면 x가 증가할때 y는 감소하는 경향을 가집니다. 만일 두변수가 아무 상관없이 흩어져 있으면 즉, 독립적이라면 '0'에 근접하게 된다는 것입니다. 열벡터를 갖는 공분산 행렬로 나타내면 다음과 같습니다.

공분산의 a,b가 상수라면 다음의 법칙이 성립합니다.

전술한 바와 같이 보정된 오차 공분산은 다음과 같이 구할 수 있습니다.

그리고 'Time Update'의 (1)단계로 되돌아 갑니다.

결국 칼만 필터는 센서를 동반한 선형시스템에서 비행역학(Flight Dynamics)을 이용해 예측오차(P)와 측정잡음(R)의 신뢰도를 공분산을 이용하여 다음 상태의 최적의 통계적 값을 결정하는 알고리즘으로 반복할수록 오차는 점점 수렴하게 됩니다. 게다가 이전의 측정값 등의 데이터를 저장하는 것이 아닌 현 상태의 평균과 같은 통계량만을 저장함으로서 계산량과 저장 장소를 최소화 한 방법이라 하겠습니다.

참고로 잘 정리된 칼만필터에 대한 개념입니다.

Understanding of the Kalman Filter.pdf

각종 센서들이 측정한 상태측정치들은 각 센서들 고유의 오차 및 잡음이 포함되기 때문에 비행제어기에서 바로 사용할 수 없습니다. 센서융합기는 자이로 센서, 가속도 센서 및 지자기 센서가 측정한 드론의 회전운동 상태측정치와 GPS 수신기 및 기압 센서가 측정한 드론의 병진운동 상태측정치들을 적절히 융합하여 각종 오차 및 잡음이 최소화 된 상태추정치를 계산합니다.

자이로 센서(Gyroscope)

관성측정장치(IMU) 내부에 있는 3축 자이로 센서를 이용해 드론 기체좌표 x, y, z 세 축이 지구관성좌표에 대하여 회전하는 각속도를 측정한 후 고정좌표로 변환된 값$\left(W_{{\mathrm{x.gyro,}}^{}}{W}_{{\mathrm{x.gyro,}}^{}}{W}_{{\mathrm{x.gyro}}^{}}\right)$을 계산합니다. 자이로 센서 측정치는 선형 미분방정식을 이용해 오일러 각도 $\left({\Phi }_{{\mathrm{gyro,}}^{}}{ϴ}_{{\mathrm{gyro,}}^{}}{\psi }_{{\mathrm{gyro}}^{}}\right)$로 변환될 수 있다. 자이로 센서 측정치는 저주파수 대역에서 바이어스(bias) 오차를 포함하기 때문에(즉, 드론이 정지해 있을 때에도 자이로 센서의 측정치가 '0'이 되지 않음) x, y, z 세 축에 대한 자이로 센서의 바이어스 오차가 제거되어야 합니다.

가속도 센서(Accelerometer)

자이로 센서의 스코프의 오차를 제거하기 위해 가속도 센서가 이용됩니다. 관성측정장치(IMU) 내부에 있는 가속도 센서를 이용해 드론 기체좌표 x, y, z 세 축의 지구관성좌표에 대한 가속도를 측정한 후 고정좌표로 변환된 값 (fx,acc, fy,acc, fz,acc)을 계산합니다. 가속도센서 측정치 역시 오일러 각도의 '롤(Φacc)'과 '피치(ϴacc)'로 변환될 수 있으며, 이 값들은 자이로 센서의 측정치를 이용해 계산한 '롤(Φgyro)'과 '피치(ϴgyro)'에 포함된 바이어스 오차를 제거하는데 이용됩니다. 하지만, 가속도 센서는 '요(yaw)'를 측정할 수 없기 때문에 자이로 센서를 이용해 측정한 '요(ψgyro)'에 포함된 바이어스 오차를 제거할 수 없습니다.

지자기 센서(Magetometer or Compass)

지자기 센서를 이용해 드론 기체좌표 x, y, z 세 축의 자북점에 대한 방향을 측정합니다. 이 값을 이용해 기체좌표의 NED 좌표에 대한 '요' 값을 계산할 수 있으며, 센서융합기는 지자기 센서로 측정한 '요(ψmag)'를 이용해 자이로 센서 측정치 '요(ψgyro)'에 포함된 바이어스 오차를 제거합니다. 고정날개 드론의 경우 몸체 전면에 피토관(Pitot Tube)을 부착해 좀 더 정확한 '요' 값을 측정할 수 있으나, 드론의 경우 몸체가 회전하면서 날아가기 때문에 피토관을 이용하기가 어렵다는 것입니다.

GPS 수신기 

GPS 수신기는 GPS 위성들로부터 수신한 신호를 이용해 NED 좌표 상에서 드론의 병진운동상태, 즉 위도(Pn.GPS), 경도(Pe.GPS), 고도(hMSL.GPS), 위도상의 속도(Vn.GPS), 경도상의 속도(Ve.GPS) 및 고도상의 속도(Vd.GPS)를 계산합니다. 여기서 첨자 MSL은 해수면(MSL: Mean Sea Level)을 의미합니다.

기압 센서(Barometer)

GPS 수신기를 통해 수신한 위치 좌표에는 항상 5~10m의 오차가 존재합니다. 민수용 GPS 수신기는 L1 주파수밴드(1.5GHz)의 C/A(Coarse-Acquisition) 코드 혹은 L2 주파수밴드(1.2GHz)의 C/A 코드 둘 중의 하나만을 수신할 수 있습니다. 하지만 군사용 GPS 수신기는 L1 C/A와 L2 C/A를 동시에 수신할 수 있어 Diversity로 인한 이득을 얻을 수 있으며, 추가로 암호화 신호(Encrypted Signal) P(Y)를 수신할 수 있어 GPS 신호가 지구의 이온층을 통과할 때 교란되는 것을 보정할 수 있습니다. 이를 이온층 보정(Ionospheric Correction라 일컫습니다. 5~10m의 GPS 고도 오차는 주로 저공비행을 하는 드론의 지상시설물들과의 충돌 위험을 야기시킵니다. 따라서 별도의 기압 센서를 이용하여 고도(hALP.baro)를 측정하기도 한다는 것입니다. 여기서, 첨자 ALP는 기압(Air-Level Pressor)를 의미하며 드론의 이륙시 기압과 현재 비행고도에서의 기압을 비교해 이륙 지점으로부터의 현재 고도를 계산합니다.

센서융합기는 회전운동상태(ΦE, ϴE, ψE)만을 추정하거나, 회전운동 상태와 병진운동상태(Plon.E, Plat.E, hE)를 동시에 추정할 수 있습니다. 회전운동상태 만을 추정하는 센서융합기를 AHRS(Attitude & Heading Reference System)라고 부르고, 회전운동상태와 병진운동상태를 동시에 추정하는 센서융합기를 관성항법기(INS; Inertial Navigation System)라고 부릅니다.

AHRS를 이용한 센서융합기

AHRS 센서융합기는 보상필터(Complimentary Filter; 상보필터)를 이용하거나 확장 칼만 필터(EKF: Extended Kalman Filter)를 이용합니다. AHRS 보상필터는 고주파 대역 특성이 좋은 자이로 센서의 상태측정치(Φgyro, ϴgyro)를 고주파 대역 필터로 추출하고, 저주파 대역 특성이 좋은 가속도 센서의 상태측정치(Φacc, ϴacc)를 저주파 대역 필터로 추출한 후 합쳐서 자이로 센서의 바이어스 오차가 최소화된 상태추정치(ΦE, ϴE)를 비행제어기(FC)로 전달해 줍니다.

AHRS 확장칼만필더(AHRS-EKF)는 드론의 비행역학(Flight Dynamics)을 이용해 각종 오차를 제거하는 방법입니다. 자이로 센서를 이용한 측정치(Φgyro, ϴgyro, ψgyro), 가속도 센서를 이용한 측정치(Φacc, ϴacc), 지자기 센서를 이용한 측정치(ψmag) 및 GPS 수신기를 이용한 측정치(Vn.GPS, Ve.GPS, Vd.GPS)를 이용해 실시간으로 드론의 회전운동역학(Rotational Dynamics)을 확장 칼만 필터를 이용해 모델링하면서 자이로 센서 및 가속도 센서의 바이어스 오차가 최소화된 상태추정치(ΦE, ϴE, ψE)를 계산해 비행제어기로 전달해 줍니다. GPS 수신기로 측정한 병진운동상태 측정치(Pn.GPS, Pe.GPS) 및 기압 센서로 측정한 고도측정치(hALP.baro)는 융합과정을 거치지 않고 그대로 비행제어기로 전달된다.

쿼드콥터(Quadcopter)는 다음 그림에서와 같이 모터 4개의 상대적인 회전속도에 의해 비행이 제어됩니다. 시계방향(CW)으로 회전하는 모터들에 장착되는 프로펠러를 '푸셔(Pusher) 프로펠러'라 부르고 반시계방향(CCW)으로 회전하는 모터들에 장착되는 프로펠러를 '트랙터(Tractor) 프로펠러'라 부릅니다.

• Yaw Left - 시계방향으로 회전하는 모터 ①, ③의 회전속도의 합 > 반시계방향으로 회전하는 모터 ②, ④의 회전속도의 합

• Yaw Right - 시계방향으로 회전하는 모터 ①, ③의 회전속도의 합 < 반시계방향으로 회전하는 모터 ②, ④의 회전속도의 합

• Hovering - 시계방향으로 회전하는 모터 ①, ③의 회전속도의 합 = 반시계방향으로 회전하는 모터 ②, ④의 회전속도의 합

소위 Hovering(정지 비행)은 전체 토크(Torque)가 상쇄되어 드론이 공중에서 정지하는 것이며, 이러한 상황에서 모든 프로펠러들이 발생시키는 추력의 합이 드론의 무게보다 크거나 작을 경우, 드론은 수직으로 상승(Throttle Up) 혹은 하강(Throttle Down)을 합니다.

• Pitch Up(후진) - 전면에 위치한 모터 ①, ②의 회전속도의 합 > 후면에 위치한 모터 ③, ④의 회전속도의 합

• Pitch Down(전진) - 전면에 위치한 모터 ①, ②의 회전속도의 합 < 후면에 위치한 모터 ③, ④의 회전속도의 합

• Roll Left - 우측에 위치한 모터 ①, ④의 회전속도의 합 > 좌측에 위치한 모터 ②, ③의 회전속도의 합

• Roll Right - 우측에 위치한 모터 ①, ④의 회전속도의 합 < 좌측에 위치한 모터 ②, ③의 회전속도의 합

전체 프로펠러들의 중력방향 추력의 합이 드론의 무게와 동일 할 경우, 드론은 좌측 혹은 우측으로 수평비행을 하게 됩니다.

모터 ①, ②, ③, ④의 회전속도를 각각 , , , 라 하고 모터들에 장착된 프로펠러들이 발생시키는 전체 추력을 라 할 때, 각 모터들의 회전속도와 오일러 각도의 변화량  및 추력의 변화량 과의 관계는 다음 수식으로 표현할 수 있습니다.

위 식을 행렬식으로 나타내면 다음과 같습니다.

그러므로 각 모터의 회전속도 관점에서 다음과 같이 나타낼 수 있다.

위 식을 시간 에서 오일러 각도 및 추력의 변화량을 시간 증분을 이용해 표시하면 다음과 같습니다.

위 식에서 는 시간 에서 Tx(송신기)로부터 수신한 비행명령어이고, 는 시간 에서 각종 센서들을 이용하여 추정한 드론의 상태추정치이며, 는 드론의 비행제어기가 수행하는 함수로 볼 수 있습니다. 이 경우, 첨자 ''는 desired(Tx가 원하는)의 ''로 대체할 수 있고, 첨자 ''은 Estimated(센서융합기가 추정한)의 ''로 대체할 수 있습니다.

Tx에 조정키들을 움직여 오일러 각도 및 추력으로 구성된 비행명령어 를 드론에 송신하고, 드론의 Rx(수신기)는 이 비행명령어를 받아서 비행제어기(FC)에 전달합니다. 센서융합기는 자이로 센서, 가속도 센서 및 지자기 센서를 이용해 측정한 회전운동 상태측정치 와 기압 센서를 이용해 측정한 고도측정치 을 적절히 융합해 각 센서들의 오차가 최대한 제거된 상태추정치 를 계산해 비행제어기에 전달합니다. 비행제어기는 Rx로부터 받은 비행명령어를 센서융합기가 보내온 상태추정치와 비교해 그 차이 값을 이용해 각 모터들의 회전속도를 계산합니다. 여기서 첨자 'M'은 Measured(각종 센서들이 측정한)을 의미합니다.

드론은 Tx를 이용하지 않고 GPS 경로비행을 할 수도 있습니다. 한편으로, 센서융합기는 드론의 회전운동 상태측정치 와 GPS 수신기와 기압 센서를 이용해 측정한 병진운동 상태측정치 를 함께 융합하여 센서 오차들을 좀 더 줄일 수 있습니다. 여기서 는 위치벡터, 는 속도 벡터, 는 고도를 의미한다. 아래 그림에서 센서융합기는 드론의 회전운동 및 병진운동 상태측정치를 융합하여 자이로 센서와 가속도 센서의 오차가 최대한 제거된 상태추정치 를 비행제어기에 전달하고, 비행제어기는 Rx로부터 전달 받은 비행명령어 혹은 GPS 비행경로 좌표와 상태추정치를 비교해 그 차이 값을 이용하여 각 모터들의 회전속도를 계산합니다. 여기서 'lon'은 longitude(경도), 'lat'는 latitude(위도)의 약자입니다.