Algorithm 3: Event based KF, Ackermann case

Input : uk,zk,xk−1,Pk−1,LGPS

Output: x̂k,Pk

Data: A, B and H from model (5), Qk,Rk,ANXT,Qx,Hp,Rk,GM,Pk,p

Initialization: x0,P0

for current time k do

Prediction Step KF:

x̂k = Ax̂k−1+Buk−1

Pk = APk−1AT+Qk

Correction Step KF:

$K_k=P_k{H}_k^T{(H_k{P}_k{H}_k^T+R_k)}^{-1}$

x̂k = x̂k + Kk[zk−Hx̂k]

Pk = (I−Kk HK)Pk

Pose estimation using (2)

Covariance propagation using (14)

3 −σ ellipsoid area Aellip using (12)

RA = Aellip/ANXT

if RA > RA,lim And Nsat > Nsat,min then

Global Correction Step, pose KF

$K_{k,KF}=P_{k,p}{H}_p^T{(H_p{P}_{k,p}{H}_p^T+R_{k,GM})}^{-1}$

x̂k,p = x̂k−1,p +Kk,KF(LGPS−Hp x̂k−1,p)

Pk,p = (I−Kk,KF H) Pk,p

end

end