Kalman filter for nonlinear system-Basic: Jacobian

목적

본 글에서는 이전 소개한 단진자와 다른 비선형 시스템에서 확장 칼만 필터(Extended Kalman Filter, EKF)를 적용하는 방법을 소개한다. 특히, 자코비안(Jacobian)을 활용해 상태 전이 함수와 측정 함수의 선형 근사 과정을 상세히 다루어, 레이더를 통한 비행체 위치 추정 예제를 통해 EKF의 적용 원리를 설명한다.

주의사항

이전글에서 이미 자코비안, 칼만필터, 상태공간모델에 대한 기초와 구현을 다루었으므로 가능한 수식 관련 설명들은 생략하도록 노력하였다.

비선형시스템 예시: 레이더를 통한 비행체 위치 추정

확장 칼만 필터에서 자코비안을 어떻게 활용할지 이해하려면 적절한 예제가 필요하다.

아래 그림과이 레이더로 비행체의 위치를 추정하는 적절한 예시다.

언제나 기본은 시스템 분석이 시작이다.

역학 분석

그림 속 비행체의 운동은 뉴턴의 운동 방정식으로 표현된다.

비행체가 일정한 속도로 이동하고 있다면, 비행체와 상호작용하는 모든 합력이 0으로 MẌ(t) = 0 인 뉴턴-운동방정식(=역학식)으로 표현할 수 있다.

그러나 비행체에 상호작용 합력이 0이 아니라면 그 역학식은 MẌ(t) = F(t) 으로 표현된다. 이전 포탄 운동 시스템은 역학식의 우항이 중력 상수였다.

F(t)는 비행체의 운동을 결정하는 동적 함수로, 조종자의 입력, 자동 비행 시스템, 외부 환경 요인(공기 저항, 중력, 전자기 간섭 등)에 의해 변한다. 이러한 동역학적 특성을 보다 엄밀하게 분석하여 적용하는 것이 군사 레이더 시스템이며, 특히 군용 항공기의 탐지 및 추적 알고리즘에 중요한 역할을 한다.

이 예제에서는, 비행체의 y방향에 사인 형태의 외력

가 작용한다고 가정한다.

비행체가 난기류 등 여러 영향에 의해 y방향으로 sin형태로 외력 F(t)항이 있다고 생각하자

관측과 시스템 정의

레이더는 물체와의 되돌아오는 신호를 통해 거리를 측정한다. 레이더는 구조상 관측을 거리r, 각도θ 극좌표계에 속하며, 비행체는 x, y 공간상 위치인 데카르트좌표계에 속해 아래와 같은 관계를 가진다.

여기서 레이더에서 관측한 r, θ 데이터를 비행체의 위치 x, y 관계식 아래와 같다.

이산화

역학식인 미분 방정식 선형조합이라면 연속시간-상태공간방정식으로 기술할 수 있다.

그리고 F(t)가 sin함수라면 아래와 같이 연속시간-상태공간이 된다.

비행체의 연속시간-상태공간모델

그리고 ODE 이산화는 아래와 같이 구한다.

비행체의 이산시간-상태공간모델

여기까지 2차-선형-상미분방정식 형태 역학식으로부터 이산시간 상태공간 모델을 도출하는 과정이었다.

위 모델식에 w = 2pif, f= 0.1Hz, mass =1Kg , A = 10N, dt = 0.1s이면 시간의 흐름에 따라 아래 그림과 같은 경로가 나온다.

x축 : 비행체 x위치, y축: 비행체 y위치

이론적으로는 모든 시스템의 역학 관계를 알고 있다면 정밀한 이산시간 상태공간 모델을 구축할 수 있다. 그리고 칼만적용도 어렵지 않다.

불완전한 역학식 하의 상태 추정

현실적의 한계:

그러나 위 케이스처럼 역학식 구할수 있으면 좋겠지만 실제 시스템에서는 비행체에 작용하는 외력 F(t)의 구체적인 형태를 정확히 파악하기 어렵다. 그러나 관측을 통해 비행체의 위치 함수(trajectory)는 얻을 수 있다.

전통적 칼만 필터 적용 과정:

기존 칼만 필터 적용 절차는 다음과 같은 단계로 진행되었다.

1. 역학식 분석: 뉴턴의 운동방정식을 기반으로 시스템의 역학식을 도출
2. 연속시간 상태공간 모델링: 도출된 역학식을 바탕으로 연속시간 상태공간 모델 구성
3. 이산화: 연속시간 모델을 이산 시간 모델로 변환
4. 예측: 이산시간 상태공간 모델을 이용한 상태 예측

칼만 필터 적용 문제점:

외력 F(t)의 형태를 모르는 상황, 즉 위치 함수만 알고 있는 경우에는 첫 단계부터 완전한 모델링이 어려워진다.

이는 대부분의 현실에서 관측 가능한 정보가 위치(trajectory)뿐인 경우와 일치한다.

이 경우 비행체에 어떤 외력F(t)이 주어 졌는지 모르겠지만 그 비행체의 위치함수만 알고 있고 그 형태가 사인(sine)으로 나타난것만 알고 있는 상황이다.

해결책 : 확장 칼만 필터(EKF)와 자코비안:

이러한 문제를 극복하기 위해, 자코비안을 활용한 확장 칼만 필터(EKF) 가 도입된다.

간략히 EKF는 비선형 시스템에서 상태 예측과 측정 과정을 선형 근사화하기 위해 자코비안(편미분 행렬)을 사용한다. 이를 통해, 정확한 역학 모델이 없더라도 관측 정보와 시스템 간의 관계를 바탕으로 상태 추정을 수행할 수 있다.

다음 문단부터는 비행체의 위치 함수만 알고 있는 상황에서, 외력에 대한 구체적인 정보 없이 칼만 필터를 적용할 수 있는 EKF의 원리와 구현 방법을 소개한다.

위치해 (Position Function) 구하기

