칼만필터와 드론의 자세제어

칼만필터는 드론시스템에서 여러 센서들의 장점만을 융합해서 측정값의 신뢰도를 높이기 위해 사용된다. 

그리고 이러한 센서 측정값은 드론의 현재 상태를 파악하는데 사용되고

파악된 현재상태와 드론에 내린 우리가 원하는 드론의 상태와의 차이

원하는 (드론의 상태 - 현재 상태) 의 크기를 줄이는 제어 알고리즘으로 PID를 사용한다.  

드론의 자세제어는 자세측정장치를 이용한다. 

1.자세측정장치(ARS)

관성센서(자이로 + 가속도센서) 및 지자기 센서로 이루어진 자세측정장치는 여러 가지 오차를 가지고 있다. 

각 센서는 측정값에 대해서 특성을 가지고 있다. 예를 들어서 가속도 센서는 특정 방향으로 편향 되지 않으나 측정 값에 대해서 오차가 존재하고, 자이로의 경우는 값을 잘 측정하나 발산될 수 있다는 한계를 가진다. 이러한 오차를 해소하여 값을 측정하기 위해 서로의 오차를 보정할 수 있도록 여러 개의 센서로 융합한 자세측정장치를 사용한다. 센서들의 오차를 보정하면서 융합하기 위한 방식으로 우리는 칼만필터를 사용한다.

칼만필터는 오차의 공분산을 줄이는것을 필터의 목적으로 한다. 

 2. 칼만필터

-칼만필터의 개념

칼만 필터는 대상 시스템의 확률적인 모델과 측정값을 이용하여 시스템의 상태변수를 찾아내는 최적 추정기법이다. 

즉, 상태식에 의한 시간전파와 측정식에 의한 개선을 통하여 상태값을 추정하게 되며, 

실제 상태값과 추정된 상태값의 오차 공분산을 최소화하는 알고리즘 이다. 

시스템이 선형이고 정규분포를 갖는 백색잡음으로 에 의해 구동되는 칼만 필터는 

 (input noise(driving noise)와 센서 noise (observation noise) 는 white noise이다. )

최소 공분산을 갖는 편향되지 않는 최적의 추정기로 알려져 있다.

칼만필터는 추정값에 대한 확률분포를 따져서 가장 확률이 높은 값을 추정값으로 선택하는 방법으로 

측정값의 예측 오차로 예측값을 적절히 보정해서 최종 추정값을 계산한다. 

칼만이득을 통한 각각의 결과로 나타나는 상태를 위한 가장 최적의 평균 인자를 찾는 것이 칼만필터의 목표이다.

< 분석된 칼만필터의 프로세스>    

 -칼만필터의 알고리즘 프로세스

 

<칼만필터 알고리즘 프로세스>

 

칼만필터의 흐름도

 a. 초기값 선정 - Initial estimate 

: 처음에 들어올 상태변수의 값

과 이 상태변수

가 가질 오차공분산을 예측하여 입력해준다.

  이 과정은 처음 한번만 해준다. (초기값 선정후엔 b를 거치치 않고 곧장 c로 간다.)

 

b. 추정값과 오차 공분산 예측 => time update

: 추정값은 전체적인 시스템 모델과 밀접하게 관련되어 있으며, 

  (상태변수는 system model과 input noise로 이루어져 있으므로) => time update (1)계산

 변수 P, 예측된 오차공분산값

은 이전에 계산되어진 값

과, 시스템모델A 그리고 시스템오차Q를 이용하여 예측하며, 이렇게 얻어진 오차공분산

값을 사용하여 칼만이득(K)값을 계산한다.

  => time update (2), measurement update(1) 계산.

 

c. 추정값 계산

: 현재 측정한 센서의 값

과 time update(1)에서 구한 예측값 

의 차이를 (오차) Kalman gain으로 곱해주고 

값을 더해주어 최종 추정값 

을 결정해 줍니다.  

=> measurement update(2)

 

 

