이번 파트에선 시스템 모델이 비선형인 경우에 적용 가능한 비선형 칼만 필터에 대해 알아보자!
대부분의 실제 시스템은 비선형 모델로 표현되는데, 칼만 필터는 선형 모델 대상!
➡ 비선형 모델을 선형으로 근사해서 선형 칼만 필터 알고리즘을 적용하는 확장 칼만 필터(Ch12)
, 비선형 시스템 모델을 나타내는 입자들(points, particles)을 활용하는 무향 칼만 필터(Ch13)
와 파티클 필터(Ch14)
에 대해 알아보자!
우리 주변에 존재하는 시스템 대부분이 비선형이기 때문에, 칼만 필터를 비선형 문제에 적용하려는 연구가 아직까지 활발!
초기 연구자들이 칼만 필터를 비선형 시스템까지 확장한 알고리즘 ➡ 확장 칼만 필터(EKF; Extended Kalman Filter)
비선형 시스템에 칼만 필터를 적용해야 한다면, 오랜 세월 검증되고 자료가 많은 EKF 고려해보기.
❗ 하지만 알고리즘이 발산할 위험이 있다는 큰 단점이 있다는 것도 고려하기. ⬅ EKF 이후에 개발된 알고리즘이 극복하고자 하는 지점
: 비선형 모델을 선형화한 다음, 이 선형 모델에 대해 설계한 칼만 필터
선형 칼만 필터 알고리즘과 완전히 동일하지만, 시스템 모델에서 차이가 존재
두 칼만 필터의 설계 과정은 똑같다.
📍 LKF를 실제 시스템에 적용할 때는 시스템의 운용 범위에 유의하기!!
선형화 모델은 기준점 근처에서만 실제 시스템과 비슷한 특성을 보이기 때문
➡ 실제 시스템에 적용할 땐 선형화 모델이 유효한 범위 내에서만 사용되어야 한다.
📌 선형화 칼만 필터가 성공하려면 비선형 시스템의 선형화 모델이 좋아야 한다.
위의 알고리즘을 보면 시스템 행렬 A와 H
가 미리 결정되어 있어야 하지만 아직 모르는 변수이다.
➡ 시스템 모델에서 받아와야 한다. 하지만 위에서 본 비선형 시스템 모델 식에선 A와 H가 보이지 않는다.
➡ 비선형 모델의 선형화
비선형 모델을 편미분해서 선형 행렬을 구하는 방법을 써보자! (선형화의 대표적인 기법)
EKF-LKF
직전 추정값(^x_k)
을 선형화 기준으로 삼음 EKF
는 미리 결정된 선형화 기준을 사용하지 않고, 직전 추정값을 기준으로 삼아 매번 새로운 선형화 모델을 구한다.LKF
사용!(선형화의 기준을 미리 알 수 있는 경우는 일정 궤도를 도는 위성, 궤적이 미리 정해져 있는 위성 발사체 등)
EKF 알고리즘의 전체 과정은 선형 칼만 필터 알고리즘과 같다.
각 단계별 계산식에서 차이점 2가지!
Ax_k, Hx_k
대신 비선형 시스템 모델식 f(x_k), h(x_k)
야코비안
으로 행렬 A, H 구함두 예제를 살펴보자!
물체까지의 직선 거리를 측정해서 위치와 고도를 추정해보자!
📌 목표: 측정 모델이 비선형인 시스템에 대해 EKF 설계하는 것
문제를 단순하게 하기 위해 물체는 2차원 평면 상에서 일정한 고도와 속도를 유지하면서 움직인다고 가정하자. (고도와 물체의 이동 속도는 모르는 값)
식 (12.7)과 (12.8)의 시스템 모델을 모두 구했다.
(12.7)은 선형인데, (12.8)의 측정 모델은 비선형!
➡ 선형 칼만 필터를 바로 적용하진 ❌
식 (12.7)의 시스템 모델은 선형
➡ EKF 알고리즘에서 상태 변수를 예측할 때 선형 칼만 필터 수식 사용
EKF 알고리즘은 아래 식과 같은 이산 시스템에 관한 알고리즘. 하지만 식 (12.7)의 시스템 모델은 이산 시스템이 아니다.
➡ 시스템 모델을 이산 시스템으로 변경해야 함
식 (12.7)을 이산화하면 시스템 행렬 A는 아래와 같다.
아래의 이산 시스템 모델은 식 (12.7)을 오일러 적분법으로 dt시간 동안 적분한 결과
측정 모델은 비선형 모델
➡ 행렬 H는 아래와 같이 식 (12.8)의 야코비안을 계산해야 한다.
식 (12.10)
RadarEKF(z,dt)
- 매개변수: z(측정 거리), dt(샘플링 간격)
- 반환값: 수평 위치, 이동 속도, 고도
: EKF 알고리즘 구현. EKF 알고리즘 순서에 따라 추정값 계산.
EKF 알고리즘을 시작하기 전에 H 먼저 계산
EKF는 선형 모델을 구할 때 현재의 추정값을 기준으로 선형 모델을 구하므로, 이 함수도 현재의 추정값을 인자로 받아야 한다.
hx 함수는 식 (12.8)의 측정 모델식을 구현한 함수
<function [pos, vel, alt] = RadarEKF(z, dt)
%
%
persistent A Q R
persistent x P
persistent firstRun
if isempty(firstRun)
A = eye(3) + dt*[ 0 1 0;
0 0 0;
0 0 0 ];
Q = [ 0 0 0;
0 0.001 0;
0 0 0.001 ];
R = 10;
x = [0 90 1100]'; % 임의로 예측한 초기 추정값
P = 10*eye(3);
firstRun = 1;
end
H = Hjacob(x); % 시스템 행렬의 야코비안 H 계산
xp = A*x;
Pp = A*P*A' + Q;
K = Pp*H'*inv(H*Pp*H' + R);
x = xp + K*(z - hx(xp));
P = Pp - K*H*Pp;
pos = x(1);
vel = x(2);
alt = x(3);
%---------------------------------
function zp = hx(xhat) % 직선 거리 반환
%
%
x1 = xhat(1);
x3 = xhat(3);
zp = sqrt(x1^2 + x3^2);
%---------------------------------
function H = Hjacob(xp)
%
%
H = zeros(1, 3);
x1 = xp(1);
x3 = xp(3);
H(1) = x1 / sqrt(x1^2 + x3^2);
H(2) = 0;
H(3) = x3 / sqrt(x1^2 + x3^2);
GetRadar.m
: 레이다의 직선 거리를 측정하는 함수
이렇게 가상으로 측정 잡을 만드는 코드는 실제 시스템에선 필요❌
일정한 고도에서 정속으로 이동하는 물체의 동역학 방정식이 구현되어 있다.
function r = GetRadar(dt)
%
%
persistent posp
if isempty(posp)
posp = 0;
end
vel = 100 + 5*randn;
alt = 1000 + 10*randn;
pos = posp + vel*dt;
v = 0 + pos*0.05*randn;
r = sqrt(pos^2 + alt^2) + v;
posp = pos;
TestRadarEKF.m
clear all
dt = 0.05;
t = 0:dt:20;
Nsamples = length(t);
Xsaved = zeros(Nsamples, 3);
Zsaved = zeros(Nsamples, 1);
for k=1:Nsamples
r = GetRadar(dt);
[pos, vel, alt] = RadarEKF(r, dt);
Xsaved(k, :) = [pos vel alt];
Zsaved(k) = r;
end
PosSaved = Xsaved(:, 1);
VelSaved = Xsaved(:, 2);
AltSaved = Xsaved(:, 3);
t = 0:dt:Nsamples*dt-dt;
figure
plot(t, PosSaved)
figure
plot(t, VelSaved)
figure
plot(t, AltSaved)
Figure 2
: 속도 추정값
Figure 1
: 고도 추정값
Figure 3
: 수평 거리 추정값
📌 레이다가 측정한 직선 거리 & EKF로 추정한 고도와 수평 거리로 계산한 추정 거리를 비교해보면 측정값의 잡음이 거의 제거되었고, 물체의 이동에 따른 측정 거리 변화도 잘 반영되었음을 확인 가능
시스템 모델이 비선형인 문제를 보자!
상태 변수를 오일러 각도에서 쿼터니언으로 변환해야만 했던 11장의 자이로와 가속도계를 이용한 기울기 자세 측정 예제를 다시 다뤄보자!
EKF를 구현하기 위해선 식 (12.11)의 야코비안을 알아야 한다.
위의 식은 식 (12.11)의 야코비안 정의이다.
이 정의에 식 (12.11)을 대입해서 미분하면 행렬 A의 수식이 나온다.
하지만 식 (12.13)은 이산 시스템의 야코비안이 아니다.
➡ 구현할 땐 이 행렬을 이산 시스템으로 변환해야 한다!
EulerEKF(z, rates, dt)
Ajacob 함수
function [phi, theta, psi] = EulerEKF(z, rates, dt)
%
%
persistent H Q R
persistent x P
persistent firstRun
if isempty(firstRun)
H = [ 1 0 0;
0 1 0 ];
Q = [ 0.0001 0 0;
0 0.0001 0;
0 0 0.1 ];
R = [ 10 0;
0 10 ];
x = [0 0 0]';
P = 10*eye(3);
firstRun = 1;
end
A = Ajacob(x, rates, dt); % 야코비안 계산
xp = fx(x, rates, dt); % 상태변수 예측값 계산 함수 호출
Pp = A*P*A' + Q;
K = Pp*H'*inv(H*Pp*H' + R);
x = xp + K*(z - H*xp);
P = Pp - K*H*Pp;
phi = x(1);
theta = x(2);
psi = x(3);
%------------------------------
function xp = fx(xhat, rates, dt)
%
%
phi = xhat(1);
theta = xhat(2);
p = rates(1);
q = rates(2);
r = rates(3);
xdot = zeros(3, 1);
xdot(1) = p + q*sin(phi)*tan(theta) + r*cos(phi)*tan(theta);
xdot(2) = q*cos(phi) - r*sin(phi);
xdot(3) = q*sin(phi)*sec(theta) + r*cos(phi)*sec(theta);
xp = xhat + xdot*dt;
%------------------------------
function A = Ajacob(xhat, rates, dt)
%
%
A = zeros(3, 3);
phi = xhat(1);
theta = xhat(2);
p = rates(1);
q = rates(2);
r = rates(3);
A(1,1) = q*cos(phi)*tan(theta) - r*sin(phi)*tan(theta);
A(1,2) = q*sin(phi)*sec(theta)^2 + r*cos(phi)*sec(theta)^2;
A(1,3) = 0;
A(2,1) = -q*sin(phi) - r*cos(phi);
A(2,2) = 0;
A(2,3) = 0;
A(3,1) = q*cos(phi)*sec(theta) - r*sin(phi)*sec(theta);
A(3,2) = q*sin(phi)*sec(theta)*tan(theta) + r*cos(phi)*sec(theta)*tan(theta);
A(3,3) = 0;
A = eye(3) + A*dt;
TestEulerEKF.m
clear all
Nsamples = 41500;
EulerSaved = zeros(Nsamples, 3);
dt = 0.01;
for k = 1:Nsamples
[p, q, r] = GetGyro();
[ax, ay, az] = GetAccel();
[phi_a, theta_a] = EulerAccel(ax, ay, az);
[phi, theta, psi] = EulerEKF([phi_a theta_a]', [p q r], dt);
EulerSaved(k, :) = [ phi theta psi ];
end
PhiSaved = EulerSaved(:, 1) * 180/pi;
ThetaSaved = EulerSaved(:, 2) * 180/pi;
PsiSaved = EulerSaved(:, 3) * 180/pi;
t = 0:dt:Nsamples*dt-dt;
figure
plot(t, PhiSaved)
figure
plot(t, ThetaSaved)
figure
plot(t, PsiSaved)