먼저 그 위치해는 어떻게 찾았고, 왜 위치해만 있어도 될까?

데이터 피팅을 통한 위치 함수 추정

대상 시스템과 상호작용의 형태(form)를 알고 있다면, 시스템의 해(solution)가 미리 알려진 상태에서 관측 결과와의 차이를 분석하여 데이터 피팅(data fitting)으로 각 파라미터를 추정할 수 있다

위 그림과 같이, 시스템의 해(solution)가 알려져 있다면, 관측된 결과와의 차이를 데이터 피팅(data fitting)하여 각 파라미터의 속성을 분석할 수 있다. 이를 통해 외력(external force) 항이 시스템에 미치는 개별적인 영향을 해석하고, 최종적으로 위치 함수(trajectory)를 결정할 수 있다.

위치해와 시스템과 외력관계

여기서 비행체의 위치(Position)는 외력(External Force)에 대한 시스템의 응답(Response) 이다.즉, 관측된 위치 정보에는 시스템 자체의 특성과 외력의 영향 이 함께 포함되어 있다

이 개념을 설명하는 것이 중첩 원리(Superposition Principle)며 이는 물리학에서 선형성(Linearity) 이 성립하는 방정식에서 적용되며, 파동 방정식, 라플라스 방정식, 슈뢰딩거 방정식, 맥스웰 방정식등 여러 물리 방정식들이 이에 해당한다.

즉, 비행체의 위치 함수도 시스템 해(자유운동)와 외력 해(외부 작용)로 분리될 수 있으며, 이를 분석하면 시스템과 외력의 특성을 파악할 수 있다.

[가정]

실제 데이터 수집 및 모델 피팅 과정을 통해 위치 함수를 도출하는 과정이 매우 복잡할 수 있다. 본 글에서는 이러한 복잡한 과정을 생략하고, “위치 함수가 이미 구해졌다” 라는 가정 하에 이후 칼만 필터 적용 등 다른 논의로 넘어가겠다.

선행지식 : 미분방정식

다만, 역학식으로부터 시스템해(자유운동 해) 와 외력해(외력에 의한 해) 를 도출하는 과정을 선행되어야, 반대 위치 함수를 관측하여 그에 맞는 모델 형태(form)이 어떻게 선정되었는지를 이해할 수 있다.

이를 위해, 다시 한 번 미분방정식의 풀이를 되짚어야 한다.

위와 같이 우항이 0이 아닌 방정식을 선형 비균일(Nonhomogeneous) 미분방정식이라 정의한다.

[미분방정식] - 선형비균일미분방정식 해

비균일 미분방정식(Nonhomogeneous Differential Equation)의 해를 구하는 방법은 중첩 원리(Superposition Principle) 를 기반으로 한다.

General Solution

이는 선형 미분방정식에서 해의 선형 결합이 또 다른 해가 됨을 의미한다.

중첩 원리에 따르면, 미분방정식의 해는 두 부분으로 구성된다. 두 부분은 항등원(=) 기준 양 방정식의 해를 각 동차해(Homogeneous Solution), 부분해(Particular Solution)라 한다.

[미분방정식] - 일반해(General Solution)

예시와 연관지으면 동차해는 외력이 없는 경우 F(t)=0에 대한 해이며 시스템이 자체적으로 가지는 기본적인 운동을 나타낸다. 그리고 부분해는 특정 외력 F(t)에 대한 해로 외부 상호작용에 의해 추가적으로 발생하는 운동이다.

일반해(General Solution) 는 두 해의 합으로 표현된다.

[미분방정식]-동차해(Homogeneous Solution)

동차해는 F(t)=0로 미분방정식을 정의하여 해를 구한다.

예시와 같이 동차해는 시스템이 외부의 힘 없이 스스로 가지는 운동을 나타내며, 일반적으로 초기 조건에 따라 결정된다.

[미분방정식]- 부분해(Particular Solution)

부분해 xp(t)를 구하는 방법은 외력 F(t)의 형태(form)에 따라 달라진다.

예시와 같이 외력항이 삼각함수(sinusoidal function) 형태를 가질 경우, 그에 대한 부분해는 아래와 같이 나타난다.

삼각함수 F(t) 부분해 x(p)는 삼각함수이다.

삼각함수 형태의 외력항 F(t)에 대응하는 부분해 xp(t)는 삼각함수 형태를 가진다. 이때 상수 C와 D는 초기조건(inital condition) 또는 경계조건(boundary condition)으로 찾아야 한다.

미분방정식을 많이 풀다 보면, 부분해는 외력항과 비슷한 형태를 갖는 경향이 있다는 점을 관찰할 수 있다.

이를 속된 표현으로 비유하자면, “콩 심은 데 콩 나고, 팥 심은 데 팥 난다.”라고 할 수 있다.

- [미분방정식]: 부분해 정의할 수 없는 경우

외력항 F(t) 을 명확히 정의할 수 없는 경우, 문제 해결이 복잡해진다. 이러한 경우에는 임펄스 응답(Impulse Response) 을 이용하는 **그린 함수(Green’s Function) 방법**이 사용된다. 그린 함수는 특정 입력(외력) 함수에 대한 시스템의 응답을 구하는 강력한 도구로, 보다 복잡한 역학적 시스템의 해를 구할 때 사용된다.

비행체 위치해

위 그림으로부터 관측된 결과와 시스템 해(solution) 간의 차이를 데이터 피팅(data fitting) 과정을 통해 아래와 같은 일반해 형태와 파라미터 값을 결정할 수 있다.

비행체의 위치 일반해

모델링을 선정하는 스킬의 주요 팩터는 직관이라 공부뿐 아니라 경험과 센스가 모두 요구한다.

위치해 가정 (Assumptions)

비행체의 운동을 단순화하기 위해 다음과 같은 가정을 설정한다.

