Hi everybody. I'm very new in Kalman filter. Today I try to design Kalman filter to get estimated postion, velocity and acceleration from measurement position (by linear encoder). I don't use optimal input $u_k$ and my Kalman equations are:
$\displaystyle \quad\ \hat x_k^- = A\hat x_{k-1} $
$\displaystyle \quad\ P_k^- = AP_{k-1}A^T+Q $
$\displaystyle \quad\ K_k = P_k^-H^T(HP_k^-H^T+R)^{-1} $
$\displaystyle \quad\ \hat x_k = \hat x_k^- + K_k(z_k-H\hat x_k^-) $
$\displaystyle \quad\ P_k = (I-K_kH)P_k^- $
$A= \left[ \begin{array}{ccc} 1 & T_s & 0.5T_s^2 \\ 0 & 1 & T_s\\ 0 & 0 & 1\\ \end{array} \right]$, $Q= \left[ \begin{array}{ccc} 5.08*10^{-8} & 0 & 0\\ 0 & 0.00033 & 0\\ 0 & 0 & 0.48796\\ \end{array} \right]$
$R=Q_{11}=5.08*10^{-8}$
$T_s$ is sampling time. The results of estimated value and Kalman gain were showed in top. The estimated position, velocity and accleleration seem correctly. But about Kalman gain, why it still constant when position, velocity and acceleration change ($t = 1.8\rightarrow 2.3s $)?
Maybe I have mistake when assume acceleration $a_k = a_{k-1}$ (look at matrix $A$) because it change by time? I hear that extended Kalman filter can solve this problem. Can someone give me some advice for my system?
No comments:
Post a Comment