d. 오차공분산 계산

: 오차 공분산은 추정값

이 얼마나 정확한지를 알려주는 척도이며, 

   이 오차 공분산을 검토하여 앞서 계산한 추정값을 사용할 것인지 버릴 것인지 판단할 수 있다.

   => 오차공분산이 크면 이 값의 신뢰도는 그만큼 떨어지는 것이기 때문에 사용할 가치가 없다고 판단한다.

  => measurement update(3)

  

 

전체적인 알고리즘은 time update(1) 에서 measurement update(3)으로 진행되며, measurement update(2)에서 구한 

값은  다음 time update(1)이 진행될 때, 

값으로 사용된다.

결론적으로 칼만필터는, 시스템모델을 기초로 하여 

다음 시점의 상태와 오차 공분산이 어떤 값이 될 것인지를 예측한 후 

측정값과 예측값의 차이를 보상하여 새로운 추정값을 계산한다. 

그러면 이 추정값이 칼만 필터의 최종 결과물이 된다. 

그리고 이 최종 결과물은 다음 첫과정 (time update(1))진행시 이전 추정값으로 사용된다.

 

또한, 칼만필터의 알고리즘은 크게 예측과정과 추정과정으로 나눌 수 있는데,

 

예측과정은 시스템 모델변수 A와 Q를 사용하여 직전 추정값 x와 오차 공분산P를 입력으로 받아서 최종결과로 예측값 x와 P를 출력한다. 이 값들을 추정과정에 사용된다. 또한, 추정과정은 예측과정의 예측값x와 P, 

그리고 측정값인 z를 전달받아 필터모델인 H와 센서오차인 R을 이용하여 추정값 x와 오차공분산 P를 출력한다. 시스템 모델의 A, H, Q, R 네개의 변수가 칼만 필터의 설계 인자가 되며, 이 값을 어떻게 선정하느냐에 따라 칼만 필터의 성능이 달라진다. 칼만필터의 성능은 시스템 모델이 실제 시스템과 가까울수록 좋아진다.

 

 

 

 

[변수의미]

 

시간이 지날 수록 위와같이 변수가 변합니다.

 

 

이것 절대 잊지마세요.

 이 값을 이용해서  

값을 만들구요.

이 값을 이용해서 

값을 만듭니다.

그리고 

이 값을 이용해서 

 값을 만듭니다.

 

 

'-' 붙은 부분과 k를 잘봐주세요. k는 시간을 의미합니다. 

k의 다음시간은 k+1, 

k의 이전시간은 k-1 입니다.

k가 2초면, k-1은 1초, k+1은 3초입니다.

 

 

그리고 x위에 모두 모자를 썼기 때문에 x_hat이라고 부릅니다.

x_hat의 의미는 예측값 (estimate) 입니다.

 

 

 

 

<전체적인 칼만필터의 흐름도>

 

 

칼만필터의 흐름도에서 좌측은 시스템 방정식이고 우측은 관측방정식이다. 

시스템 방정식을 보면 x는 칼만필터를 통해 최적화 하고자 하는 상태변수를 의미하고, 

A는 이전단계에서 다음단계로 연결하는 변환계수를 의미한다. 

B와 u는 하나로 볼 수 있으며 이들은 시스템에 무관한 추가 입력 값이다. 

P는 오차 공분산 값으로 시스템 오차인 Q값과 이전 상태의 공분산 값으로 구할 수 있다. 

여기서 Q는 시스템 오차로써 구하는 방법이 있겠지만 .. 

시스템 오차를 직접 계산한다는 것은 그리 쉽지 않은 일이므로..

사용자가 임의적으로 지정하여 넣어주도록 하며,

R값은 센서자체의 오차로 사용하는 센서의 specification을 살펴보면 확인할 수 있다.

 

칼만게인값은 시스템 방정식에서 구한 P(공분산)값과 R(센서오차)값을 통해서 구할 수 있다.