초기조건은 t = 0 일때 sin 영점에 위치하며, 추정된 파라메터가 C는 0, D는 100, 각속도 w는 0.1로 정한다. 외력 영향이 x축은 없고 y축 방향만 있다고 가정한다.

앞으로 예시로 사용될 일반해

위치해를 이용한 시뮬레이션

본 시뮬레이션는 비행체의 위치를 레이더를 통해 관측하는 상황을 재현한다. 비행체는 x축으로 등속운동을 수행하며, 난기류 등의 여러 외력 영향으로 y축 방향으로 사인(sine) 형태의 진동을 보이는 모델이다.

시뮬레이션 코드

다음 코드는 비행체의 궤적을 생성하고, 이를 레이더 관측 데이터(극좌표)로 변환하는 과정이다.

import numpy as np
import matplotlib.pyplot as plt

# simple airplane property
# initial velocity
init_vel = np.array([[100.0],
                     [0.0]]);
# initial position
init_pos = np.array([[0.0],
                     [3000.0]])

# observe time
# 관측 시간 간격
delta_t = 0.1#s
# 관측 시간
total_time = 20;#s
# 관측 시간 간격의 수
total_step = int(total_time/delta_t);

t = [(i*delta_t) for i in range(total_step+1)];

# 데카르트좌표게에서 극좌표계로 변환 함수
def CartesianToPolar(x,y):
    r = np.sqrt((x*x)+(y*y));
    theta = np.arctan(y/x);
    return theta,r;

# 극좌표계에서 데카르트좌표로 변환 함수
def PolarToCartesian(theta,r):
    x = r*np.cos(theta);
    y = r*np.sin(theta);
    return x,y

# sin형태 외력에 의한 이동변위 정의(난기류 가정)
frequncy = 0.1#Hz 
offset = 0.0;
magnitude = 100.0;# (A/(m*w^2))

# 데카르트 좌표로 기록
path_list = [];
# 극 좌표로 기록
radardata_list = [];

for time in t:
    # 동차해(Mx'' =0)
    # 등속운동의 위치해 x = x0 + v*t
    pos = init_pos + init_vel*time;

    # 부분해(외력항에 의한) y만 외력 영향이 있음.
    x = pos[0][0];
    # x = x0 + v0*t + (A/(m*w^2)) * sin(w*t)
    y = pos[1][0] + magnitude*np.sin((2*np.pi*frequncy)*time+offset);


    path_list.append([x,y]);
    theta_,r_=CartesianToPolar(x,y)
    radardata_list.append([theta_,r_]);

np_path_list = np.array(path_list);
np_radardata_list = np.array(radardata_list);

왼쪽 그래프 : X,Y 데카르트좌표계에서 비행체 움직임, 오른쪽 그래프 : r, θ 극좌표계인 라이더에서 본 비행체 움직임

데카르트 좌표계에서 비행체 움직임 (왼쪽 그래프)

• 초록색 선: 비행체의 실제 이동 경로
• 파란색 점: 각 시간 스텝별 위치
• x축 방향으로 등속 이동, y축 방향으로 사인(sine)형 변위 발생

극좌표계에서의 레이더 관측 (오른쪽 그래프)

• 초록색 선: 비행체의 실제 이동 경로를 극좌표계로 변환
• 파란색 점: 각 시간 스텝별 거리(r)와 각도(θ)
• 비행체가 x축을 따라 이동함에 따라 θ가 점진적으로 변화
• 난기류에 의한 y축 변위로 인해 거리(r)의 진동이 발생

이때 그래프의 영점(0,0)은 레이더 센서 중심이다.

관측 노이즈 추가

실제 환경에서는 센서 측정값에 노이즈가 포함될 수 있다. 본 시뮬레이션에서는 레이더의 각도(θ)는 매우 정밀하여 노이즈가 없고, 거리(r) 값에만 노이즈가 포함된다고 가정한다

노이즈 생성코드

noise_radardata_list = np_radardata_list.copy();

# ±1% error
noise_std = 3000.0*(0.01/3)
# noise generation
noise = np.random.normal(0.0, noise_std, noise_radardata_list[:,1].shape)

# r에만 노이즈 영향이 있으며 각도는 노이즈 영향이 거의 없다 가정
noise_radardata_list[:,1] = noise_radardata_list[:,1] + noise;

위 코드로 생성한 노이즈관측 데이터를 각 좌표계별로 표현하면 아래의 그래프들과 같다.

[왼쪽&중앙 그래프 : X,Y 데카르트좌표계에서 비행체 움직임], [오른쪽 그래프 : r, θ 극좌표계인 라이더에서 본 비행체 움직임]

그래프 애니메이션에서 파란색 점은 각 시간 스텝별 위치 빨간점은 노이즈가 포함된 관측값이다.

중앙 그래프를 보면 노이즈의 방향이 위치에 따라 변하는 현상을 잘 관찰해 두자!

확장칼만필터

이제 비행체의 위치해(Position Function)와 관측 데이터 가 주어졌으므로, 이를 칼만필터(KF)에 확장 적용하는 방법을 살펴보겠다.

왼쪽그림: 칼만필터, 오른쪽그림: 확장칼만필터

먼저 위에서 왼쪽그림은 이전 칼만필터이며 오른쪽그림은 확장칼만필터 수식이다.

수식적으로도 확장 칼만 필터(EKF) 는 기존 칼만 필터(KF) 큰 차이는 없지만, 비선형 함수 f(x)와 측정 함수 h(x)를 선형 근사하는 과정이 추가된다는 점이 핵심이다.

위 그림에서는 F는 상태전이행렬인 A와 같지만 기존 칼만필터와 구별을 위해 표기를 바꾸었다.

자세히 확장 칼만 필터(EKF)는 기존 칼만 필터(KF)와 동일한 원리를 따르지만, 비선형 시스템에서도 적용할 수 있도록 설계되었다.

