The system to be estimated is defined as a discrete nonlinear dynamic dystem:
x(k) = f[x(k-1), u(k-1)] + v(k) ; x = Nx1, u = Mx1
y(k) = h[x(k)] + n(k) ; y = Zx1
Where:
x(k) : State Variable at time-k : Nx1
y(k) : Measured output at time-k : Zx1
u(k) : System input at time-k : Mx1
v(k) : Process noise, AWGN assumed, w/ covariance Qn : Nx1
n(k) : Measurement noise, AWGN assumed, w/ covariance Rn : Nx1
f(..), h(..) is a nonlinear transformation of the system to be estimated.
***************************************************************************************************
Unscented Kalman Filter algorithm:
Initialization:
P (k=0|k=0) = Identitas * covariant(P(k=0)), typically initialized with some big number.
x(k=0|k=0) = Expected value of x at time-0 (i.e. x(k=0)), typically set to zero.
Rv, Rn = Covariance matrices of process & measurement. As this implementation
the noise as AWGN (and same value for every variable), this is set
to Rv=diag(RvInit,...,RvInit) and Rn=diag(RnInit,...,RnInit).
Wc, Wm = First order & second order weight, respectively.
alpha, beta, kappa, gamma = scalar constants.
lambda = (alpha^2)*(N+kappa)-N, gamma = sqrt(N+alpha) ...{UKF_1}
Wm = [lambda/(N+lambda) 1/(2(N+lambda)) ... 1/(2(N+lambda))] ...{UKF_2}
Wc = [Wm(0)+(1-alpha(^2)+beta) 1/(2(N+lambda)) ... 1/(2(N+lambda))] ...{UKF_3}
UKF Calculation (every sampling time):
Calculate the Sigma Point:
Xs(k-1) = [x(k-1) ... x(k-1)] ; Xs(k-1) = NxN
GPsq = gamma * sqrt(P(k-1))
XSigma(k-1) = [x(k-1) Xs(k-1)+GPsq Xs(k-1)-GPsq] ...{UKF_4}
Unscented Transform XSigma [f,XSigma,u,Rv] -> [x,XSigma,P,DX]:
XSigma(k) = f(XSigma(k-1), u(k-1)) ...{UKF_5a}
x(k|k-1) = sum(Wm(i) * XSigma(k)(i)) ; i = 1 ... (2N+1) ...{UKF_6a}
DX = XSigma(k)(i) - Xs(k) ; Xs(k) = [x(k|k-1) ... x(k|k-1)]
; Xs(k) = Nx(2N+1) ...{UKF_7a}
P(k|k-1) = sum(Wc(i)*DX*DX') + Rv ; i = 1 ... (2N+1) ...{UKF_8a}
Unscented Transform YSigma [h,XSigma,u,Rn] -> [y_est,YSigma,Py,DY]:
YSigma(k) = h(XSigma(k), u(k|k-1)) ; u(k|k-1) = u(k) ...{UKF_5b}
y_est(k) = sum(Wm(i) * YSigma(k)(i)) ; i = 1 ... (2N+1) ...{UKF_6b}
DY = YSigma(k)(i) - Ys(k) ; Ys(k) = [y_est(k) ... y_est(k)]
; Ys(k) = Zx(2N+1) ...{UKF_7b}
Py(k) = sum(Wc(i)*DY*DY') + Rn ; i = 1 ... (2N+1) ...{UKF_8b}
Calculate Cross-Covariance otMatrix:
Pxy(k) = sum(Wc(i)*DX*DY(i)) ; i = 1 ... (2N+1) ...{UKF_9}
Calculate the Kalman Gain:
K = Pxy(k) * (Py(k)^-1) ...{UKF_10}
Update the Estimated State Variable:
x(k|k) = x(k|k-1) + K * (y(k) - y_est(k)) ...{UKF_11}
Update the Covariance otMatrix:
P(k|k) = P(k|k-1) - K*Py(k)*K' ...{UKF_12}
*Additional Information:
- Dengan asumsi masukan plant ZOH, u(k) = u(k|k-1),
Dengan asumsi tambahan observer dijalankan sebelum pengendali, u(k|k-1) = u(k-1),
sehingga u(k) [untuk perhitungan kalman] adalah nilai u(k-1) [dari pengendali].
- Notasi yang benar adalah u(k|k-1), tapi disini menggunakan notasi u(k) untuk
menyederhanakan penulisan rumus.
- Pada contoh di atas X~(k=0|k=0) = [0]. Untuk mempercepat konvergensi bisa digunakan
informasi plant-spesific. Misal pada implementasi Kalman Filter untuk sensor
IMU (Inertial measurement unit) dengan X = [quaternion], dengan asumsi IMU
awalnya menghadap ke atas tanpa rotasi, X~(k=0|k=0) = [1, 0, 0, 0]'