Analyze and compare the performance of UKF and PF
Example of Nonlinear System
Suppose that we have a scalar system given by the following equations:
x k = 1 2 x k − 1 + 25 x k − 1 1 + x k − 1 2 + 8 cos [ 1.2 ( k − 1 ) ] + w k y k = 1 20 x k 2 + v k \begin{aligned} x_{k} &=\frac{1}{2} x_{k-1}+\frac{25 x_{k-1}}{1+x_{k-1}^{2}}+8 \cos [1.2(k-1)]+w_{k} \\ y_{k} &=\frac{1}{20} x_{k}^{2}+v_{k} \end{aligned} xkyk=21xk−1+1+xk−1225xk−1+8cos[1.2(k−1)]+wk=201xk2+vk
where { w k } \left\{w_{k}\right\} {
wk} are { v k } \left\{v_{k}\right\} {
vk} zero-mean Gaussian white noise sequences, both with variances equal to 1.
We take the initial state as x 0 = 0.1 x_{0} = 0.1 x0=0.1, the initial state estimate as x ^ 0 = x 0 \hat{x}_{0}= x_{0} x^0=x0 ,and the initial estimation covariance for the Kalman filter as P 0 + = 2 P_{0}^{+} = 2 P0+=2. We can simulate the EKF, the UKF and the particle filter to estimate the state x x x. We used a simulation length of 50 time steps, and 200 particles in the particle filter.
Conclusions
Particle filter shows greater performance than both the EKF and the UKF. And we its preformance gets better when the number of particles increase. UKF shows greater performance than the EKF.
Coding
import package
from __future__ import division
from filterpy.kalman import unscented_transform
from filterpy.kalman import UnscentedKalmanFilter as UKF
import numpy as np
from numpy.random import multivariate_normal
import matplotlib.pyplot as plt
import scipy as sp
import math
Initialization
x = 0.1 # initial state
Q = 1 # process noise covariance
R = 1 # measurement noise covariance
tf = 50 # simulation length
N = 200 # number of particles in the particle filter
#intial covariance
P = 2
sp.random.seed(3)
# true state, observation
xArr = [x] # true state
yArr = [x**2/20 + math.sqrt(R)*sp.random.normal()]
# Initialize the Extended Kalman Filter.
xhat = x
xhatArr = [xhat]
PArr = [P]
# Initialize the unscented kalman filter.
kappa = 0.
w = UKF.weights(2, kappa)
p_ukf = np.array([[1, 0],
[0, 1]])
ukf_mean = [x,x**2/20]
xhatukfArr = [x] # state estimates of the unscented Kalman Filter
# set the seed in order to duplicate the run exactly
# Initialize the particle filter.
xpart = x + math.sqrt(P)*sp.random.normal(size=N)
xhatPart = x
xhatPartArr = [xhatPart] # state estimates of the Particle Filter
for k in range(tf):
################################################
# System simulation
x = 0.5*x + 25*x/(1 + x*x) + 8*math.cos(1.2*k) + math.sqrt(Q)*sp.random.normal()
y = x**2/20 + math.sqrt(R)*sp.random.normal()
################################################
# Extended Kalman filter
F = 0.5 + 25*(1 - xhat**2)/(1 + xhat**2)**2
P = F*P*F + Q
H = xhat/10
K = P*H*(H*P*H + R)**(-1)
xhat = 0.5*xhat + 25*xhat/(1 + xhat**2) + 8*math.cos(1.2*(k))
xhat = xhat + K*(y - xhat**2/20)
P = (1 - K*H)*P
################################################
# Unscented Kalman filter
ukf_mean = ukf_mean
sigmas = UKF.sigma_points(ukf_mean,p_ukf, kappa)
sigmas_f = np.zeros((5, 2))
for i in range(5):
sigmas_f[i,0] = 0.5 * sigmas[i, 0] + 25 * sigmas[i, 0] / (1 + sigmas[i, 0]* sigmas[i, 0]) + 8 * math.cos(1.2 * k) + math.sqrt(Q) * sp.random.normal()
sigmas_f[i,1] = sigmas[i, 0] ** 2 / 20 + math.sqrt(R) * sp.random.normal()
ukf_mean, p_ukf = unscented_transform(sigmas_f, w, w, np.zeros((2, 2)))
################################################
# Particle filter
xpartminus = 0.5*xpart + 25*xpart/(1 + xpart**2) + 8*math.cos(1.2*(k)) + math.sqrt(Q)*sp.random.normal(size=N)
ypart = xpartminus**2/20
q=(1/math.sqrt(R)/math.sqrt(2*math.pi))*sp.exp(-(y-ypart)**2/2/R)
# Normalize the likelihood of each a priori estimate.
qsum = sp.sum(q)
q = [i/qsum for i in q]
# Resample.
for i in range(N):
u = sp.random.uniform() # uniform random number between 0 and 1
qtempsum = 0
for j in range(N):
qtempsum += q[j]
if qtempsum >= u:
xpart[i] = xpartminus[j]
break
# The particle filter estimate is the mean of the particles.
xhatPart = sp.mean(xpart)
# Save data
xArr.append(x) # true state
yArr.append(y) # observation
xhatArr.append(xhat) # state estimates of the Extended Kalman Filter
PArr.append(P)
xhatPartArr.append(xhatPart) # state estimates of the Particle Filter
xhatukfArr.append(ukf_mean[0])# state estimates of the unscented Kalman Filter
Compare different models
t = range(tf+1)
plt.figure()
#plt.grid()
plt.title('State')
plt.plot(t, xArr, 'b', label='True')
plt.plot(t, xhatArr, '^k:', label='EKF')
plt.plot(t, xhatPartArr, 'or-', label='PF')
plt.plot(t, xhatukfArr, '.g-.',label='UKF')
plt.legend(loc='best')
plt.show()
What if we only use 5 particles?
Example of linear System
Suppose that we have a scalar system given by the following equations:
x k = x k − 1 + w k y k = x k + v k \begin{aligned} x_{k} &= x_{k-1}+w_{k} \\ y_{k} &=x_{k}+v_{k} \end{aligned} xkyk=xk−1+wk=xk+vk
where { w k } \left\{w_{k}\right\} {
wk} are { v k } \left\{v_{k}\right\} {
vk} zero-mean Gaussian white noise sequences, both with variances equal to 1.
We take the initial state as x 0 = 0.1 x_{0} = 0.1 x0=0.1, the initial state estimate as x ^ 0 = x 0 \hat{x}_{0}= x_{0} x^0=x0 ,and the initial estimation covariance for the Kalman filter as P 0 + = 2 P_{0}^{+} = 2 P0+=2. We can simulate the EKF, the UKF and the particle filter to estimate the state x x x. We used a simulation length of 50 time steps, and 200 particles in the particle filter.
Conclusion
Particle filter shows greater performance than the UKF.
Coding
Initialization
x = 0.1 # initial state
Q = 0.01 # process noise covariance
R = 1e-5 # measurement noise covariance
tf = 100 # simulation length
N = 200 # number of particles in the particle filter
#initital state
xhat = x
P = 2 #intial covariance
sp.random.seed(3)
# true process.
xArr = [x] # true state
yArr = [x + math.sqrt(R)*sp.random.normal()] # observation
# state estimates of the Kalman Filter
xhatArr = [x]
PArr = [P]
#? Initialize the unscented kalman filter.
kappa = 0.
w = UKF.weights(2, kappa)
p_ukf = np.array([[1, 0],
[0, 1]])
xhatukfArr = [x] # state estimates of the unscented Kalman Filter
# Initialize the particle filter.
# set the seed in order to duplicate the run exactly
xpart = x + math.sqrt(P)*sp.random.normal(size=N)
xhatPart = x
xhatPartArr = [xhatPart] # state estimates of the Particle Filter
for k in range(tf):
################################################
# System simulation
y = x + math.sqrt(R)*sp.random.normal() #generate observed value
### compute mean via KF
xhat = xhat
P = P + Q
# measurement update
K = P / (P + R)
xhat = xhat + K * (y - xhat)
P = (1 - K) * P
################################################
###Unscented Kalman filter
ukf_mean = [x,x]
sigmas = UKF.sigma_points(ukf_mean,p_ukf, kappa)
sigmas_f = np.zeros((5, 2))
for i in range(5):
sigmas_f[i,0] = sigmas[i, 0] + math.sqrt(Q) * sp.random.normal()
sigmas_f[i,1] = sigmas[i, 0] + math.sqrt(R) * sp.random.normal()
ukf_mean, p_ukf = unscented_transform(sigmas_f, w, w, np.zeros((2, 2)))
################################################
# Particle filter
xpartminus = xpart + math.sqrt(Q)*sp.random.normal(size=N)
ypart = xpartminus
q=(1/math.sqrt(R)/math.sqrt(2*math.pi))*sp.exp(-(y-ypart)**2/2/R)
# Normalize the likelihood of each a priori estimate.
qsum = sp.sum(q)
q = [i/qsum for i in q]
# Resample.
for i in range(N):
u = sp.random.uniform() # uniform random number between 0 and 1
qtempsum = 0
for j in range(N):
qtempsum += q[j]
if qtempsum >= u:
xpart[i] = xpartminus[j]
break
# The particle filter estimate is the mean of the particles.
xhatPart = sp.mean(xpart)
# Save data
xArr.append(x) # true state
yArr.append(y) # observation
xhatArr.append(xhat) # state estimates of the Kalman Filter
PArr.append(P)
xhatPartArr.append(xhatPart) # state estimates of the Particle Filter
xhatukfArr.append(ukf_mean[0])# state estimates of the unscented Kalman Filter
Compare different models
t = range(tf+1)
plt.figure()
#plt.grid()
plt.title('State')
plt.plot(t, xArr, 'b', label='True')
plt.plot(t, xhatArr, '^k:', label='KF')
plt.plot(t, xhatPartArr, 'or-', label='PF')
plt.plot(t, xhatukfArr, '.g-.',label='UKF')
plt.legend(loc='best')
plt.show()