기본 칼만 필터는 선형 시스템을 가정하고 상태 전이를 행렬 연산으로 표현하지만, 확장 칼만 필터는 비선형 시스템에서도 동작할 수 있도록 상태 변화를 함수 형태로 표현하며, 이를 선형 근사하기 위해 자코비안 행렬을 활용한다.

즉, 기존 칼만 필터는 상태 변화를 행렬로 표현하는 반면, 확장 칼만 필터는 비선형 상태 변화를 함수 형태로 모델링하고, 이를 선형 근사하여 적용하는 방식이다.

확장 칼만 필터(EKF)의 적용 과정

확장 칼만 필터를 적용하기 위해서는 다음 세 가지 핵심 요소를 정의해야 한다.

1. 상태 예측 함수 (State Transition Function, f(x))
2. 상태 전이 행렬 (State Transition Matrix, F)
3. 측정 행렬 (Measurement Matrix, H)

1. 변분법을 이용한 상태 예측 함수 도출

다시 비행체의 위치 함수는 시스템의 응답경로(response trajectory)을 나타내는 식이며, 현재 상태(위치)에서 직접적인 상태 전이 방정식이 아니다.

즉, 현재 상태와 속도를 기반으로 다음 상태를 예측할 수 있는 상태 전이 함수를 새롭게 도출해야 한다.

이전까지는 연속 상태 공간 모델의 ODE(상미분방정식)를 이산화하여 해결했지만, 위치 함수만 주어진 경우 상태 전이 함수를 어떻게 도출할 수 있을까?

이 문제를 해결하기 위해 변분법(Variational Method) 을 활용할 수 있다.

변분법은 함수의 작은 변화량 δx에 따른 시스템 응답 변화를 계산하는 방법이다.

각 상태 예측에 영향을 주는 요소인 x, ẋ, t이며 각 요인별로 f를 편미분하여 합하고 근사하여 상태전이함수 f(x,ẋ,t)를 도출할 수 있다.

아래의 풀이는 주어진 위치해를 가지고 상태전이함수를 도출하는 방법을 기술한다.

위치 함수는

이다.

속도 함수는

이다.

각 변수에 대한 편미분은 다음과 같다.

• 상태 x에 대한 편미분

• 속도 ẋ 에 대한 편미분

• 시간 t에 대한 편미분

각 변위함수를 ∂x(t), ∂ẋ(t) 별로 모두 합하면

위치에 대한 총 변위함수은

그리고, 속도에 대한 총 변위함수은

이다.

위 에서 도출한 식은 ∂x(t), ∂ẋ(t) 함수는 각 x, ẋ, t 변수에 대한 변위량이다. 따라서

아래와 같이 정리될 수 있다.

우리가 구하고자 상태전이는 현재 상태 x,ẋ,t 가 주어질때 다음상태를 구하는 x(x,ẋ,t), ẋ(x,ẋ,t) 함수이다.

이를 오일러 방법(x = x + Δx , ẋ = ẋ + Δẋ)으로 표현하면,

이다.

이산 시간 t_k​에서의 업데이트 식으로 표현하면 다음과 같이 된다.

여기서 f는 상태전이함수f로 이며, uk는 제어입력으로 자유운동의 경우 uk=0

이 과정을 통해 비행체의 상태 변화에 대한 수식을 직접 도출 하고, 이를 오일러 방법(Euler Method)을 사용하여 이산 시간 방정식으로 변환한다.

이와 같은 변분법을 이용한 상태함수를 만드는 과정은 유체역학의 나비에-스토크스 방정식(Navier–Stokes equation)이나 최적 제어(Optimal Control) 문제에서도 자주 활용된다.

이제 도출한 상태 예측 함수를 기반으로 이산 상태 행렬 F(=A)와 측정 행렬 H도 동일한 방식으로 자코비안을 사용해 구할 수 있다.

변분법 과 자코비안(Jacobian) 관계

자코비안 행렬은 다변수 함수의 편미분 관계를 나타내는 행렬이다. 편미분을 이용하여 이산 시간 상태 공간 모델을 도출하는 것은 오일러방법(Euler Method)에 기반한다. 오일러 방법은 테일러 급수에서 유도된 x=x+Δx 형태의 식을 기반으로 하는, 기본적인 미적분 개념을 이용한 수치해석 방법이다.

즉, 변분법을 통해 함수의 작은 변화를 선형 근사하면, 그 결과는 자코비안 행렬의 형태를 띠게 된다.

2. 자코비안 행렬을 활용한 상태 전이 행렬 도출

상태 전이 행렬 F(=A) 는 상태 변수의 변화량을 나타내는 자코비안 행렬 이다.

비행체의 운동 방정식에서 도출된 상태전이함수 f(x) 를 각 상태 변수에 대해 편미분하면 상태 전이 행렬을 얻을 수 있다.

아래는 상태 전이 행렬을 구하는 과정을 보여준다.

일반해인 f(t)을 시스템 입력인 각 변위량 X=[x, ẋ] 대한 편미분 이므로 구한 편미분 함수를 f′(t,x)라 하자

이 미분 함수 ∂f/∂x=f′(t,x)를 적분하면,

로 표현된다. 이를 오일러방법으로 근사한 x=x+Δx 형태로 나타내면,

이 된다.

스칼라 표현으로는

이지만, 행렬 표현에 의해 x와 Δx가 입력으로 분리되어 자코비안(Jacobian) 행렬 형태

으로 나타낼 수 있다.

이때 칼만필터의 이산시간 상태공간 모델인

과 자코비안 표현식

을 비교하면, 기존 칼만 필터에서 사용하던 이산 상태 행렬 Ad 와 동일한 역할을 수행하며, 확장 칼만 필터에서는 자코비안 행렬 J(t,xk)을 사용하여 이를 대체할수 있음을 알 수 있다.

따라서 위치해로부터 편미분하여 얻은 자코비안 행렬 J(t,xk)은 연속시간 상태공간 모델의 시스템을 이산화할 때, 이산 상태 행렬 A로 사용할 수 있다.

