Kalman Filter-constant Velocity Model

Suppose you drive into the tunnel and the GPS signal is lost, now we need to determine the position of the car within the tunnel. The absolute speed of the car can be calculated from the wheel speed, the car facing can be through yaw rate sensor (a yaw-rate sensor is a gyroscopic device that measures A vehicle ' s angular vel Ocity around its vertical axis. ) so that the x-axis and y-axis velocity components can be obtained vx,vy

First determine the state variables, the constant velocity model to take the state variable is the car position and speed:

According to the Law of kinematics (the basic idea of any motion models is, a mass cannot move arbitrarily due to inertia):

Since the GPS signal is lost and the vehicle position cannot be measured directly, the observed model is:

The Kalman filter steps are as follows:

1 #-*-coding:utf-8-*-2 ImportNumPy as NP3 ImportMatplotlib.pyplot as Plt4 5 #Initial State x06x = Np.matrix ([[0.0, 0.0, 0.0, 0.0]]). T7 8 #Initial Uncertainty P09P = Np.diag ([1000.0, 1000.0, 1000.0, 1000.0])Ten  OneDT = 0.1#Time Step between Filter Steps A  - #Dynamic Matrix A -A = Np.matrix ([[1.0, 0.0, DT, 0.0], the[0.0, 1.0, 0.0, DT], -[0.0, 0.0, 1.0, 0.0], -[0.0, 0.0, 0.0, 1.0]]) -  + #Measurement Matrix - #We directly measure the velocity VX and VY +H = Np.matrix ([[0.0, 0.0, 1.0, 0.0], A[0.0, 0.0, 0.0, 1.0]]) at  - #Measurement Noise Covariance -RA = 10.0**2 -R = Np.matrix ([[Ra, 0.0], -[0.0, RA]]) -  in #Process Noise Covariance - #the Position of the car can be influenced by a force (e.g wind), which leads to #To an acceleration disturbance (noise). This process noise have to be modeled + #With the process noise covariance matrix Q. -SV = 8.8 theG = Np.matrix ([[0.5*dt**2], *[0.5*dt**2], $ [DT],Panax Notoginseng [DT]]) -Q = g*g.t*sv**2 the  +I = Np.eye (4) A  the #Measurement +m = 200#200 points of measurement -vx= 20#In X $vy= 10#In Y $mx = Np.array (vx+Np.random.randn (m)) -my = Np.array (vy+Np.random.randn (m)) -measurements =Np.vstack ((mx,my)) the  - #preallocation for plottingWuyiXT = [] theYT = [] -  Wu  - #Kalman Filter About  forNinchRange (len (measurements[0)): $      -     #Time Update (prediction) -     # ======================== -     #Project the state ahead Ax = A *x +      the     #Project the error covariance ahead -P = A*P*A.T +Q $  the      the     #Measurement Update (Correction) the     # =============================== the     #Compute the Kalman Gain -S = h*p*h.t +R inK = (p*h.t) *NP.LINALG.PINV (S) the  the     #Update The estimate via Z AboutZ = Measurements[:,n].reshape (2,1) they = Z-(h*x)#Innovation or residual thex = x + (k*y) the      +     #Update the error covariance -P = (I-(k*h)) *P the Bayi      the     #Save states for plotting the xt.append (float (x[0])) -Yt.append (Float (x[1])) -  the  the #State estimate:position (x, y) theFig = Plt.figure (figsize= (16,16)) thePlt.scatter (Xt,yt, s=20, label=' State', c='k') -Plt.scatter (Xt[0],yt[0], s=100, label='Start', c='g') thePlt.scatter (Xt[-1],yt[-1], s=100, label='Goal', c='R') the  thePlt.xlabel ('X')94Plt.ylabel ('Y') thePlt.title ('Position') thePlt.legend (loc=' Best') thePlt.axis ('Equal')98Plt.show ()

The estimated position of the car in the tunnel is as follows:

Kalman Filter-constant Velocity Model