Files | |
file | otUKF.h |
Class for Discrete Unscented Kalman Filter. | |
Modules | |
Version History | |
Data Structures | |
class | otUKF |
Unscented Kalman Filter. More... | |
class | otUKF |
Unscented Kalman Filter. More... |
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]'