ó èÏOc@sšdZddlZdefd„ƒYZdefd„ƒYZdefd„ƒYZd efd „ƒYZd efd „ƒYZd efd„ƒYZ dS(sJ easyleed.kalman ---------------- Kalman filters for tracking the spots iÿÿÿÿNtAbstractKalmanFiltercBsVeZdZd„Zejdƒd„Zd d„Zd„Z d„Z d d„Z RS( sà Abstract implementation of a Kalman filter. Matrices and Vectors can be given in any input format np.matrix() understands. Vectors are internally transposed and should therefore be given as column vectors. cCswtt|ƒjƒtj|ƒj|_tj|ƒ|_tj|ƒ|_tjtj t |jj ƒƒƒ|_ dS(s‰ Initialize Kalman filter. x: start state vector P: start state covariance matrix H: measurement matrix N( tsuperRt__init__tnptasmatrixtTtxtPtHtidentitytmaxtshapet_1(tselfRRR((sM/home/salopaasi/Documents/Moo/easyleed_andy/easyleed/trunk/easyleed/kalman.pyRs icCsMtj|ƒ}tj|ƒ}||j|_||j|j||_dS(sf Predict next state. F: state transition matrix Q: process covariance matrix N(RRRRR(R tFtQ((sM/home/salopaasi/Documents/Moo/easyleed_andy/easyleed/trunk/easyleed/kalman.pytpredict!scCsD|dk r(|j|j|jj|S|j|j|jjSdS(s; Returns the covariance matrix of a predicted measurement. N(tNoneRRR(R tR((sM/home/salopaasi/Documents/Moo/easyleed_andy/easyleed/trunk/easyleed/kalman.pytpredict_measurement_covariance-s cCs|j|jS(s$ Returns the predicted measurement. (RR(R ((sM/home/salopaasi/Documents/Moo/easyleed_andy/easyleed/trunk/easyleed/kalman.pytpredict_measurement4scCs„tj|ƒj}tj|ƒ}|j|jj|j|ƒj}|j|||jƒ|_|j ||j|j|_dS(sh Update state estimate. z: measurement vector R: measurement covariance matrix N( RRRRRRtIRRR (R tzRtK((sM/home/salopaasi/Documents/Moo/easyleed_andy/easyleed/trunk/easyleed/kalman.pytupdate8s #cCsstj|ƒj}|dkr3tjddgƒn tj|ƒ}|jƒ}||}|j|j|ƒj|S(ss Returns the squared Mahalanobis distance of the given measurement. z: measurement vector iN( RRRRtdiagtasarrayRRR(R RRt z_predictedtresidual((sM/home/salopaasi/Documents/Moo/easyleed_andy/easyleed/trunk/easyleed/kalman.pytmeasurement_distanceFs 0  (iiN( t__name__t __module__t__doc__RRtzerosRRRRRR(((sM/home/salopaasi/Documents/Moo/easyleed_andy/easyleed/trunk/easyleed/kalman.pyR s    tAbstractPVKalmanFiltercBs/eZdZddd„Zd„Zd„ZRS(sN Kalman filter for 2d-tracking using position and velocity as state variables.ic Cs_||_||||g}ddddgddddgg}tt|ƒj|||ƒdS(Nii(told_timeRR"R( R tx_inty_inRttimetvx_intvy_inRR((sM/home/salopaasi/Documents/Moo/easyleed_andy/easyleed/trunk/easyleed/kalman.pyRTs $cCs$t|jdƒt|jdƒfS(Nii(tfloatR(R ((sM/home/salopaasi/Documents/Moo/easyleed_andy/easyleed/trunk/easyleed/kalman.pyt get_positionZscCs,t|jdƒdt|jdƒdfS(Nigà?i(ii(ii(R)R(R ((sM/home/salopaasi/Documents/Moo/easyleed_andy/easyleed/trunk/easyleed/kalman.pytget_position_err]s(RRR RR*R+(((sM/home/salopaasi/Documents/Moo/easyleed_andy/easyleed/trunk/easyleed/kalman.pyR"Rs tPVKalmanFilter0cBseZd„ZRS(cOsx||j}dd|dgddd|gddddgddddgg}tt|ƒj|||Ž||_dS(Nii(R#RR,R(R R&targstkwargstdtR((sM/home/salopaasi/Documents/Moo/easyleed_andy/easyleed/trunk/easyleed/kalman.pyRas B(RRR(((sM/home/salopaasi/Documents/Moo/easyleed_andy/easyleed/trunk/easyleed/kalman.pyR,`stPVKalmanFilter1cBseZd„ZRS(c Os©||j}d|j}d||}|d||d}dd|dgddd|gdd|dgddd|gg}tt|ƒj|||Ž||_dS(Ngø¿igà?ii(R#RR0R( R R&R-R.R/tatv_uptpos_upR((sM/home/salopaasi/Documents/Moo/easyleed_andy/easyleed/trunk/easyleed/kalman.pyRhs  B(RRR(((sM/home/salopaasi/Documents/Moo/easyleed_andy/easyleed/trunk/easyleed/kalman.pyR0gstPVKalmanFilter2cBseZd„ZRS(c OsÚ||j}d|j}d|jd}d||||d}|d||ddd||d}dd |d gd dd |gd d |d gd d d |gg} tt|ƒj| ||Ž||_dS( Ngø¿gþ?iigà?gð?g@ii(R#RR4R( R R&R-R.R/R1ta_dotR2R3R((sM/home/salopaasi/Documents/Moo/easyleed_andy/easyleed/trunk/easyleed/kalman.pyRrs  *B(RRR(((sM/home/salopaasi/Documents/Moo/easyleed_andy/easyleed/trunk/easyleed/kalman.pyR4qstPVKalmanFilter3cBseZd„ZRS(c Os ||j}d|j}d|jd}d|jd}d||||d||d}|d||ddd ||ddd ||d } dd | d gd dd | gd d |d gd d d |gg} tt|ƒj| ||Ž||_dS( Ngø¿gþ?ig€Àiigà?gð?g@g@ii(R#RR6R( R R&R-R.R/R1R5ta_ddotR2R3R((sM/home/salopaasi/Documents/Moo/easyleed_andy/easyleed/trunk/easyleed/kalman.pyR}s  &>B(RRR(((sM/home/salopaasi/Documents/Moo/easyleed_andy/easyleed/trunk/easyleed/kalman.pyR6|s( R tnumpyRtobjectRR"R,R0R4R6(((sM/home/salopaasi/Documents/Moo/easyleed_andy/easyleed/trunk/easyleed/kalman.pyts G