def kalman_estimate_1D(x,P,va_measurements,x_history):
"""
This function takes the filtered velocity, acceleration data and covariance matrix (from previous filtration),
the filtered position history (for only a single dimension) and the measurements (for velocity
and acceleration). And returns the updated velocity,acceleration data and covariance matrix
and also updates the position history.
It uses an Additive Unscented Kalman Filter and calls "kalman_estimate_v_a" in order to achieve this.
The functions 'lambda x,w' and 'lambda x,v' are respectively the transition and observation functions
(https://pykalman.github.io/#id3).
# measurements for 1 dim in the form [v,a] # x_history is history of in format [x1,x2,x3,...]
"""
(x,P)=kalman_estimate_v_a(x,P,va_measurements)
aukf = AdditiveUnscentedKalmanFilter(lambda x, w: x + w, lambda x, v: x + v, observation_covariance=1)
x_history=x_history[-30:] + x[0]*dt + 0.5 * x[1] * dt * dt
x_filtered,temp = aukf.filter(x_history)
return (x,P,x_filtered)
def kalman_estimate_1D(x, P, va_measurements, x_history):
"""
This function takes the filtered velocity, acceleration data and covariance matrix (from previous filtration),
the filtered position history (for only a single dimension) and the measurements (for velocity
and acceleration). And returns the updated velocity,acceleration data and covariance matrix
and also updates the position history.
It uses an Additive Unscented Kalman Filter and calls "kalman_estimate_v_a" in order to achieve this.
The functions 'lambda x,w' and 'lambda x,v' are respectively the transition and observation functions
(https://pykalman.github.io/#id3).
# measurements for 1 dim in the form [v,a] # x_history is history of in format [x1,x2,x3,...]
"""
(x, P) = kalman_estimate_v_a(x, P, va_measurements)
aukf = AdditiveUnscentedKalmanFilter(lambda x, w: x + w,
lambda x, v: x + v,
observation_covariance=1)
x_history = x_history[-30:] + x[0] * dt + 0.5 * x[1] * dt * dt
x_filtered, temp = aukf.filter(x_history)
return (x, P, x_filtered)
initial_state_covariance = [[1, 0.1], [0.1, 1]]
# sample from model
ukf = UnscentedKalmanFilter(transition_function,
observation_function,
transition_covariance,
observation_covariance,
initial_state_mean,
initial_state_covariance,
random_state=random_state)
akf = AdditiveUnscentedKalmanFilter(additive_transition_function,
additive_observation_function,
transition_covariance,
observation_covariance, initial_state_mean,
initial_state_covariance)
states, observations = ukf.sample(50, initial_state_mean)
# estimate state with filtering
ukf_state_estimates = ukf.filter(observations)[0]
akf_state_estimates = akf.filter(observations)[0]
# draw estimates
pl.figure()
lines_true = pl.plot(states, color='b')
lines_ukf = pl.plot(ukf_state_estimates, color='r')
lines_akf = pl.plot(akf_state_estimates, color='g')
pl.legend((lines_true[0], lines_ukf[0], lines_akf[0]),
('true', 'UKF', 'AddUKF'),
loc='upper left')
pl.show()
initial_state_covariance = [[1, 0.1], [-0.1, 1]]
# sample from model
ukf = UnscentedKalmanFilter(
transition_function, observation_function,
transition_covariance, observation_covariance,
initial_state_mean, initial_state_covariance,
random_state=random_state
)
akf = AdditiveUnscentedKalmanFilter(
additive_transition_function, additive_observation_function,
transition_covariance, observation_covariance,
initial_state_mean, initial_state_covariance
)
states, observations = ukf.sample(50, initial_state_mean)
# estimate state with filtering
ukf_state_estimates = ukf.filter(observations)[0]
akf_state_estimates = akf.filter(observations)[0]
# draw estimates
pl.figure()
lines_true = pl.plot(states, color='b')
lines_ukf = pl.plot(ukf_state_estimates, color='r')
lines_akf = pl.plot(akf_state_estimates, color='g')
pl.legend((lines_true[0], lines_ukf[0], lines_akf[0]),
('true', 'UKF', 'AddUKF'),
loc='upper left'
)
pl.show()
n = int(len(observations))
# init
stateMean = np.zeros((n+1, 2))
stateCov = np.zeros((n+1, 2, 2))
stateMean[0] = param['x0']
stateCov[0] = param['p0']
for i in range(n):
(stateMeanPred, stateCovPred) = filter_predict(f, jf, Q, stateMean[i], stateCov[i])
(stateMean[i+1], stateCov[i+1]) = filter_update(h, jh, np.array([param['r']]), stateMeanPred, stateCovPred, observations[i])
plt.plot(stateMean[:,0])
plt.plot(states[:,0])
plt.plot(observations, '.')
## Unscented Kalman Filter
# In[107]:
ukf = AdditiveUnscentedKalmanFilter(f, h, Q, param['r'], param['x0'], param['p0'])
stateMean = ukf.filter(observations)[0]
plt.plot(stateMean[:,0])
plt.plot(states[:,0])
plt.plot(observations, '.')
from __future__ import division
from pykalman import AdditiveUnscentedKalmanFilter
import numpy as np
import numpy.linalg as la
def AFK(y):
#Inputs: GPS coordinates; nx2 vector
#Outputs: smoothed states; nx2 vector
def TransObsFunc(state): #Same functions for simplified filter
return state
alpha = 0.8 #Tuning parameter for smoothness
akf = AdditiveUnscentedKalmanFilter(
TransObsFunc, TransObsFunc,
np.eye(2)*alpha, np.eye(2),
[y[0,0], y[0,1]], np.eye(2))
return akf.filter(y)[0]
from pykalman import AdditiveUnscentedKalmanFilter
'Initialize Parameters'
def transition_function(X):
...
def observation_function(X):
...
transition_covariance = np.eye(2)
observation_covariance = np.eye(2) + something
initial_state_mean = [0, 0]
initial_state_covariance = [[1, 0.1], [0.1, 1]]
akf = AdditiveUnscentedKalmanFilter(transition_function,observation_function, \
transition_covariance,observation_covariance,initial_state_mean, \
initial_state_covariance)
akf_state_estimates = akf.filter(timesteps, states)
pl.figure()
lines_true = pl.plot(states, color='b')
lines_akf = pl.plot(akf_state_estimates, color='g', ls='-.')
pl.show()
# init
stateMean = np.zeros((n + 1, 2))
stateCov = np.zeros((n + 1, 2, 2))
stateMean[0] = param['x0']
stateCov[0] = param['p0']
for i in range(n):
(stateMeanPred, stateCovPred) = filter_predict(f, jf, Q, stateMean[i],
stateCov[i])
(stateMean[i + 1],
stateCov[i + 1]) = filter_update(h, jh, np.array([param['r']]),
stateMeanPred, stateCovPred,
observations[i])
plt.plot(stateMean[:, 0])
plt.plot(states[:, 0])
plt.plot(observations, '.')
## Unscented Kalman Filter
# In[107]:
ukf = AdditiveUnscentedKalmanFilter(f, h, Q, param['r'], param['x0'],
param['p0'])
stateMean = ukf.filter(observations)[0]
plt.plot(stateMean[:, 0])
plt.plot(states[:, 0])
plt.plot(observations, '.')
