Foreword
In the previous article [Machine Learning] Derivation of Particle Filtering Principle (2), we have deduced and solved the principle and implementation process of particle filtering. Next, let’s take a look at the practical application of particle filtering in terms of cases.
Case
Suppose there is a car whose initial position is
x
,
y
=
(
0
,
0
)
x,y =(0,0)
x,y=(0,0), after 50 moments, according to our motion equation, assuming it can go to the point
x
,
y
=
(
0
,
65
)
x,y=(0,65)
The position of x,y=(0,65). That is, make a semicircular motion.
However, in real life, there are often various noises. The ground may be uneven or the wind resistance is large, so we introduce errors. The resulting picture looks like this.
It can be seen that at every moment, it is not as specified by the equation of motion. There is a certain error. Maybe it was a little faster at the previous moment, and maybe a little slower at the next moment.
Because of the error, we equipped this car with a GPS positioning and returned the car’s location information at every moment. However, GPS positioning is not accurate and has certain errors. The resulting picture looks like this
So how to use particle filtering to make its predictions relatively more reasonable.
We define the equations of motion, and for the x coordinate we use
z
1
z^1
Z1 means that a moment is assumed to be 1 second. The speed of the car is
2
m
/
s
2m/s
2m/s, angular velocity
3.6
°
/
s
3.6°/s
3.6°/s, so the abscissa position at time t is
z
t
1
=
z
t
?
1
1
+
v
?
cos
?
(
t
?
a
n
g
l
e
)
+
u
1
z^1_{t}=z^{1}_{t-1} + v*\cos(t*angle) + u_1
zt1?=zt?11? + v?cos(t?angle) + u1?
u
1
u_1
u1? is the random error, v is the velocity, angle is the angular velocity,
z
t
?
1
1
z^{1}_{t-1}
zt?11? is the abscissa position of the previous moment
For the y coordinate y, we use
z
2
z^2
z2 means
z
t
2
=
z
t
?
1
2
+
v
?
sin
?
(
t
?
a
n
g
l
e
)
+
u
2
z_t^{2}=z_{t-1}^{2} + v*\sin(t*angle) + u_2
zt2?=zt?12? + v?sin(t?angle) + u2?
u
2
u_2
u2? is the random error, everything else is the same
In the same way, for the x coordinate of GPS we use
x
1
x^1
x1 means
x
t
1
=
z
t
1
+
ω
1
x^1_t=z_t^1 + \omega_1
xt1?=zt1? + ω1?
ω
1
\omega_1
ω1? is the random error,
z
t
1
z_t^1
zt1? is the abscissa at time t in the equation of motion. In the same way to get the y coordinate, we use
x
2
x^2
x2 means
x
t
2
=
z
t
2
+
ω
2
x_t^2=z_t^2 + \omega_2
xt2?=zt2? + ω2?
Express the above into matrix form
Equations of motion
z
t
=
A
z
t
?
1
+
B
+
u
z_t=Az_{t-1} + B + u
zt?=Azt?1? + B + u
GPS observation equation
x
t
=
z
t
+
ω
x_t=z_t + \omega
xt?=zt? + ω
z
t
=
(
z
t
1
z
t
2
)
;
x
t
=
(
x
t
1
x
t
2
)
;
u
=
(
u
1
u
2
)
;
ω
=
(
ω
1
ω
2
)
z_t=\begin{pmatrix} z^1_t \ z^2_t \end{pmatrix}; x_t=\begin{pmatrix} x_t^1 \ x_t^2 \end{pmatrix}; u=\begin{pmatrix} u_1 \ u_2 \end{pmatrix}; \omega=\begin{pmatrix} \omega_1 \ \omega_2 \end{pmatrix}
zt?=(zt1?zt2);xt?=(xt1?xt2);u=(u1?u2);ω=(ω1?ω2)
For the coefficient
A
=
(
1
0
0
1
)
;
B
=
(
v
?
c
o
s
(
a
n
g
l
e
?
t
)
v
?
s
i
n
(
a
n
g
l
e
?
t
)
)
A=\begin{pmatrix} 1 & amp; 0 \ 0 & amp;1 \end{pmatrix};B=\begin{pmatrix} v*cos(angle*t)\ v *sin(angle*t) \end{pmatrix}
A=(10?01?);B=(v?cos(angle?t)v?sin(angle?t)?)
For simplicity, we take the random error term
u
,
ω
u,\omega
u,ω are assumed to obey the expectation as
0
0
0, the covariance matrices are respectively
Q
,
R
Q,R
Q,R.
Particle filtering process
When
t
=
1
t=1
When t=1
① Randomly initialize the particle swarm in a uniform distribution, and let the weight w at the current moment =
1
N
\frac{1}{N}
N1? (N is the number of sampling times).
When
t
≥
2
t\ge2
t≥2
②From
P
(
z
t
∣
z
t
?
1
)
P(z_t|z_{t-1})
The sampled particle swarm in P(zt?∣zt?1?) is expressed as
p
i
,
i
∈
1
,
2
,
?
?
,
N
p_{i},i\in1,2,\cdots,N
pi?,i∈1,2,?,N
③According to the formula
w
t
=
P
(
x
t
∣
z
t
)
w_t=P(x_t|z_t)
wt?=P(xt?∣zt?)The weight of the current moment. and normalized.
④ Resample to obtain a new particle swarm, and find the mean value of the particle swarm, which is the predicted value at the current moment.
⑤Iterate steps ②~④ to get all the means.
System resampling process
? ①Normalize the weights so that the sum of all weights is 1.
?② Calculate the cumulative function value of the weight, denoted as
c
c
c
? ③ Divide the (0, 1) interval into N parts (N is the number of resampling). Get n intervals and record all critical points as arrays
d
d
d
?④From (0,
1
N
\frac{1}{N}
N1?) A value is randomly sampled from a uniform distribution. Referred to as
a
a
a
?⑤Calculation
s
=
a
+
d
[
i
]
s=a + d[i]
s=a + d[i](
i
i
i is the index of an interval in array d), we iterate over all intervals in array d and obtain array s. and initialize j=0
? ⑥for i in N:
? if s[ i ] > c [ j ]: j + +
? Otherwise save index j
? ⑦ After iterating N times, obtain the values of the particles at the corresponding positions based on all indexes j. This is the sample after we resample.
Code implementation
Let’s take a look at the effect first. The left is the trajectory graph, and the right is the error graph. In the code, I set the variance of the motion equation to be smaller, but for GPS positioning, I set the variance to be larger, so for the trajectory graph of particle filtering, it is more accurate. is close to the equation of motion, and the error is relative to the equation of motion, so the error of particle filtering is smaller than that of writing GPS positioning.
import matplotlib.pyplot as plt
import numpy as np
from scipy import stats
plt.rcParams['font.sans-serif'] = ['SimHei']
plt.rcParams['axes.unicode_minus'] = False
fig,axes=plt.subplots(1,2) #Drawing, one row and two columns of subplots
np.random.seed(3) #Random number seed
class Particle_Filter():
def __init__(self):
pass
def train(self):
#Initialize weights and particle swarm
P=np.random.uniform(0,40,size=(2,N)) #Sampling from [0,40] uniform distribution
w=np.ones(shape=N)/N #Set the weights to 1/N
centers=np.zeros(shape=(2,T)) #The center point of the particle swarm, used to store the center point at each moment
for t in np.arange(1,T): #Start from the first moment
#B
B = np.array([[np.cos(angle_speed * t) * 2],
[np.sin(angle_speed * t) * 2]])
#Sample particles and update weights
for i in np.arange(w.shape[0]):
#Sample from P(z_t|z_t-1)
P[:,i] =stats.multivariate_normal.rvs(mean=A@P[:,i] + B.reshape(-1), cov=Q).reshape(-1)
#Use P(x_t|z_t) to calculate the weight, where z_t is the particle and x_t is the value obtained by GPS positioning
w[i]=stats.multivariate_normal.pdf(x=X[:,t],mean=P[:,i],cov=R)
#Normalized
w=w/np.sum(w)
#Resampling (system resampling)
cum=np.cumsum(w) #cumulative function
s=(np.random.uniform(0,1/N) + np.arange(0,1,1/N)) #Interval s
P_index=np.zeros(shape=N,dtype=int) #Initialize an array to store the corresponding index
index=0 #Start from the first item
for i in np.arange(N):
while s[i]>cum[index]:
index + =1
P_index[i]=index #storage index
P=P[:,P_index] #Get the particles corresponding to the index
center=P.mean(axis=1) #Find the mean
centers[:,t]=center #Save the results
plot_figure(Z, X,centers)
def plot_figure(x,y,centers): #Plotting function
axes[0].plot(x[0,:],x[1,:],label="equation of motion",linewidth=2,marker="o") #Draw the trajectory diagram of the equation of motion
axes[0].plot(y[0,:],y[1,:],label="GPS",linewidth=2,marker="o") #Draw GPS trajectory map
axes[0].plot(centers[0, :], centers[1, :], label="particle filter", linewidth=2, marker="o")#Particle filter trajectory chart
axes[0].set_title("track")
axes[0].set_xlabel("position x")
axes[0].set_ylabel("position y")
axes[0].legend()
err_X = np.linalg.norm(X - Z, axis=0) #Observation error
err_particle = np.linalg.norm(centers - Z, axis=0) #Particle filter error
axes[1].plot(np.arange(T), err_X, label="observation error", linewidth=2, marker="o")
axes[1].plot(np.arange(T), err_particle, label="Filter error", linewidth=2, marker="o")
axes[1].set_title("Error")
axes[1].set_xlabel("time t")
axes[1].set_ylabel("error")
axes[1].legend()
plt.show()
if __name__ == '__main__':
pi=np.pi
V=2 # speed 2m/s
T=50 # record 10 moments
angle_speed=pi/T #Angular speed
X=np.zeros(shape=(2,T)) # Observation variable (GPS positioning) 2 rows and 10 columns used to store data at each time
Z=np.zeros(shape=(2,T)) # Hidden variable (equation of motion) 2 rows and 10 columns used to store data at each time
Q=np.array([[0.1,0],
[0,0.1]]) #The process noise of the motion equation is assumed to obey the normal distribution with a mean value of 0 and a covariance matrix of Q.
R=np.array([[10,0],
[0,0.10]]) #GPS process noise, assuming that it obeys the normal distribution with a mean of 0 and a covariance matrix of R
mean = np.array([0, 0]) #Mean value of random error (process noise)
A=np.array([[1,0],
[0,1]]) #Coefficients of the equation of motion
N=1000 #The number of particles sampled each time
for t in np.arange(1,T):
B=np.array([[np.cos(angle_speed*t)*2],
[np.sin(angle_speed*t)*2]]) #Intercept term B
#Calculate the equation of motion at each moment and add the error term
Z[:,t]=A@Z[:,t-1] + B.reshape(-1) + stats.multivariate_normal.rvs(mean=mean,cov=Q)
#Calculate GPS position observation data at each moment
X[:,t]=Z[:,t] + stats.multivariate_normal.rvs(mean=mean,cov=R)
particle_filter=Particle_Filter() #Initialization
particle_filter.train() #training
End
In the final analysis, this problem is still a linear problem. In fact, it is no different from Kalman filtering, except that particle filtering is used to solve it. If there are any problems, please point them out. Thank you.