이렇게 구한 칼만게인을 통해 예측된 상태변수 값으로 올바른 상태변수 값을 구할 수 있게 된다. 

또한 한번의 계산으로 끝나는 것이 아니라

이 칼만게인으로 현재 상태의 P값을 재조정 함으로써 다음상태에 영향을 미치게 된다.

 

 

 

 

-자세측정장치(ARS)에 칼만필터를 적용

 

 

 

<자이로센서와 가속도센서의 융합>

 

 

 

가속도계로 구한 자세는 시간이 지나도 오차가 커지지 않고 일정 범위로 제한되는 장점을 갖고 있고, 

자이로는 자세 변화는 잘 감지하지만 시간이 지나면 오차가 누적되어 발산하는 문제가 있기 때문에 

자이로 센서와 가속도 센서를 융합하여 사용하도록 한다. 

즉, 자이로와 가속도계의 상호 보완적인 특성을 이용하여 가속도계로 자이로를 보상하는 센서 융합을 한다.

자이로센서에서 측정값을 오일러각의 변화율로 바꿔서 적분해서 오일러각을 구하게 되는데 

오일러각을 칼만필터에 적용시키면 

특정상황에서 두개의 축이 하나의 축으로 겹쳐져 두개의 축이 같은 축으로 회전하는 짐벌락이 발생하기 때문에 

시스템 모델이 칼만 필터가 적용되지 않는 형태가 된다. 

그러므로 상태변수를 오일러각에서 쿼터니언으로 바꾸고, 측정값도 오일러각에서 쿼터니언으로 바꿈으로써 

시스템 모델을 칼만 필터가 적용될 수 있도록 변경하여 준다.

 

 

 

각속도를 쿼터니안으로 바꾼 후,

 

 

 

<각속도를 쿼터니안으로 변경>

 

 

 

이산 시스템으로 변경하면,

 

 

 

<이산 시스템이 적용된 쿼터니안>

 

 

 

시스템 모델의 행렬 A를 얻을 수 있다.

 

 

 

<행렬 A>

 

 

 

가속도로 계산한 오일러각을 칼만필터의 측정값으로 사용하기 위해 가속도로 계산한 측정값을 

시스템 모델에 맞게 쿼터니안으로 변경한다.

 

 

 

 

 

 

잡음의 공분산 행렬 Q와 R은 시스템의 신호 특성과 관련 있는 값이라, 

이론적으로 구하기는 어렵고 실제 데이터를 분석하여 얻도록 한다. 

보통은 이 행렬을 설계인자로 보고 여러 값들을 넣어 성능 변화의 추이를 보면서 결정하기도 한다.

-출처: MATLAB활용 칼만필터의 이해. 김성필저 (아진출판사)

 

 

-칼만필터와 보상필터와의 차이.

 

칼만필터와 보상필터의 기본적인 개념은 1차 저주파 통과 필터라는 점에서 비슷하다. 

하지만 보상필터는 LPF와 HPF의 가중치가 고정되어 있는 반면에 칼만필터는 예측값과 측정값에 적절한 가중치를 곱하여 

두 값을 더해 최종 추정값을 계산하게 되는데, 가중치 값은 칼만이득(K)으로 칼만필터의 알고리즘을 반복하면서 

칼만 이득값은 새로 계산된다.

즉, 추정값을 계산하는 이득값이 매번 다시 조정되는 것이 보상필터와 칼만필터의 차이로써,

칼만필터는 변화되는 이득값으로 인해 보상필터보다 응답성이 빠르다.

 

 

 

-칼만필터적용 테스트

 

엔코더 값을 이용하여 필터가 적용되지 않았을 경우의 ARS센서의 데이터를 확보하고, 

그리고 나서 보상필터가 적용되었을 때의 데이터 값을 기존의 엔코더를 이용하여 출력한 데이터와 비교한다. 

그리고 마지막으로 칼만필터를 적용하여 데이터화시켜 이전의 데이터들과 비교하도록 한다.