The five kalman filter equations:
State Update Equation
x
^
n
,
n
=
x
^
n
,
n
−
1
+
α
(
z
n
−
x
^
n
,
n
−
1
)
\hat{x}_{n,n} = \hat{x}_{n,n-1} + \alpha(z_n-\hat{x}_{n,n-1})
x^n,n=x^n,n−1+α(zn−x^n,n−1)
State Extrapolation Equation
x
^
n
+
1
=
x
^
n
+
Δ
t
x
˙
^
n
x
˙
^
n
+
1
=
x
˙
^
n
\hat{x}_{n+1} = \hat{x}_n + \Delta t \hat{\dot{x}}_n\\ \hat{\dot{x}}_{n+1} = \hat{\dot{x}}_n
x^n+1=x^n+Δtx˙^nx˙^n+1=x˙^n
Kalman Gain
K
n
=
p
n
,
n
−
1
p
n
,
n
−
1
+
r
n
K_n =\frac{p_{n,n-1}}{p_{n,n-1} + r_n}
Kn=pn,n−1+rnpn,n−1
Covariance Update
p
n
,
n
=
(
1
−
K
n
)
p
n
,
n
−
1
p_{n,n} = (1 - K_n)p_{n,n-1}
pn,n=(1−Kn)pn,n−1
Covariance Extrapolation
p
n
+
1
,
n
=
p
n
,
n
p_{n+1,n} = p_{n,n}
pn+1,n=pn,n
to be continue…