주의: 주의해야 할 점은, 자코비안 행렬 J(t,xk)의 미분 대상이 시간 t 가 아니라, 상태 변수 x 에 대한 것 이라는 점이다.

즉, 상태 예측을 위한 전이 함수와는 일부 차이가 있다.

처음 미분방정식 설명하면서 등장한 동치해가 시스템행렬이고 상태전이함수와 차이가 사실 부분해라는걸 알아두면 좋다.

자코비안 행렬 J과 이산 상태 행렬 A 관계

위 그림은 각 역학식에서 구한 이산 상태 행렬 A과 일반해에서 도출한 자코비안J 구하는 차이를 보여준다.

이 차이가 미분방정식인 ẍ(t) 에서 시작했느냐 그 해인 x(t)로 시작하는지에 대한 차이일 뿐이다.

이를 차이를 쉽게 ẍ(t)를 쌓아 모델을 구축하는 것과 x(t)를 깍아 모델구하는 것의 차이에 비유할 수 있다.

3. 자코비안 행렬과 측정 행렬 H의 도출

측정 행렬 H는 측정 함수 h(x)를 편미분한 자코비안 행렬 로 정의된다.

센서(레이더)는 비행체의 위치를 극좌표( r, θ ) 형태로 측정 하므로, 데카르트 좌표( x,y) 로 변환하는 변환식이 필요하다.

이때 측정함수h는 데카르트 좌표계에서 극좌표계로 변환식이다.

측정함수 h(x, y) 변환식

측정 행렬 H는 측정 데이터(극좌표)와 시스템 상태(데카르트 좌표) 간의 관계를 나타내는 선형 근사 행렬이다.

측정 행렬 H 의 필요성

여기서 잘 생각해보면 센서가 제공하는 데이터( r,θ ) 를 미리 변환하여 데카르트 좌표( x,y) 형태로 변환 후 사용한다면, 측정 행렬 H를 단위 행렬(I)로 설정할 수도 있다.

하지만, 이 방법은 측정 노이즈를 왜곡시킬 위험이 있다.

이 왜곡은 극좌표 데이터를 미리 데카르트 좌표로 변환하면, 노이즈의 분포와 상관관계가 복잡해지기 때문이다.

극좌표에서 발생하는 노이즈는 거리( r) 방향으로 존재하지만, 이를 데카르트 좌표로 변환하면 노이즈의 방향과 분포가 복잡해진다.

따라서, 측정 노이즈를 보다 선형적으로 처리하고 시스템과 관측 모델을 분리하여 모듈성을 유지하려면, 자코비안 행렬을 이용하여 측정 행렬 H 를 직접 정의하는 것이 바람직하다.

이렇게 하면, 센서의 비선형적인 측정 특성을 더 정확하게 반영할 수 있으며, 확장 칼만 필터의 업데이트 과정에서 측정값과 상태 추정 간의 관계를 보다 정밀하게 조정할 수 있다.

말로는 복잡하니 비행체의 위치를 레이더 관측을 통해 이해를 도울 수 있다.

아래 그림들을 보면 비행체의 위치를 레이더로 관측할 때, 시간 흐름에 따라 관측된 데이터(빨간점)와 실제 궤적(파란점)의 차이가 발생 한다.

X,Y 데카르트좌표계에서 비행체 움직임(파란점)과 관측위치(빨간점)

이 차이는 센서의 노이즈( r 방향 오차)와 극좌표 변환 과정에서 발생하는 왜곡으로 인해 발생 한다.

이 왜곡 현상은 두 관측계와 시스템계간 관계성을 보면 답이 있다.

관측 노이즈(noise)는 레이더의 거리r 만 있지만 그 관측 노이즈가 시스템이 속한 데카르트계인 x, y 위치에 삼각함수로 반영되기 때문이다.

극좌표계에서 발생한 노이즈와 데카르트에서 발생한 노이즈의 관계

따라서 관측값을 미리 x, y 형태로 변환하여 사용한다면 관측값 속 비선형 노이즈는 칼만필터에 비선형 공분산으로 영향을 준다.

따라서, 확장 칼만 필터에서는 측정 함수 h(x)를 사용하여 극좌표 데이터를 직접 활용하고, 이를 선형 근사한 자코비안 행렬을 측정 행렬 H 로 설정한다.

확장칼만필터 구현

이 글에서는 두 방법을 비교하기 위해 저역통과 필터(LPF)와 함께 확장 칼만 필터(EKF)를 구현 하고 성능을 분석한다.

바로 위에서 설명한 측정 왜곡을 보이기 위해 확장 칼만필터를 두가지로 구현하였다.

자코비안-확장칼만 필터 (Jacobian-EKF)

이 방법에서는 센서 데이터를 직접 사용하고, 측정 행렬 H를 자코비안 행렬로 계산하여 적용한다. 이를 통해 관측 노이즈의 영향을 보다 정확하게 반영할 수 있다.

선변환-확장칼만 필터 (Transform-EKF)

이 방법에서는 센서 데이터를 시스템 좌표계로 변환한 후 EKF를 적용한다. 그러나 변환 과정에서 노이즈가 왜곡될 가능성이 있다.

비교코드

비교를 위해 이 코드에서는 선변환-확장칼만필터, 자코비안-확장칼만필터, 저역통과필터를 구현한다.

import numpy as np
import matplotlib.pyplot as plt
from scipy.special import ellipj
from scipy.special import ellipk
from matplotlib.animation import FuncAnimation
from scipy.linalg import expm

