추정할 상태와 센서가 읽는 양을 먼저 적는다
위치를 알고 싶다고 모든 센서가 위치를 직접 알려 주는 것은 아니다. 상태 x가 위치/속도를 포함할 수도 있고 센서는 그중 위치만 읽거나 비선형 거리만 읽을 수도 있다. 상태 모델은 시간에 따라 상태가 어떻게 변한다고 가정하는지, 관측 모델은 그 상태에서 어떤 센서값을 예상하는지를 설명한다. 관측값을 상태와 바로 빼기 전에 같은 양·단위·프레임·시각인지 확인한다.
선형 모델을 x_k=F x_{k−1}+w, z_k=H x_k+v로 적는다. k는 현재 시각의 번호, F는 상태 변화, H는 상태에서 관측을 만드는 행렬이다. w는 모델로 설명하지 못한 변화, v는 관측 잡음이다. 상태가 n성분, 관측이 m성분이면 F는n×n, H는m×n이고 상태와 관측의 길이가 같을 필요는 없다. 상태가(위치,속도)이고 관측이위치라면 H=[1,0]이다. 제어입력은 이번 단순 모델에서 생략한다.
Kalman필터는 예측한 상태와 관측의 관계를 불확실성과 함께 갱신한다. 선형 모델과 초기/잡음 가정이 맞는 조건에서 의미가 있는 계산이다. 가우시안·평균0·정해진 독립성 가정은 센서가 원래 완벽하다는 뜻이 아니다. 바이어스·잘못된 대응·시간/축 오류나 상관을 누락하면 계산은 되어도 추정과 보고된 신뢰도가 틀릴 수 있다. FilterPy 저자 문서는 상태/관측 차원과 각 행렬을 구분한다. 기본값만 둔다고 문제에 맞는 모델이 완성되는 것은 아니다.
P/Q/R는 서로 다른 불확실성이다
P는 상태 추정 오차의 공분산, Q는 이번 예측 구간의 모델 잡음 공분산, R은 관측 잡음 공분산이다. 1D위치라면 각각분산이고 단위는m²다. 표준편차가0.5m이면 분산R=0.25m²다. 센서가 “오차0.5”라고 썼다는 이유만으로 R=0.5를 넣지 말고 무엇의 어떤 척도인지 확인한다. 위치/속도가 섞인 상태에서는 성분별 단위도 달라진다.
그림 M14.04-1. 각 칸은 서로 다른 역할/근거다. 아래 방향은 자료/처리의 연결이며 실제 성공을 자동으로 보장하지 않는다. 직접 제작한 개념도이며 본문의 조건과 함께 읽는다.
공분산의 대각은 각 성분의 분산이고 비대각은 함께 변하는 오차의 관계다. 예를 들어 위치x/y의 행렬[[4,1],[1,1]]m²는 x분산4,y분산1과양의공분산1을 뜻한다. 이를 한 숫자의 확신 점수로 줄이면 방향/상관 정보를 잃는다. 축을 회전하면 공분산도 같은 좌표변환에 맞게 바꿔야 한다. M14.02의 센서축을 바꾸고 공분산만 원래축으로 두지 않는다.
유효한 공분산은 대칭이며 어떤 선형 조합의 분산도 음수가 되지 않아야 한다. 어려운 행렬판정법 대신 반례를 보자. [[1,2],[2,1]]이라면 x−y의분산은1+1−2×2=−2다. 분산은음수일수없으므로 이행렬은유효하지않다. 대각이양수라는검사만으로충분하지않다. 어떤메시지의0/미제공표시와수학적으로알려진무오차0도구분한다.
한 차원에서 예측과 갱신을 따라간다
기본 예제는 위치만 있는 random walk다. 이는 이번 구간의 평균 이동을0으로 두고 설명하지 못한 변화w를 더한다는 모델이다. F=H=1, 이전 평균x=10m/P=4m², 이번구간 Q=1m²다. 모델잡음과이전추정오차는독립이라고가정한다. 위첨자−는관측전예측, +는관측후갱신값을뜻하며음수/양수부호가아니다.
예측 평균은 x⁻=10m, 예측 분산은 P⁻=P+Q=5m²다. 위치 평균이 그대로여도 시간이 지나며 모델오차가 늘어 분산은 커졌다. 실제 시간 간격이 달라지면 F/Q도 그 모델에 맞춰 다시 정해야 한다. 여기의 Q1을 모든 센서 주기의 보편값으로 쓰지 않는다.
현재 관측z=12m, R=5m²라고 하자. 혁신(innovation) ν=z−H x⁻는 관측에서 예측관측을 뺀 값으로2m다. M13.02에서 잔차를 예측−목표로 정의했으므로 여기서는 부호 계약이 반대임을 주의한다. 일반식의 혁신 공분산은 S=H P⁻ H전치+R이며, 이번 H1에서는 S=5+5=10m²다. 이 합은 예측오차와 관측잡음의 독립성 가정에 의존한다.
Kalman이득 K는 이 예제에서 P⁻/(P⁻+R)=0.5다. 관측과예측의차이를얼마나반영할지를정하는값이다. 다음식은 H=1인이번스칼라갱신이다.
따라서 x⁺=10+0.5×2=11m, P⁺=(1−K)P⁻=2.5m²다. 평균은 중간값으로 이동했고 모델이 계산한 분산은 줄었다. 실제정답이11m라고확인한것은아니다. 두정보의분산이같아중간이된것이며관측마다무조건반씩섞는일반규칙도아니다.
이 가중치의 이유는 고정된 예측과관측을 각각의분산으로나눈제곱오차 (x−x⁻)²/P⁻+(z−x)²/R를작게만드는것으로볼수있다. x에대한변화율을0으로놓으면 (R x⁻+P⁻ z)/(P⁻+R)가되고위갱신식과같다. 불확실성이큰값에더큰가중치를준것이아니다. 정확한분산/독립성가정이있을때의해석이다.
관측 신뢰도를 바꿀 때와 관측이 없을 때
아래 비교는 모두 동일한예측x⁻10/P⁻5와관측z12에서별도로시작한다. 표의위에서아래로연속갱신하지않는다.
| 관측분산R(m²) | K | 갱신평균x⁺(m) | 갱신분산P⁺(m²) |
|---|---|---|---|
| 0.5 | 10/11 | 130/11≈11.818 | 5/11≈0.455 |
| 5 | 1/2 | 11 | 2.5 |
| 50 | 1/11 | 112/11≈10.182 | 50/11≈4.545 |
이 모델에서 R이작으면관측을더많이반영한다. 센서값이“내가원하는값”에가깝다는이유로R을작게쓰는것은정당한신뢰도모형이아니다. 실제오차·환경/거리·보정·상관에대한근거가필요하다. 잘못된좌표나고정바이어스를분산튜닝으로숨기지않는다.
관측이없으면이예제는x⁻10/P⁻5까지만예측하고갱신을생략한다. z=0을새관측으로만들어위치를원점으로끌지않는다. 같은예측구간이한번더지나고추가정보가없으면 F1/Q1의가정에서평균10,분산6이다. 예측의시각은진행되지만마지막실측시각은유지한다. M13.06의가림/수명기록과연결되는구분이다.
혁신의 크기를 보고도 모델과 입력을 재검토한다
차이의 제곱을 S로나눈 정규화 혁신제곱 d²=ν²/S는 이번1D에서 단위 없는 검사량이다. 예측/관측불확실성에비해차이가얼마나큰지읽는다. 교육용수용조건을d²≤9로명시한다. 이는통계적조건을검증한보편적인센서문턱이나안전기준이아니다.
앞 예제z12는ν2/S10이라d²=0.4로조건안이다. 별도관측z22는ν12,S10이라d²=14.4로조건밖이다. 이번정책은제외이유/원관측/시각을남기고관측갱신없이예측값10/P5를유지한다. R을임의로늘려무조건통과시키거나제외된관측을정확한0으로바꾸지않는다.
이상이 센서오류인지, 로봇이모델보다빠르게움직였는지, 시각/대응/축이틀렸는지는이숫자하나로알수없다. 조건에어긋나는혁신이계속되면모델·잡음/바이어스·입력계약을검토한다. 작은혁신도공유오차때문일수있다. 이미같은IMU를사용한두추정값을독립이라고다시융합하면확신만과해질수있다. 관측개수보다독립정보의근거가중요하다.
EKF는 비선형 관측을 현재 추정 주변에서 근사한다
EKF는 Extended Kalman Filter다. 비선형상태/관측함수의현재추정주변변화율을써서필터를근사한다. FilterPy EKF 공식문서는 관측예측함수와그Jacobian을구분한다. Jacobian은여러입력/출력사이의국소변화율을모은행렬이며이번1D관측에서는미분값하나다. 함수값과변화율은다른역할이다.
알려진수직거리b=1m에있는센서가수평위치x의물체까지읽는거리를 z=h(x)=sqrt(x²+b²)로모델링하자. z/x/b는모두m이고측정잡음은거리에더해진다고가정한다. 관측함수 h는현재x에서예상거리를만든다. 국소변화율은 H=x/sqrt(x²+b²)다. 제곱근의변화율1/(2sqrt(·))에안쪽x²의변화율2x를곱해얻는다. 이H는일반적으로x마다달라진다.
스칼라 EKF에서는 S=H²P⁻+R, K=P⁻H/S로 계산하고 x⁺=x⁻+K(z−h(x⁻)), P⁺=(1−KH)P⁻로 갱신한다. H는 예측 위치에서 평가한 변화율이다. 이 식의 P는 국소 선형화에 근거한 근사 불확실성이다.
초기예측x⁻=0m,P⁻=1m²,R=0.1m²,거리관측z=2m를보자. h(0)=1m라혁신은1m지만H=0이다. EKF이득의분자는P⁻H전치여서0, S=R=0.1이다. 따라서이국소선형갱신은x를0에서움직이지못하고 P도1로남는다. 유효한관측이있어도현재선형근사가그방향정보를못보는경우다. 잔차가남았는데필터가정지했다고좋은해로판정하지않는다.
실제식 h(x)=2를풀면 x=+sqrt3m와−sqrt3m가모두가능하다. 초기값0의선형화문제뿐아니라관측자체의좌우모호성도있다. 초기위치의근거·다른방향의관측이나추가상태제약으로모호성을줄여야한다. 단일가우시안의평균/공분산만으로두해를모두충실히표현했다고생각하지않는다. x⁻=0.5에서는H가0이아니지만그사실도전역정답/올바른쪽선택을보장하지않는다.
계산된 확신과 확인된 정확도를 따로 보고한다
기록에는 상태/관측성분과단위·프레임/시각·모델개정·초기평균/P·구간F/Q·관측H또는h/Jacobian·R의근거·혁신/S·수용/제외·갱신후값을남긴다. 실제입력계약은M14.02/03에서함께확인한다. EKF선형화·비선형모호성·공유정보·바이어스의한계도보고한다.
P가작아졌다는사실은가정한모델이계산한불확실성이줄었다는뜻이다. 독립정답과비교한실제오차가줄었거나센서보정·주행안전을검증했다는의미는아니다. 실제구현에서는행렬의차원/대칭/수치적안정성과양의준정부호도관리해야한다. 여기서는라이브러리를설치하거나실제센서를실행하지않았다. 학습 경로는 M06.05의 모바일 기구학으로 이어진다. 이후 M31.03의 모바일 작업 재현에서는 이런 모델/관측/실패 근거를 검토 가능한 작업 기록으로 연결한다.