칼만 필터 (Kalman filter)는 잡음 이 포함되어 있는 측정치를 바탕으로 선형 역학계 의 상태를 추정하는 재귀 필터 로, 루돌프 칼만 이 개발하였다. 칼만 필터는 컴퓨터 비전 , 로봇 공학 , 레이다 등의 여러 분야에 사용된다. 칼만 필터는 과거에 수행한 측정값을 바탕으로 현재의 상태 변수의 결합분포를 추정한다.
알고리즘은 예측과 업데이트의 두 단계로 이루어진다. 예측 단계에서는 현재 상태 변수의 값과 정확도를 예측한다. 현재 상태 변수의 값이 실제로 측정된 이후, 업데이트 단계에서는 이전에 추정한 상태 변수를 기반으로 예측한 측정치와 실제 측정치의 차이를 반영해 현재의 상태 변수를 업데이트한다.
확장 칼만 필터는 비선형 시스템을 기반으로 동작한다.
칼만 필터는 물체의 측정값에 확률적인 오차가 포함되고, 또한 물체의 특정 시점에서의 상태가 이전 시점의 상태와 선형적인 관계를 가지고 있는 경우 적용이 가능하다. 예를 들어, 레이다 추적 의 경우 특정 물체의 위치, 속도, 가속도 등을 측정할 수 있지만 이 측정값에 오차가 포함되어 있을 수 있다. 이 경우, 연속적으로 측정하는 값들을 칼만 필터를 이용해서 해당 물체의 위치를 추정할 수 있다.
칼만 필터는 이산 시간 선형 동적 시스템을 기반으로 동작하며, 각 시간에서의 상태 벡터는 이전 시간의 상태 벡터를 통해서 결정된다는 마르코프 연쇄 를 가정하고 있다.
칼만 필터 모델. 사각형은 행렬을 나타낸다. 타원은 (행렬 내부의 평균 과 공분산 을 포함한) 다변량의 정규 분포를 나타낸다. 내부 변수의 값은 벡터이다.
특정 시간
k
{\displaystyle k}
에서의 상태 벡터를
x
k
{\displaystyle {\textbf {x}}_{k}}
라고 정의하고, 또한 그 시간에서의 사용자 입력을
u
k
{\displaystyle {\textbf {u}}_{k}}
라고 정의할 때, 칼만 필터에서는 다음과 같은 관계식을 가정하고 있다.
x
k
=
F
k
x
k
−
1
+
B
k
u
k
+
w
k
{\displaystyle {\textbf {x}}_{k}={\textbf {F}}_{k}{\textbf {x}}_{k-1}+{\textbf {B}}_{k}{\textbf {u}}_{k}+{\textbf {w}}_{k}}
여기에서
F
k
{\displaystyle {\textbf {F}}_{k}}
는 해당 시간에서 이전 상태에 기반한 상태 전이 행렬,
B
k
{\displaystyle {\textbf {B}}_{k}}
는 사용자 입력에 의한 상태 전이 행렬, 그리고
w
k
{\displaystyle {\textbf {w}}_{k}}
는 공분산행렬
Q
k
{\displaystyle {\textbf {Q}}_{k}}
을 가지는 다변량 정규분포
w
k
∼
N
(
0
,
Q
k
)
{\displaystyle {\textbf {w}}_{k}\sim N(0,{\textbf {Q}}_{k})}
잡음 변수이다.
또한, 상태 벡터
x
k
{\displaystyle {\textbf {x}}_{k}}
와 그 벡터를 측정했을 때 실제로 얻어진 벡터
z
k
{\displaystyle {\textbf {z}}_{k}}
는 다음과 같은 관계식을 가지고 있다.
z
k
=
H
k
x
k
+
v
k
{\displaystyle {\textbf {z}}_{k}={\textbf {H}}_{k}{\textbf {x}}_{k}+{\textbf {v}}_{k}}
여기에서
H
k
{\displaystyle {\textbf {H}}_{k}}
는 해당 시간의 상태에서 측정값을 도출하는 행렬이고,
v
k
{\displaystyle {\textbf {v}}_{k}}
는 공분산행렬
R
k
{\displaystyle {\textbf {R}}_{k}}
을 가지는 다변량 정규분포
v
k
∼
N
(
0
,
R
k
)
{\displaystyle {\textbf {v}}_{k}\sim N(0,{\textbf {R}}_{k})}
잡음 변수이다.
초기 상태와 각 잡음 변수
{
x
0
,
w
1
,
⋯
,
w
k
,
v
1
,
⋯
,
v
k
}
{\displaystyle \{{\textbf {x}}_{0},{\textbf {w}}_{1},\cdots ,{\textbf {w}}_{k},{\textbf {v}}_{1},\cdots ,{\textbf {v}}_{k}\}}
는 모두 상호 독립 이라는 가정이 필요하다.
많은 경우 실제 동적 시스템이 이 모델에 정확히 부합하지는 않는다. 특히, 선형성이나 상호 독립과 같은 중요한 가정이 맞지 않는 시스템의 경우, 칼만 필터의 성능을 심각하게 떨어뜨릴 수도 있고, 예측값이 발산하는 경우도 있다.
어떤 트럭이 마찰력이 없는 x축 공간에 있다. 이 트럭은 처음에는 원점 위치에 정지하고 있지만, 이후 임의로 가해지는 미지의 가속도를 받으면서 움직인다. 관찰자는
Δ
t
{\displaystyle \Delta t}
시간 간격으로 트럭의 위치를 측정하지만, 이 측정값은 정확하지 않고 실제 트럭의 위치와는 어느 정도 차이가 있을 수 있다. 이 때, 관찰자는 칼만 필터를 이용해서 부정확한 측정값을 기반으로 트럭의 실제 위치를 추정할 수 있다.
우선, 트럭의 움직임에 대한 관계식
x
k
=
F
k
x
k
−
1
+
B
k
u
k
+
w
k
{\displaystyle {\textbf {x}}_{k}={\textbf {F}}_{k}{\textbf {x}}_{k-1}+{\textbf {B}}_{k}{\textbf {u}}_{k}+{\textbf {w}}_{k}}
을 알맞게 이끌어내야 한다. 트럭의 상태를 나타내는 벡터
x
k
{\displaystyle \mathbf {x} _{k}}
는 그 트럭의 위치와 속도로 나타내면 적당하다. 즉, 해당 벡터를 다음과 같이 정의할 수 있다.
x
k
=
[
x
k
x
˙
k
]
{\displaystyle \mathbf {x} _{k}={\begin{bmatrix}x_{k}\\{\dot {x}}_{k}\end{bmatrix}}}
여기에서
x
k
{\displaystyle x_{k}}
는 시점 k에서의 위치,
x
˙
k
{\displaystyle {\dot {x}}_{k}}
는 시점 k에서의 속도를 의미한다.
a
k
{\displaystyle a_{k}}
는 트럭에 임의로 가해지는 미지의 가속도를 표시한다.
뉴턴의 운동 법칙 에 의해,
x
k
{\displaystyle \mathbf {x} _{k}}
와
x
k
−
1
{\displaystyle \mathbf {x} _{k-1}}
사이에는 다음과 같은 관계식이 근사적으로 성립한다.
x
k
=
x
k
−
1
+
x
˙
k
−
1
Δ
t
+
a
k
Δ
t
2
2
{\displaystyle {x}_{k}={x}_{k-1}+{\dot {x}}_{k-1}\Delta t+{a}_{k}{\frac {\Delta t^{2}}{2}}}
x
˙
k
=
x
˙
k
−
1
+
a
k
Δ
t
{\displaystyle {\dot {x}}_{k}={\dot {x}}_{k-1}+{a}_{k}\Delta t}
이것을
x
{\displaystyle {\textbf {x}}}
로 표현하면 다음과 같다.
x
k
=
F
x
k
−
1
+
w
k
{\displaystyle {\textbf {x}}_{k}={\textbf {F}}{\textbf {x}}_{k-1}+{\textbf {w}}_{k}}
w
k
=
B
a
k
{\displaystyle {\textbf {w}}_{k}={\textbf {B}}a_{k}}
F
=
[
1
Δ
t
0
1
]
{\displaystyle {\textbf {F}}={\begin{bmatrix}1&\Delta t\\0&1\end{bmatrix}}}
B
=
[
Δ
t
2
2
Δ
t
]
{\displaystyle {\textbf {B}}={\begin{bmatrix}{\frac {\Delta t^{2}}{2}}\\\Delta t\end{bmatrix}}}
이 모델의 경우
F
k
{\displaystyle {\textbf {F}}_{k}}
,
B
k
{\displaystyle {\textbf {B}}_{k}}
등의 값이 시간 k에 관계없이 일정하다. 이러한 상수의 경우 첨자를 생략하여 표기하였다.
여기에서
a
k
{\displaystyle a_{k}}
가 평균이 0이고 표준편차가
σ
a
{\displaystyle \sigma _{a}}
인 정규 분포 를 따른다고 가정하면, 잡음 변수
w
k
=
B
a
k
{\displaystyle \mathbf {w} _{k}=\mathbf {B} a_{k}}
는 평균이 0이고 공분산이
Q
=
cov
(
B
a
)
=
E
[
(
B
a
)
(
B
a
)
T
]
=
B
E
[
a
2
]
B
T
=
B
[
σ
a
2
]
B
T
=
σ
a
2
B
B
T
{\displaystyle {\textbf {Q}}={\textrm {cov}}({\textbf {B}}a)={\textrm {E}}[({\textbf {B}}a)({\textbf {B}}a)^{\text{T}}]={\textbf {B}}{\textrm {E}}[a^{2}]{\textbf {B}}^{\text{T}}={\textbf {B}}[\sigma _{a}^{2}]{\textbf {B}}^{\text{T}}=\sigma _{a}^{2}{\textbf {B}}{\textbf {B}}^{\text{T}}}
(
σ
a
{\displaystyle \sigma _{a}}
가 상수이므로 성립한다)
라는 것을 구할 수 있다. 이렇게 해서 트럭의 움직임에 대한 관계식을 유도했다.
그 다음에는 측정에 대한 관계식을 유도한다. 측정 잡음
v
k
{\displaystyle \mathbf {v} _{k}}
가 평균이 0이고 표준편차가
σ
z
{\displaystyle \sigma _{z}}
인 정규 분포를 따른다고 가정하면,
z
k
=
Hx
k
+
v
k
{\displaystyle {\textbf {z}}_{k}={\textbf {Hx}}_{k}+{\textbf {v}}_{k}}
H
=
[
1
0
]
{\displaystyle {\textbf {H}}={\begin{bmatrix}1&0\end{bmatrix}}}
R
=
E
[
v
k
v
k
T
]
=
[
σ
z
2
]
{\displaystyle {\textbf {R}}={\textrm {E}}[{\textbf {v}}_{k}{\textbf {v}}_{k}^{\text{T}}]={\begin{bmatrix}\sigma _{z}^{2}\end{bmatrix}}}
가 얻어진다.
트럭의 초기 위치와 속도는 0이라고 가정했으므로, 초기 변수를 다음과 같이 놓을 수 있다.
x
^
0
|
0
=
[
0
0
]
{\displaystyle {\hat {\textbf {x}}}_{0|0}={\begin{bmatrix}0\\0\end{bmatrix}}}
P
0
|
0
=
[
0
0
0
0
]
{\displaystyle {\textbf {P}}_{0|0}={\begin{bmatrix}0&0\\0&0\end{bmatrix}}}
만약에 트럭의 초기 상태를 알지 못한다면, 불확실성을 의미하는 공분산행렬
P
0
|
0
{\displaystyle {\textbf {P}}_{0|0}}
는 다음과 같이 적당히 큰 값으로 초기화하면 된다.
P
0
|
0
=
[
σ
x
2
0
0
σ
x
˙
2
]
{\displaystyle {\textbf {P}}_{0|0}={\begin{bmatrix}\sigma _{x}^{2}&0\\0&\sigma _{\dot {x}}^{2}\end{bmatrix}}}
불변 값인 잔차 공분산 P k |k 에서 시작한다.
P
k
|
k
=
cov
(
x
k
−
x
^
k
|
k
)
{\displaystyle {\textbf {P}}_{k|k}={\textrm {cov}}({\textbf {x}}_{k}-{\hat {\textbf {x}}}_{k|k})}
x
^
k
|
k
{\displaystyle {\hat {\textbf {x}}}_{k|k}}
의 정의를 대입하면,
P
k
|
k
=
cov
(
x
k
−
(
x
^
k
|
k
−
1
+
K
k
y
~
k
)
)
{\displaystyle {\textbf {P}}_{k|k}={\textrm {cov}}({\textbf {x}}_{k}-({\hat {\textbf {x}}}_{k|k-1}+{\textbf {K}}_{k}{\tilde {\textbf {y}}}_{k}))}
y
~
k
{\displaystyle {\tilde {\textbf {y}}}_{k}}
를 대입하면,
P
k
|
k
=
cov
(
x
k
−
(
x
^
k
|
k
−
1
+
K
k
(
z
k
−
H
k
x
^
k
|
k
−
1
)
)
)
{\displaystyle {\textbf {P}}_{k|k}={\textrm {cov}}({\textbf {x}}_{k}-({\hat {\textbf {x}}}_{k|k-1}+{\textbf {K}}_{k}({\textbf {z}}_{k}-{\textbf {H}}_{k}{\hat {\textbf {x}}}_{k|k-1})))}
z
k
{\displaystyle {\textbf {z}}_{k}}
를 대입하면,
P
k
|
k
=
cov
(
x
k
−
(
x
^
k
|
k
−
1
+
K
k
(
H
k
x
k
+
v
k
−
H
k
x
^
k
|
k
−
1
)
)
)
{\displaystyle {\textbf {P}}_{k|k}={\textrm {cov}}({\textbf {x}}_{k}-({\hat {\textbf {x}}}_{k|k-1}+{\textbf {K}}_{k}({\textbf {H}}_{k}{\textbf {x}}_{k}+{\textbf {v}}_{k}-{\textbf {H}}_{k}{\hat {\textbf {x}}}_{k|k-1})))}
잔차 벡터를 묶어내면,
P
k
|
k
=
cov
(
(
I
−
K
k
H
k
)
(
x
k
−
x
^
k
|
k
−
1
)
−
K
k
v
k
)
{\displaystyle {\textbf {P}}_{k|k}={\textrm {cov}}((I-{\textbf {K}}_{k}{\textbf {H}}_{k})({\textbf {x}}_{k}-{\hat {\textbf {x}}}_{k|k-1})-{\textbf {K}}_{k}{\textbf {v}}_{k})}
측정 에러 v k 는 다른 항과 독립이므로,
P
k
|
k
=
cov
(
(
I
−
K
k
H
k
)
(
x
k
−
x
^
k
|
k
−
1
)
)
+
cov
(
K
k
v
k
)
{\displaystyle {\textbf {P}}_{k|k}={\textrm {cov}}((I-{\textbf {K}}_{k}{\textbf {H}}_{k})({\textbf {x}}_{k}-{\hat {\textbf {x}}}_{k|k-1}))+{\textrm {cov}}({\textbf {K}}_{k}{\textbf {v}}_{k})}
벡터 공분산의 속성에 따라,
P
k
|
k
=
(
I
−
K
k
H
k
)
cov
(
x
k
−
x
^
k
|
k
−
1
)
(
I
−
K
k
H
k
)
T
+
K
k
cov
(
v
k
)
K
k
T
{\displaystyle {\textbf {P}}_{k|k}=(I-{\textbf {K}}_{k}{\textbf {H}}_{k}){\textrm {cov}}({\textbf {x}}_{k}-{\hat {\textbf {x}}}_{k|k-1})(I-{\textbf {K}}_{k}{\textbf {H}}_{k})^{\text{T}}+{\textbf {K}}_{k}{\textrm {cov}}({\textbf {v}}_{k}){\textbf {K}}_{k}^{\text{T}}}
이것은, 불변 값
P
k
|
k
−
1
{\displaystyle {\textbf {P}}_{k|k-1}}
와 R k 의 정의를 이용하면,
P
k
|
k
=
(
I
−
K
k
H
k
)
P
k
|
k
−
1
(
I
−
K
k
H
k
)
T
+
K
k
R
k
K
k
T
{\displaystyle {\textbf {P}}_{k|k}=(I-{\textbf {K}}_{k}{\textbf {H}}_{k}){\textbf {P}}_{k|k-1}(I-{\textbf {K}}_{k}{\textbf {H}}_{k})^{\text{T}}+{\textbf {K}}_{k}{\textbf {R}}_{k}{\textbf {K}}_{k}^{\text{T}}}
이 공식은 때로는 공분산 업데이트 방정식의 "조셉 형식(Joseph form) "이라 불리는데 K k 의 값이 최적 칼만이득인지 여부에 관계없이 성립한다. 만약 K k 가 최적 칼만이득이라면 이것을 아래처럼 더욱 단순화 할 수 있다.
칼만 필터는 최소 제곱 오차(minimum mean-square error, MMSE) 추정이다. 후부 상태 추정에서 잔차는 다음 식과 같다.
x
k
−
x
^
k
|
k
{\displaystyle {\textbf {x}}_{k}-{\hat {\textbf {x}}}_{k|k}}
이 벡터의 크기의 제곱의 기댓값
E
[
|
x
k
−
x
^
k
|
k
|
2
]
{\displaystyle {\textrm {E}}[|{\textbf {x}}_{k}-{\hat {\textbf {x}}}_{k|k}|^{2}]}
을 최소화하는
K
k
{\displaystyle {\textbf {K}}_{k}}
를 찾는 것이 목적이다. 이것은 후부 추정 공분산 행렬
P
k
|
k
{\displaystyle {\textbf {P}}_{k|k}}
의 대각합을 최소화하는 것과 같다. 위 방정식에서
P
k
|
k
{\displaystyle {\textbf {P}}_{k|k}}
를 전개하여 묶어내면
P
k
|
k
{\displaystyle {\textbf {P}}_{k|k}}
=
P
k
|
k
−
1
−
K
k
H
k
P
k
|
k
−
1
−
P
k
|
k
−
1
H
k
T
K
k
T
+
K
k
(
H
k
P
k
|
k
−
1
H
k
T
+
R
k
)
K
k
T
{\displaystyle ={\textbf {P}}_{k|k-1}-{\textbf {K}}_{k}{\textbf {H}}_{k}{\textbf {P}}_{k|k-1}-{\textbf {P}}_{k|k-1}{\textbf {H}}_{k}^{\text{T}}{\textbf {K}}_{k}^{\text{T}}+{\textbf {K}}_{k}({\textbf {H}}_{k}{\textbf {P}}_{k|k-1}{\textbf {H}}_{k}^{\text{T}}+{\textbf {R}}_{k}){\textbf {K}}_{k}^{\text{T}}}
=
P
k
|
k
−
1
−
K
k
H
k
P
k
|
k
−
1
−
P
k
|
k
−
1
H
k
T
K
k
T
+
K
k
S
k
K
k
T
{\displaystyle ={\textbf {P}}_{k|k-1}-{\textbf {K}}_{k}{\textbf {H}}_{k}{\textbf {P}}_{k|k-1}-{\textbf {P}}_{k|k-1}{\textbf {H}}_{k}^{\text{T}}{\textbf {K}}_{k}^{\text{T}}+{\textbf {K}}_{k}{\textbf {S}}_{k}{\textbf {K}}_{k}^{\text{T}}}
이 행렬의 대각합이 최소가 되는 조건은
∂
tr
(
P
k
|
k
)
∂
K
k
=
−
2
(
H
k
P
k
|
k
−
1
)
T
+
2
K
k
S
k
=
0
{\displaystyle {\frac {\partial \;{\textrm {tr}}({\textbf {P}}_{k|k})}{\partial \;{\textbf {K}}_{k}}}=-2({\textbf {H}}_{k}{\textbf {P}}_{k|k-1})^{\text{T}}+2{\textbf {K}}_{k}{\textbf {S}}_{k}=0}
최적의 칼만 이득
K
k
{\displaystyle {\textbf {K}}_{k}}
를 산출하면
K
k
S
k
=
(
H
k
P
k
|
k
−
1
)
T
=
P
k
|
k
−
1
H
k
T
{\displaystyle {\textbf {K}}_{k}{\textbf {S}}_{k}=({\textbf {H}}_{k}{\textbf {P}}_{k|k-1})^{\text{T}}={\textbf {P}}_{k|k-1}{\textbf {H}}_{k}^{\text{T}}}
K
k
=
P
k
|
k
−
1
H
k
T
S
k
−
1
{\displaystyle {\textbf {K}}_{k}={\textbf {P}}_{k|k-1}{\textbf {H}}_{k}^{\text{T}}{\textbf {S}}_{k}^{-1}}
최적의 칼만 이득 이라 불리는 이 이득은 MMSE 추정을 사용했을 때 산출되는 값이다.
칼만 이득이 위에서 이끌어 낸 최적의 값과 같을 때 후부 잔차 공분산을 단순화할 수 있도록 계산하기 위해 공식을 사용한다. S k K k T 을 칼만 이득 수식의 양쪽 모두에 곱하면,
K
k
S
k
K
k
T
=
P
k
|
k
−
1
H
k
T
K
k
T
{\displaystyle {\textbf {K}}_{k}{\textbf {S}}_{k}{\textbf {K}}_{k}^{T}={\textbf {P}}_{k|k-1}{\textbf {H}}_{k}^{T}{\textbf {K}}_{k}^{T}}
후부 잔차 공분산을 위한 확장된 공식을 다시 검토하면,
P
k
|
k
=
P
k
|
k
−
1
−
K
k
H
k
P
k
|
k
−
1
−
P
k
|
k
−
1
H
k
T
K
k
T
+
K
k
S
k
K
k
T
{\displaystyle {\textbf {P}}_{k|k}={\textbf {P}}_{k|k-1}-{\textbf {K}}_{k}{\textbf {H}}_{k}{\textbf {P}}_{k|k-1}-{\textbf {P}}_{k|k-1}{\textbf {H}}_{k}^{T}{\textbf {K}}_{k}^{T}+{\textbf {K}}_{k}{\textbf {S}}_{k}{\textbf {K}}_{k}^{T}}
맨 뒤의 두 항이 없어지는 것을 알 수 있고, 따라서
P
k
|
k
=
P
k
|
k
−
1
−
K
k
H
k
P
k
|
k
−
1
=
(
I
−
K
k
H
k
)
P
k
|
k
−
1
.
{\displaystyle {\textbf {P}}_{k|k}={\textbf {P}}_{k|k-1}-{\textbf {K}}_{k}{\textbf {H}}_{k}{\textbf {P}}_{k|k-1}=(I-{\textbf {K}}_{k}{\textbf {H}}_{k}){\textbf {P}}_{k|k-1}.}
이 공식은 간단하고 거의 항상 실제에 사용되지만, 최적의 이득일 때만 성립한다. 만약 수치 정밀도가 낮거나 최적이 아닌 칼만 이득을 사용할 경우 이 단순화는 성립하지 않는다. 그런 경우에는 위에서 유도한 후부 잔차 공분산 공식을 사용해야만 한다.
최적의 고정 지연 스무더는
z
1
{\displaystyle {\textbf {z}}_{1}}
에서
z
k
{\displaystyle {\textbf {z}}_{k}}
까지의 측정값을 이용하여 주어진 고정된 지연값 N을 통해 최적의
x
^
k
−
N
|
k
{\displaystyle {\hat {\textbf {x}}}_{k-N|k}}
추정을 제공한다. 이것은 위의 이론에서 상태 벡터를 확장 정의해 아래와 같이 유도할 수 있다.
[
x
^
t
|
t
x
^
t
−
1
|
t
⋮
x
^
t
−
N
+
1
|
t
]
=
[
I
0
⋮
0
]
x
^
t
|
t
−
1
+
[
0
…
0
I
0
⋮
⋮
⋱
⋮
0
…
I
]
[
x
^
t
−
1
|
t
−
1
x
^
t
−
2
|
t
−
1
⋮
x
^
t
−
N
|
t
−
1
]
+
[
K
(
1
)
K
(
2
)
⋮
K
(
N
)
]
y
t
|
t
−
1
{\displaystyle {\begin{bmatrix}{\hat {\textbf {x}}}_{t|t}\\{\hat {\textbf {x}}}_{t-1|t}\\\vdots \\{\hat {\textbf {x}}}_{t-N+1|t}\\\end{bmatrix}}={\begin{bmatrix}I\\0\\\vdots \\0\\\end{bmatrix}}{\hat {\textbf {x}}}_{t|t-1}+{\begin{bmatrix}0&\ldots &0\\I&0&\vdots \\\vdots &\ddots &\vdots \\0&\ldots &I\\\end{bmatrix}}{\begin{bmatrix}{\hat {\textbf {x}}}_{t-1|t-1}\\{\hat {\textbf {x}}}_{t-2|t-1}\\\vdots \\{\hat {\textbf {x}}}_{t-N|t-1}\\\end{bmatrix}}+{\begin{bmatrix}K^{(1)}\\K^{(2)}\\\vdots \\K^{(N)}\\\end{bmatrix}}y_{t|t-1}}
여기서:
1)
x
^
t
|
t
−
1
{\displaystyle {\hat {\textbf {x}}}_{t|t-1}}
은 표준 칼만 필터를 통하여 추정된다;
2)
y
t
|
t
−
1
=
z
(
t
)
−
x
^
t
|
t
−
1
{\displaystyle y_{t|t-1}=z(t)-{\hat {\textbf {x}}}_{t|t-1}}
은 표준 칼만 필터의 추정을 고려함으로써 생성된 혁신(잔차)이다.
3) 변수
x
^
t
−
i
|
t
{\displaystyle {\hat {\textbf {x}}}_{t-i|t}}
(
i
=
0
,
…
,
N
{\displaystyle i=0,\ldots ,N}
)는 새로운 변수이다. 즉 이것은 표준 칼만 필터에서는 나타나지 않는다;
4) 이득은 다음의 개요에 따라 계산된다:
K
(
i
)
=
P
(
i
)
H
T
[
H
P
H
T
+
R
]
−
1
{\displaystyle K^{(i)}=P^{(i)}H^{T}\left[HPH^{T}+R\right]^{-1}}
그리고
P
(
i
)
=
P
[
[
F
−
K
H
]
T
]
i
{\displaystyle P^{(i)}=P\left[\left[F-KH\right]^{T}\right]^{i}}
여기서
P
{\displaystyle P}
와
K
{\displaystyle K}
은 표준 칼만 필터의 예측 오차 공분산과 이득이다.
만약 추정 오차 공분산을 정의한다면 다음과 같다.
P
i
:=
E
[
(
x
t
−
i
−
x
^
t
−
i
|
t
)
∗
(
x
t
−
i
−
x
^
t
−
i
|
t
)
|
z
1
…
z
t
]
{\displaystyle P_{i}:=E\left[\left({\textbf {x}}_{t-i}-{\hat {\textbf {x}}}_{t-i|t}\right)^{*}\left({\textbf {x}}_{t-i}-{\hat {\textbf {x}}}_{t-i|t}\right)|z_{1}\ldots z_{t}\right]}
이경우 다음에 주어진
x
t
−
i
{\displaystyle {\textbf {x}}_{t-i}}
로 추정이 개선된다:
P
−
P
i
=
∑
j
=
0
i
[
P
(
j
)
H
T
[
H
P
H
T
+
R
]
−
1
H
(
P
(
i
)
)
T
]
{\displaystyle P-P_{i}=\sum _{j=0}^{i}\left[P^{(j)}H^{T}\left[HPH^{T}+R\right]^{-1}H\left(P^{(i)}\right)^{T}\right]}
칼만 필터에서는 기본적으로 모델의 선형성을 가정하고 있지만, 실제적으로는 많은 모델이 비선형 구조를 가지고 있다. 이런 경우 칼만 필터를 그대로 근사화해서 적용하면 그리 좋지 않은 결과를 얻는 경우가 많다. 이러한 경우, 칼만 필터를 수정해 비선형에도 사용할 수 있도록 한 필터가 사용된다.
확장 칼만 필터 (Extended Kalman Filter, EKF )에서는 칼만 필터에서의 선형성 가정을 완화시켜, 더 일반적인 시스템에 대해서도 사용이 가능하도록 확장했다. 이 필터는 내비게이션이나 GPS와 같은 비선형 상태 추정에 주로 사용되고 있다.
확장 칼만 필터에서는 모델의 선형성 가정 대신, 상태 변화 함수의 미분가능성을 가정한다.
x
k
=
f
(
x
k
−
1
,
u
k
−
1
)
+
w
k
−
1
{\displaystyle {\textbf {x}}_{k}=f({\textbf {x}}_{k-1},{\textbf {u}}_{k-1})+{\textbf {w}}_{k-1}}
z
k
=
h
(
x
k
)
+
v
k
{\displaystyle {\textbf {z}}_{k}=h({\textbf {x}}_{k})+{\textbf {v}}_{k}}
여기에서
f
{\displaystyle f}
,
h
{\displaystyle h}
는 미분가능한 함수여야 한다.
예측
예측된 상태
x
^
k
|
k
−
1
=
f
(
x
^
k
−
1
|
k
−
1
,
u
k
−
1
)
{\displaystyle {\hat {\textbf {x}}}_{k|k-1}=f({\hat {\textbf {x}}}_{k-1|k-1},{\textbf {u}}_{k-1})}
예측된 추정 공분산
P
k
|
k
−
1
=
F
k
−
1
P
k
−
1
|
k
−
1
F
k
−
1
⊤
+
Q
k
−
1
{\displaystyle {\textbf {P}}_{k|k-1}={\color {Red}{\textbf {F}}_{k-1}}{\textbf {P}}_{k-1|k-1}{\color {Red}{\textbf {F}}_{k-1}^{\top }}+{\textbf {Q}}_{k-1}}
업데이트
혁신 혹은 추정 나머지
y
~
k
=
z
k
−
h
(
x
^
k
|
k
−
1
)
{\displaystyle {\tilde {\textbf {y}}}_{k}={\textbf {z}}_{k}-h({\hat {\textbf {x}}}_{k|k-1})}
혁신(혹은 나머지) 공분산
S
k
=
H
k
P
k
|
k
−
1
H
k
⊤
+
R
k
{\displaystyle {\textbf {S}}_{k}={\color {Red}{\textbf {H}}_{k}}{\textbf {P}}_{k|k-1}{\color {Red}{\textbf {H}}_{k}^{\top }}+{\textbf {R}}_{k}}
최적의 칼만 이득
K
k
=
P
k
|
k
−
1
H
k
⊤
S
k
−
1
{\displaystyle {\textbf {K}}_{k}={\textbf {P}}_{k|k-1}{\color {Red}{\textbf {H}}_{k}^{\top }}{\textbf {S}}_{k}^{-1}}
업데이트된 상태 추정
x
^
k
|
k
=
x
^
k
|
k
−
1
+
K
k
y
~
k
{\displaystyle {\hat {\textbf {x}}}_{k|k}={\hat {\textbf {x}}}_{k|k-1}+{\textbf {K}}_{k}{\tilde {\textbf {y}}}_{k}}
업데이트된 추정 공분산
P
k
|
k
=
(
I
−
K
k
H
k
)
P
k
|
k
−
1
{\displaystyle {\textbf {P}}_{k|k}=(I-{\textbf {K}}_{k}{\color {Red}{\textbf {H}}_{k}}){\textbf {P}}_{k|k-1}}
여기서 상태 변이와 관측 매트릭스는 자코비안에 따라 정의 되었다.
F
k
−
1
=
∂
f
∂
x
|
x
^
k
−
1
|
k
−
1
,
u
k
{\displaystyle {\textbf {F}}_{k-1}=\left.{\frac {\partial f}{\partial {\textbf {x}}}}\right\vert _{{\hat {\textbf {x}}}_{k-1|k-1},{\textbf {u}}_{k}}}
H
k
=
∂
h
∂
x
|
x
^
k
|
k
−
1
{\displaystyle {\textbf {H}}_{k}=\left.{\frac {\partial h}{\partial {\textbf {x}}}}\right\vert _{{\hat {\textbf {x}}}_{k|k-1}}}
선형의 것과는 다르게, 일반적인 확장 칼만 필터는 최적의 추정이 아니다(물론 만약 측정과 상태 천이 모델이 둘 다 선형이라면 최적이다. 그 경우엔 확장 칼만 필터는 보통의 것과 같다.). 덧붙여 말해서, 만약 상태의 초기 추정이 틀리거나 프로세스가 옳지 않게 설계됐다면, 필터는 필터의 선형성으로 인해 금방 발산할 수도 있다. 확장 칼만 필터의 다른 문제는 추정된 공분산 매트릭스가 실제 공분산 매트릭스를 경시하도록 의도되었고, 결론적으로 "안정적인 노이즈"의 추가됨이 없는 통계적 의미에 모순되는 위험을 감수한다.
이러한 단점에도 불구하고, 확장 칼만 필터는 적당한 수행을 할 수 있고, 내비게이션이나 GPS에서 거의 사실상 표준이다.
상태 전이와 관찰 모델(예측과 업데이트 함수
f
{\displaystyle f}
와
h
{\displaystyle h}
)이 매우 비선형일 경우, 확장 칼만 필터는 일부 형편없는 성능을 나타낼 수 있다. 이것은 평균과 공분산이 비선형 모델의 선형화로 인해 전달되었기 때문이다. 무향 칼만 필터(The unscented Kalman filter, UKF)는 평균 주변에 (시그마 점(sigma point)으로 불리는) 샘플 포인트의 최소 집합을 얻기 위해,무향 변환으로 알려져 있는 결정론적인 샘플링 기술을 사용한다. 이 시그마 점들은 비선형 함수를 통해 전달되고, 변환된 점들에 대해 평균과 공분산을 구하는 형태를 가진다. 결과는 더 정확하게 평균과 공분산을 잡아내는 필터이다.(이것은 몬테 카를로 샘플링(Monte Carlo sampling)이나 예측 통계의 테일러 시리즈 확장을 통해 수정 될 수 있다.) 덧붙여 말해서, 이 기술은 명시적인 자코비안 계산을 요구하지 않는다. 이것은 복잡한 함수를 위한 것이라면 더 어려운 임무일 수 있다. (i.e., 복잡하게 유도된 요구는 분석적 하게 되거나 수치적으로 비용이 많이 드는 컴퓨터 계산이 된다.)
예측
EKF와 같이, UKF 예측은 UKF 업데이트(선형 또는 EKF 업데이트의 조합)와 독립적으로 사용될 수 있고, 그 반대도 가능하다.
추정된 상태와 공분산은 프로세스 잡음의 평균과 공분산으로 증대된다.
x
k
−
1
|
k
−
1
a
=
[
x
^
k
−
1
|
k
−
1
T
E
[
w
k
T
]
]
T
{\displaystyle {\textbf {x}}_{k-1|k-1}^{a}=[{\hat {\textbf {x}}}_{k-1|k-1}^{T}\quad E[{\textbf {w}}_{k}^{T}]\ ]^{T}}
P
k
−
1
|
k
−
1
a
=
[
P
k
−
1
|
k
−
1
0
0
Q
k
]
{\displaystyle {\textbf {P}}_{k-1|k-1}^{a}={\begin{bmatrix}&{\textbf {P}}_{k-1|k-1}&&0&\\&0&&{\textbf {Q}}_{k}&\end{bmatrix}}}
2L +1 시그마 포인트의 집합은 증대되는 상태와 공분산으로부터 전달된다. 여기서 L 은 증대되는 상태의 차원이다.
χ
k
−
1
|
k
−
1
0
{\displaystyle \chi _{k-1|k-1}^{0}}
=
x
k
−
1
|
k
−
1
a
{\displaystyle ={\textbf {x}}_{k-1|k-1}^{a}}
χ
k
−
1
|
k
−
1
i
{\displaystyle \chi _{k-1|k-1}^{i}}
=
x
k
−
1
|
k
−
1
a
+
(
(
L
+
λ
)
P
k
−
1
|
k
−
1
a
)
i
{\displaystyle ={\textbf {x}}_{k-1|k-1}^{a}+\left({\sqrt {(L+\lambda ){\textbf {P}}_{k-1|k-1}^{a}}}\right)_{i}}
i
=
1..
L
{\displaystyle i=1..L\,\!}
χ
k
−
1
|
k
−
1
i
{\displaystyle \chi _{k-1|k-1}^{i}}
=
x
k
−
1
|
k
−
1
a
−
(
(
L
+
λ
)
P
k
−
1
|
k
−
1
a
)
i
−
L
{\displaystyle ={\textbf {x}}_{k-1|k-1}^{a}-\left({\sqrt {(L+\lambda ){\textbf {P}}_{k-1|k-1}^{a}}}\right)_{i-L}}
i
=
L
+
1
,
…
2
L
{\displaystyle i=L+1,\dots {}2L\,\!}
여기서,
(
(
L
+
λ
)
P
k
−
1
|
k
−
1
a
)
i
{\displaystyle \left({\sqrt {(L+\lambda ){\textbf {P}}_{k-1|k-1}^{a}}}\right)_{i}}
은 다음 식의 매트릭스 스퀘어 루트의 i 번째 행이다.
(
L
+
λ
)
P
k
−
1
|
k
−
1
a
{\displaystyle (L+\lambda ){\textbf {P}}_{k-1|k-1}^{a}}
정의를 사용 : 매트릭스 B의 스퀘어루트 A를 푼다.
B
≡
A
A
T
{\displaystyle B\equiv AA^{T}}
.
매트릭스 스퀘어 루트는 수치적으로 효율적인 계산이어야 하고, 초레스키 분해(the Cholesky decomposition)와 같이 안정적인 메서드이어야 한다.
시그마 포인트는 전달함수 f 를 통해 전달된다.
χ
k
|
k
−
1
i
=
f
(
χ
k
−
1
|
k
−
1
i
)
i
=
0..2
L
{\displaystyle \chi _{k|k-1}^{i}=f(\chi _{k-1|k-1}^{i})\quad i=0..2L}
웨이티드 시그마 포인트는 예측된 상태와 공분산을 생산하기 위해 재결합 된다.
x
^
k
|
k
−
1
=
∑
i
=
0
2
L
W
s
i
χ
k
|
k
−
1
i
{\displaystyle {\hat {\textbf {x}}}_{k|k-1}=\sum _{i=0}^{2L}W_{s}^{i}\chi _{k|k-1}^{i}}
P
k
|
k
−
1
=
∑
i
=
0
2
L
W
c
i
[
χ
k
|
k
−
1
i
−
x
^
k
|
k
−
1
]
[
χ
k
|
k
−
1
i
−
x
^
k
|
k
−
1
]
T
{\displaystyle {\textbf {P}}_{k|k-1}=\sum _{i=0}^{2L}W_{c}^{i}\ [\chi _{k|k-1}^{i}-{\hat {\textbf {x}}}_{k|k-1}][\chi _{k|k-1}^{i}-{\hat {\textbf {x}}}_{k|k-1}]^{T}}
여기서 상태와 공분산을 위한 웨이트는 다음과 같이 주어진다:
W
s
0
=
λ
L
+
λ
{\displaystyle W_{s}^{0}={\frac {\lambda }{L+\lambda }}}
W
c
0
=
λ
L
+
λ
+
(
1
−
α
2
+
β
)
{\displaystyle W_{c}^{0}={\frac {\lambda }{L+\lambda }}+(1-\alpha ^{2}+\beta )}
W
s
i
=
W
c
i
=
1
2
(
L
+
λ
)
{\displaystyle W_{s}^{i}=W_{c}^{i}={\frac {1}{2(L+\lambda )}}}
λ
=
α
2
(
L
+
κ
)
−
L
{\displaystyle \lambda =\alpha ^{2}(L+\kappa )-L\,\!}
α
{\displaystyle \alpha }
,
β
{\displaystyle \beta }
,
κ
{\displaystyle \kappa }
를 위한 대표적인 수로는 각각
10
−
3
{\displaystyle 10^{-3}}
, 2, 0이다.(이들의 값은 최대한 목적을 만족시켜야 한다)[출처 필요 ]
업데이트
예측된 상태와 공분산은 전보다 증대되었다. 다만 노이즈 측정의 평균과 공분산은 제외된다.
x
k
|
k
−
1
a
=
[
x
^
k
|
k
−
1
T
E
[
v
k
T
]
]
T
{\displaystyle {\textbf {x}}_{k|k-1}^{a}=[{\hat {\textbf {x}}}_{k|k-1}^{T}\quad E[{\textbf {v}}_{k}^{T}]\ ]^{T}}
P
k
|
k
−
1
a
=
[
P
k
|
k
−
1
0
0
R
k
]
{\displaystyle {\textbf {P}}_{k|k-1}^{a}={\begin{bmatrix}&{\textbf {P}}_{k|k-1}&&0&\\&0&&{\textbf {R}}_{k}&\end{bmatrix}}}
예전과 같이, 2L +1 시그마 포인트의 집합은 증대된 상태와 공분산으로부터 전달된다. 여기서 L은 증대된 상태의 차원이다.
χ
k
|
k
−
1
0
{\displaystyle \chi _{k|k-1}^{0}}
=
x
k
|
k
−
1
a
{\displaystyle ={\textbf {x}}_{k|k-1}^{a}}
χ
k
|
k
−
1
i
{\displaystyle \chi _{k|k-1}^{i}}
=
x
k
|
k
−
1
a
+
(
(
L
+
λ
)
P
k
|
k
−
1
a
)
i
{\displaystyle ={\textbf {x}}_{k|k-1}^{a}+\left({\sqrt {(L+\lambda ){\textbf {P}}_{k|k-1}^{a}}}\right)_{i}}
i
=
1..
L
{\displaystyle i=1..L\,\!}
χ
k
|
k
−
1
i
{\displaystyle \chi _{k|k-1}^{i}}
=
x
k
|
k
−
1
a
−
(
(
L
+
λ
)
P
k
|
k
−
1
a
)
i
−
L
{\displaystyle ={\textbf {x}}_{k|k-1}^{a}-\left({\sqrt {(L+\lambda ){\textbf {P}}_{k|k-1}^{a}}}\right)_{i-L}}
i
=
L
+
1
,
…
2
L
{\displaystyle i=L+1,\dots {}2L\,\!}
이에 대한 대안으로, UKF 예측은 시그마 포인트를 사용하여 다음과 같이 증대될 수 있다.
χ
k
|
k
−
1
:=
[
χ
k
|
k
−
1
T
E
[
v
k
T
]
]
T
±
(
L
+
λ
)
R
k
a
{\displaystyle \chi _{k|k-1}:=[\chi _{k|k-1}^{T}\quad E[{\textbf {v}}_{k}^{T}]\ ]^{T}\pm {\sqrt {(L+\lambda ){\textbf {R}}_{k}^{a}}}}
여기서,
R
k
a
=
[
0
0
0
R
k
]
{\displaystyle {\textbf {R}}_{k}^{a}={\begin{bmatrix}&0&&0&\\&0&&{\textbf {R}}_{k}&\end{bmatrix}}}
시그마 포인트는 관측 함수 h 를 통하여 설계된다.
γ
k
i
=
h
(
χ
k
|
k
−
1
i
)
i
=
0..2
L
{\displaystyle \gamma _{k}^{i}=h(\chi _{k|k-1}^{i})\quad i=0..2L}
웨이티드 시그마 포인트는 예측된 측정과 예측된 측정 공분산을 생성하기 위해 재구성된다.
z
^
k
=
∑
i
=
0
2
L
W
s
i
γ
k
i
{\displaystyle {\hat {\textbf {z}}}_{k}=\sum _{i=0}^{2L}W_{s}^{i}\gamma _{k}^{i}}
P
z
k
z
k
=
∑
i
=
0
2
L
W
c
i
[
γ
k
i
−
z
^
k
]
[
γ
k
i
−
z
^
k
]
T
{\displaystyle {\textbf {P}}_{z_{k}z_{k}}=\sum _{i=0}^{2L}W_{c}^{i}\ [\gamma _{k}^{i}-{\hat {\textbf {z}}}_{k}][\gamma _{k}^{i}-{\hat {\textbf {z}}}_{k}]^{T}}
상태 측정 크로스-공분산 매트릭스,
P
x
k
z
k
=
∑
i
=
0
2
L
W
c
i
[
χ
k
|
k
−
1
i
−
x
^
k
|
k
−
1
]
[
γ
k
i
−
z
^
k
]
T
{\displaystyle {\textbf {P}}_{x_{k}z_{k}}=\sum _{i=0}^{2L}W_{c}^{i}\ [\chi _{k|k-1}^{i}-{\hat {\textbf {x}}}_{k|k-1}][\gamma _{k}^{i}-{\hat {\textbf {z}}}_{k}]^{T}}
는 UKF 칼만 필터 이득을 계산하기 위해 사용된다.
K
k
=
P
x
k
z
k
P
z
k
z
k
−
1
{\displaystyle K_{k}={\textbf {P}}_{x_{k}z_{k}}{\textbf {P}}_{z_{k}z_{k}}^{-1}}
칼만필터로서, 업데이트된 상태는 예측된 상태에, 칼만 필터에 의해서 웨이티드된 혁신 값을 더한다.
x
^
k
|
k
=
x
^
k
|
k
−
1
+
K
k
(
z
k
−
z
^
k
)
{\displaystyle {\hat {\textbf {x}}}_{k|k}={\hat {\textbf {x}}}_{k|k-1}+K_{k}({\textbf {z}}_{k}-{\hat {\textbf {z}}}_{k})}
그리고 업데이트된 공분산은 예측된 공분산에, 칼만 이득에 의해 웨이티드된 예측된 측정 공분산 값을 빼준다.
P
k
|
k
=
P
k
|
k
−
1
−
K
k
P
z
k
z
k
K
k
T
{\displaystyle {\textbf {P}}_{k|k}={\textbf {P}}_{k|k-1}-K_{k}{\textbf {P}}_{z_{k}z_{k}}K_{k}^{T}}