class JacobianEKF:
    def __init__(self, init_state, dt=0.01):
        self.dt = dt
        self.x = init_state  # [ [x], [x'], [y], [y'] ]

        # 통계로 에러추청
        r_sigma = (noise_radardata_list[:,1] - np_radardata_list[:,1]).std()
        theta_sigma = (noise_radardata_list[:,0] - np_radardata_list[:,0]).std() 

        # 프로세스 공분산 설정   [ [σx], [σx'], [σy], [σy'] ]
        self.Q = np.eye(4) * [0.01, 0.01, 0.5, 15.0];# y축 변화량이 x축보다 크므로 비대칭 설정 필요


        # 측정 노이즈 공분산 행렬 R (관측 기준)
        self.R = np.array([ [r_sigma**2, 0.0], # 거리 오차 
                            [0.0, theta_sigma**2]])  # 각도 오차

        self.P = self.Q.copy()

    def h(self, state):
        """
        측정방정식 h
        """
        x, vx, y, vy = state[0,0], state[1,0], state[2,0], state[3,0]
        r       = np.sqrt(x**2 + y**2)
        theta   = np.arctan2(y,x)
        return np.array([[r],[theta]])

    def step(self, meas_r, meas_theta,t):

        previous_state = self.x;

        x, vx, y, vy = previous_state[0,0], previous_state[1,0], previous_state[2,0], previous_state[3,0]

        # 상태에 따라 F, H 계산
        # 상태전이행렬 계산
        Fk =  np.array([    [1.0, self.dt, 0.0, 0.0],
                            [0.0, 1.0, 0.0, 0.0],
                            [0.0, 0.0, 1.0, self.dt],
                            [0.0, 0.0, 0.0, 1.0],]);
        # 측정행렬 계산
        r = np.sqrt(x**2 + y**2)
        rr = (x**2 + y**2)
        Hk = np.array([ [x/r, 0, y/r, 0],
                        [-y/rr, 0, x/rr, 0], ])

        # 실제 측정값 (노이즈 포함)
        z_meas = np.array([[meas_r],[meas_theta]])

        # 상태예측
        x_next              = x + vx*self.dt;
        vx_next             = vx;
        y_next              = y + vy*self.dt + magnitude*omega*np.cos(omega*t)*self.dt;
        vy_next             = vy - magnitude*omega*omega*np.sin(omega*t)*self.dt;
        predic_state = np.array([[x_next],[vx_next],[y_next],[vy_next]]);

        # 오차 공분산 예측
        P_pred = Fk @ self.P @ Fk.T + self.Q

        # 칼만이득 계산
        S = Hk @ P_pred @ Hk.T + self.R
        K = P_pred @ Hk.T @ np.linalg.pinv(S)

        #추종값 계산
        z_pred = self.h(predic_state)
        z = z_meas - z_pred
        self.x = predic_state + K @ z

        # 오차공분산 계산
        I = np.eye(4)
        self.P = (I - K @ Hk) @ P_pred
        return self.x.copy()

class TransformEKF:
    def __init__(self, init_state, dt=0.01):
        self.dt = dt
        self.x = init_state  # [ [x], [x'], [y], [y'] ]

        # 통계로 에러추청
        y_sigma = (noise_pathdata_list[:,1] - np_path_list[:,1]).std()
        x_sigma = (noise_pathdata_list[:,0] - np_path_list[:,0]).std()

        # 프로세스 공분산 설정  [ [σx], [σx'], [σy], [σy'] ]
        self.Q = np.eye(4) * [0.01, 0.01, 0.5, 15.0]# y축 변화량이 x축보다 크므로 비대칭 설정 필요

        # 측정 노이즈 공분산 행렬 R (시스템기준)
        self.R = np.eye(4) * [x_sigma**2, x_sigma**4 ,y_sigma**2, y_sigma**4]

        self.P = self.Q.copy();

    def h(self, state):
        """
        측정방정식 h
        """
        x, vx, y, vy = state[0,0], state[1,0], state[2,0], state[3,0]
        return np.array([[x], [vx], [y], [vy]])


    def step(self, meas_x, meas_y,t):        
        previous_state = self.x;
        x, vx, y, vy = previous_state[0,0], previous_state[1,0], previous_state[2,0], previous_state[3,0]

        # 상태에 따라 F, H 계산
        # 상태전이행렬 계산
        Fk =  np.array([    [1.0, self.dt, 0.0, 0.0],
                            [0.0, 1.0, 0.0, 0.0],
                            [0.0, 0.0, 1.0, self.dt],
                            [0.0, 0.0, 0.0, 1.0],]);

        # 측정행렬 계산
        Hk = np.array([ [1, 0, 0, 0],
                        [0, 0, 0, 0],
                        [0, 0, 1, 0],
                        [0, 0, 0, 0], ])

        # 실제 측정값 (노이즈 포함)
        z_meas = np.array([[meas_x],[0],[meas_y],[0]])

        # 상태예측
        x_next              = x + vx*self.dt;
        vx_next             = vx;
        y_next              = y + vy*self.dt + magnitude*omega*np.cos(omega*t)*self.dt;
        vy_next             = vy - magnitude*omega*omega*np.sin(omega*t)*self.dt;
        predic_state = np.array([[x_next],[vx_next],[y_next],[vy_next]]);

        # 오차 공분산 예측
        P_pred = Fk @ self.P @ Fk.T + self.Q

        # 칼만이득 계산
        S = Hk @ P_pred @ Hk.T + self.R
        K = P_pred @ Hk.T @ np.linalg.pinv(S)

        #추종값 계산
        z_pred = self.h(predic_state)
        z = z_meas - z_pred
        self.x = predic_state + K @ z

        # 오차공분산 계산
        I = np.eye(4)
        self.P = (I - K @ Hk) @ P_pred
        return self.x.copy()



#자코비안-확장칼만============================================================
px, py = init_pos[0][0],init_pos[1][0]
vx, vy = init_vel[0][0],init_vel[1][0]
init_state = np.array([[px],[vx],[py],[vy]])
jacobian_ekf = JacobianEKF(init_state, dt=delta_t)

#기록용
trajectory_list = [[px, py]]
lpf_trajectory_list = [[px, py]]

#저역통과필터 변수
lpf_x,lpf_y,gain=px,py,0.85

for i,(theta_,r_) in enumerate(noise_radardata_list):
    if(i==0):
        continue;

    # 자코이안 확장칼만추정
    x_state = jacobian_ekf.step(r_,theta_,delta_t*(i+1))
    x, vx, y, vy = x_state[0,0], x_state[1,0], x_state[2,0], x_state[3,0]
    trajectory_list.append([x, y])

    #저역통과필터
    x_,y_ = PolarToCartesian(theta_,r_)
    lpf_x = lpf_x*gain + (1-gain)*x_
    lpf_y = lpf_y*gain + (1-gain)*y_
    lpf_trajectory_list.append([lpf_x, lpf_y])

trajectory_list = np.array(trajectory_list)
lpf_trajectory_list = np.array(lpf_trajectory_list)

#선변환-확장칼만============================================================
px, py = init_pos[0][0],init_pos[1][0]
vx, vy = init_vel[0][0],init_vel[1][0]
init_state = np.array([[px],[vx],[py],[vy]])
pretrans_ekf = TransformEKF(init_state, dt=delta_t)

#기록용
trajectory2_list = [[px, py]]
for i,(theta_,r_) in enumerate(noise_radardata_list):
    if(i==0):
        continue;

    # 선변환
    x_, y_ = PolarToCartesian(theta_,r_)
    # 선변환 확장칼만추정
    x_state = pretrans_ekf.step(x_, y_,delta_t*(i+1))

    x, vx, y, vy = x_state[0,0], x_state[1,0], x_state[2,0], x_state[3,0]
    trajectory2_list.append([x, y])

trajectory2_list = np.array(trajectory2_list)



plt.clf()
plt.plot(np_path_list[:,0],np_path_list[:,1],'g');
plt.plot(np_noise_path_list[:,0],np_noise_path_list[:,1],'ro');
plt.plot(trajectory_list[:,0],trajectory_list[:,1],'b');
plt.plot(lpf_trajectory_list[:,0],lpf_trajectory_list[:,1],'y');
plt.plot(trajectory2_list[:,0],trajectory2_list[:,1],'m');

비교 결과

빨간점: 관측값(w노이즈), 노란선: 저역통과필터, 파란선:자코비안-확장칼만, 보라선 : 선행변환-확장칼만, 초록선: 비행체 경로

두 자코비안-확장칼만과 선변환-확장칼만간 차이는 뚜렿하다. 그러나 칼만필터같은 재귀식필터들은 경로만 보고는 판단하기 힘들다.

경로만 보면 저역통과필터 성능도 나름 나쁘지는 않게 보이기 때문이다.

시간 순으로 각 필터들의 추정위치를 살펴보자

저역통과필터(LPF)

애니메이션 특성상 시각적 혼돈을 막기 위해 노이즈 위치는 표시하지 않았다.

비행체의 위치(=파란점)와 비교해보면 LPF의 추정 위치(=빨간점)는 실시간으로 볼때 지연과 상(Phase) 왜곡이 확인된다.

두 확장칼만필터 비교

왼쪽그래프: 자코비안-확장칼만필터 ,오른쪽 그래프: 선변환-확장칼만필터

자코비안-확장칼만은 노이즈 환경속에서 원래 궤적을 정밀하게 추정하는 모습을 보인다.

그에 반해 선변환-확장칼만과 지연 부분을 좋으나 궤적의 형상은 LPF과 비교했을때 비슷한 경향을 가진다.

자코비안-확장칼만와 선변환-확장칼만간 차이의 보다 효과적을 설명하기 위해 비행체의 원본 궤적과 오차 그래프로 아래에서 나타내었다.

극좌표계 : 시간에 따른 거리 R, θ오차 비교

비행체의 극좌표 위치에서 추정값을 극좌표로 변환하여 빼서 오차를 비교함.

[왼쪽 그래프 Y축 : R, 오른쪽 그래프 Y축 : θ], [왼쪽,오른쪽 그래프 X축: 시간]

빨간점: 관측값(w노이즈), 노란점: 저역통과필터, 파란점:자코비안-확장칼만, 보라점 : 선행변환-확장칼만

왼쪽 그래프 R비교를 보면 선변환-확장칼만와 자코비안-확장칼만은 비교하면 차이가 크게 발생하지 않았다. 그러나 오른쪽 그래프 θ비교를 보면 자코비안-확장칼만은 추정 일관성을 가짐을 알 수 있다.

이는 자코비안-확장칼만 측정행렬 H에 의한 오차 공분산 독립성이 반영되기 때문이다.

데카르트좌표계 : X, Y 오차 분포 비교(시간누적)

비행체의 데카르트 위치에서 추정값을 빼서 오차를 비교함.

[왼쪽,오른쪽 그래프 Y축 : y], [왼쪽,오른쪽 그래프 X축 : x]

빨간점: 관측값(w노이즈) , 노란점: 저역통과필터, 파란점:자코비안-확장칼만, 보라점 : 선변환-확장칼만

오른쪽 그래프는 왼쪽 그래프의 확대그림이다. 확대된 그래프에서 자코비안-확장칼만의 추정위치분포는 노이즈가 있는 관측값내 위치함을 볼 수 있다. 위에서 보았던 각도θ 추정성에 의한 것이다.

그리고 변환-확장칼만의 추정위치의 분포 자코비안-확장칼만과 비슷한 범위 군에 있지만 원형에 가까운 모습을 보인다.

이것은 선변환-확장칼만의 추정위치의 분포가 원형으로 나타나는것도 각도에 따른 노이즈가 시간에 따라 변하여 그 공분산이 x, y 방향으로 고르게 반영이 되었기 때문이다.

또한 자코비안-확장칼만의 경우 계측 오차의 공분산 R 설정부분을 보면, 이상적 비행궤적과 차이로 구하게 되는데 각도θ와 거리r의 통계가 θ는 0으로 나타나게 된다. 따라서 자코비안-확장칼만은 실질적으로 계측 오차중 거리r만 고려하면 된다.

그러나 선변환-확장칼만은 이상적 비행궤적과 차이는 x, y방향으로 나타나 계측 오차가 비선형적으로 x, y 공간에 반영되었고 시간에 흐름에 따라 부정확한 값을 가지게 된다.

특히 선변환-확장칼만 그래프애니메이션에서 잘보면 x, y가 변화량이 많은 변하는 각도θ(=90,180,270,360도)에서 더 부정확한 감소 경향을 모습을 보이게 된다.

결론

비선형 시스템에서 확장 칼만 필터(EKF)와 자코비안 적용이 어떻게 되는지 소개하였으며, 분석과 확장 칼만 필터 구현을 통해 역학식을 모르더라도 비선형 해로부터 칼만 필터에 확장 적용이 가능함을 확인하였다.

또한, 기존 역학식과 비교했을 때 자코비안-확장 칼만 필터는 모델 도출 과정의 시작점이 다르다는 점을 보였다.

마지막으로, 관측과 시스템 간 좌표계 차이로 인해 발생하는 노이즈 공분산 문제를 해결하기 위해 H 행렬의 역할이 중요함을 보였다. 특히, 비선형성을 방지하고 안정적인 추정을 수행하기 위해 자코비안-확장 칼만 필터가 관측 모델을 선형 근사하여 보다 신뢰성 있는 결과를 도출할 수 있음을 확인하였다.

이 글을 통해 관측과 시스템 간 차이에서 발생하는 오차가 자코비안-확장 칼만 필터와 선변환-확장 칼만 필터에서 어떤 영향을 미치는지 이해할 수 있을 것이다.

확장 칼만 필터(EKF)의 효과

확장 칼만 필터는 구조적으로 관측값과 재귀성(Recursion)으로 인해 시스템 정의가 엄밀하지 않더라도 동작할 수 있다.

자코비안-확장칼만 상태예측 부분 코드

시간편미분없는 상태전이행렬과 시간편미분있는 상태전이행렬 비교

        # 상태예측 - 시간편미분 포함
        #y_next              = y + vy*self.dt + magnitude*omega*np.cos(omega*t)*self.dt;
        #vy_next             = vy - magnitude*omega*omega*np.sin(omega*t)*self.dt;
        # 상태예측 - 시간편미분 미포함
        y_next              = y + vy*self.dt ;
        vy_next             = vy ; 

왼쪽그래프: 시간편미분포함-자코비안-확장칼만필터 ,오른쪽 그래프: 시간편미분미포함-자코비안-확장칼만필터

빨간점: 관측값(w노이즈), 파란선:시간편미분포함-상태전이행렬(자코비안-확장칼만), 보라선 :시간편미분미포함-상태전이행렬(자코비안-확장칼만), 초록선: 비행체 경로

결과는 시간편미분항이 추가되면 초기추정에 강한 영향을 주지만 시간이 충분한 흐른다면 시간편미분항이 고려하지 않더라도 수렴하는 모습을 확인할 수 있다.

이는 일부 오차를 프로세스 노이즈로 간주할 수 있기 때문이다.

그러나 확장 칼만 필터가 모든 오차를 완전히 극복할 수 있을까?

한계

결론적으로, 작은 오차는 극복 가능하지만, 일정 수준을 넘어서면 필터가 발산할 수 있다. 이 발산은 역설적으로 자코비안과 재귀적 업데이트 구조 자체에서 기인한다.

자세히, 예측은 비선형 함수를 기반으로 도출되며, 현재 입력 상태 x및 시간 t 에 따라 변하는 상태 전이 함수이다. 근본적으로 이는 근사 모델을 사용하므로 필연적으로 모델 오차가 존재하며, 이 모델 오차와 관측(=계측) 오차가 칼만 필터의 재귀적 업데이트 과정에서 누적되면서 임계치를 넘어서면 필터가 발산할 수밖에 없다.

이를 방지하기 위해 프로세스 공분산을 크게 설정하면, 결과적으로 칼만 이득(Kalman Gain)의 조정으로 인해 관측값의 비중이 증가하게 되는데, 이로 인해 필터의 성능이 저하되고 효과를 상실할 수 있다.

본문에서 소개한 선변환-칼만 필터(Pre-transformed Kalman Filter)의 Q 및 R공분산 조정을 통해 구조적 수렴 한계를 확인하는 실험을 진행하면 이를 더욱 명확히 이해할 수 있다.

확장 칼만 필터(EKF)도 결국 선형 칼만 필터에서 선형화 과정을 거쳐 확장된 형태이므로, 시스템 정의의 엄밀성에 따라 필터 성능의 한계가 존재한다.

여담

선형 칼만 필터든 확장 칼만 필터든, 둘 다 응용과 설계의 핵심은 각 시스템에 대한 명확한 수학적 모델링과 정의에 있다.

필자 역시 처음 칼만 필터를 배울 때, 교수나 선배들이 행렬과 수식을 물 흐르듯 설명해 주었다. 그 설명을 들을 때는 마치 밥 로스가 그림을 그리듯 쉽게 이해되는 듯했지만, 막상 실제로 응용하려고 하면 A 행렬조차 제대로 정의하는 것이 쉽지 않았다. 그때마다 하나씩 공부하고 경험을 쌓아가면서 결국 패턴을 파악하고 익숙해지게 되었다.

그리고 나중에 역학들을 마스터하면 선형화나 최적화의 개념은 기계공학에서 다루는 시스템 접근법과 유사하다는 점도 깨달았다.

예를 들어, 진동학, 고체역학, 동역학, 유체역학, 열역학 등의 강의에서 설명하는 방식은 다를지라도, 근본적인 접근 방식은 상당히 비슷하다는 점이 흥미롭다.