def run_filter(list_args: List[float],
data: pd.DataFrame) -> Tuple[KalmanFilter, Saver]:
if isinstance(data, tuple):
# data is the first *args
data = data[0]
# store the parameters in the list_args
s_noise_Q00, r_noise_Q11, q_meas_error_R00, r_meas_error_R11 = list_args
kf = init_filter(
r0=0.0,
S0=5.74,
s_variance_P00=1,
r_variance_P11=100,
s_noise_Q00=s_noise_Q00,
r_noise_Q11=r_noise_Q11,
q_meas_error_R00=q_meas_error_R00,
r_meas_error_R11=r_meas_error_R11,
)
s = Saver(kf)
lls = []
for z in np.vstack([data["q_obs"], data["r_obs"]]).T:
kf.predict()
kf.update(z)
log_likelihood = kf.log_likelihood_of(z)
lls.append(log_likelihood)
s.save()
s.to_array()
lls = np.array(lls)
return kf, s, lls
def test_functions():
x, P = predict(x=10., P=3., u=1., Q=2.**2)
x, P = update(x=x, P=P, z=12., R=3.5**2)
x, P = predict(x=np.array([10.]), P=np.array([[3.]]), Q=2.**2)
x, P = update(x=x, P=P, z=12., H=np.array([[1.]]), R=np.array([[3.5**2]]))
x = np.array([1., 0])
P = np.diag([1., 1])
Q = np.diag([0., 0])
H = np.array([[1., 0]])
x, P = predict(x=x, P=P, Q=Q)
assert x.shape == (2, )
assert P.shape == (2, 2)
x, P = update(x, P, z=[1], R=np.array([[1.]]), H=H)
assert x[0] == 1 and x[1] == 0
# test velocity predictions
x, P = predict(x=x, P=P, Q=Q)
assert x[0] == 1 and x[1] == 0
x[1] = 1.
F = np.array([[1., 1], [0, 1]])
x, P = predict(x=x, F=F, P=P, Q=Q)
assert x[0] == 2 and x[1] == 1
x, P = predict(x=x, F=F, P=P, Q=Q)
assert x[0] == 3 and x[1] == 1
def test_functions():
x, P = predict(x=10., P=3., u=1., Q=2.**2)
x, P = update(x=x, P=P, z=12., R=3.5**2)
x, P = predict(x=np.array([10.]), P=np.array([[3.]]), Q=2.**2)
x, P = update(x=x, P=P, z=12., H=np.array([[1.]]), R=np.array([[3.5**2]]))
x = np.array([1., 0])
P = np.diag([1., 1])
Q = np.diag([0., 0])
H = np.array([[1., 0]])
x, P = predict(x=x, P=P, Q=Q)
assert x.shape == (2,)
assert P.shape == (2, 2)
x, P = update(x, P, z=[1], R=np.array([[1.]]), H=H)
assert x[0] == 1 and x[1] == 0
# test velocity predictions
x, P = predict(x=x, P=P, Q=Q)
assert x[0] == 1 and x[1] == 0
x[1] = 1.
F = np.array([[1., 1], [0, 1]])
x, P = predict(x=x, F=F, P=P, Q=Q)
assert x[0] == 2 and x[1] == 1
x, P = predict(x=x, F=F, P=P, Q=Q)
assert x[0] == 3 and x[1] == 1
def get_predictions(self):
"""
:description
Starting from the current position, make predictions and propagate using the Kalman transition matrix (defined
by our model) to make predictions on futur positions.
:return:
([x1, y1], [x2, y2], ...) predictions -- a list of size NUMBER_PREDICTIONS containing the
predicted positions of the target beeing tracked
"""
predictions = []
# only return if still seeing the object
if (not len(self.kalman_memory) < 2) and (
self.kalman_memory[-2][2] - self.kalman_memory[-1][2] > 3):
return predictions
current_state = self.filter.x
current_P = self.filter.P
dt = 0.4
try:
for _ in range(NUMBER_PREDICTIONS):
new_state, new_P = predict(current_state, current_P,
self.filter_model.model_F(dt),
self.filter.Q)
predictions.append((new_state, new_P))
current_state, current_P = update(
new_state, new_P,
new_state[0:KALMAN_MODEL_MEASUREMENT_DIM], self.filter.R,
self.filter.H)
except ValueError:
print("error in prediction")
import sys
print(sys.exc_info())
print("current_state: ", current_state)
return predictions
def update_mixture(xs, Ps, ws, z, R, H, Pd, lamc):
'''
xs - list of state
Ps - list of state covariance matrices
ws - list of weights
z - list of measurments
R - measurement noise matrix
H - measurement matrix
Pd - probability of detection
lamc - clutter intensity function handle
'''
hypotheses = range(0, len(z) + 1)
ws_u, xs_u, Ps_u = [],[],[]
for x_prev, P_prev, w_prev in zip(xs, Ps, ws):
for h in hypotheses:
if h == 0:
weight = w_prev * (1 - Pd)
ws_u.append(weight)
xs_u.append(x_prev)
Ps_u.append(P_prev)
else:
zt = z[h - 1]
weight = w_prev * Pd * predict_measurement(x_prev, P_prev, zt, R, H) / lamc(zt)
x_u, P_u = update(x_prev, P_prev, zt, R, H)
ws_u.append(weight)
xs_u.append(x_u)
Ps_u.append(P_u)
# normalize weights
ws_u = ws_u / np.sum(ws_u)
return xs_u, Ps_u, ws_u
def get_current_rssi(self):
if len(self.rssihistory) == 0:
return -256, -256
self.x, self.P = kf.predict(x=self.x, P=self.P, u=0, Q=2) # rely more on the prediction by using a high Q variance
self.x, self.P = kf.update(x=self.x, P=self.P, z=self.rssihistory[-1], R=1) # measurement variance could be up to 1 per RSSI read
estimated_rssi = self.x
#print self.timehistory[-1], estimated_rssi, self.rssihistory[-1]
return estimated_rssi, self.rssihistory[-1]
def test_procedure_form():
dt = 1.
std_z = 10.1
x = np.array([[0.], [0.]])
F = np.array([[1., dt], [0., 1.]])
H = np.array([[1.,0.]])
P = np.eye(2)
R = np.eye(1)*std_z**2
Q = Q_discrete_white_noise(2, dt, 5.1)
kf = KalmanFilter(2, 1)
kf.x = x.copy()
kf.F = F.copy()
kf.H = H.copy()
kf.P = P.copy()
kf.R = R.copy()
kf.Q = Q.copy()
measurements = []
results = []
xest = []
ks = []
pos = 0.
for t in range (2000):
z = pos + random.randn() * std_z
pos += 100
# perform kalman filtering
x, P = predict(x, P, F, Q)
kf.predict()
assert norm(x - kf.x) < 1.e-12
x, P, _, _, _, _ = update(x, P, z, R, H, True)
kf.update(z)
assert norm(x - kf.x) < 1.e-12
# save data
xest.append (x.copy())
measurements.append(z)
xest = np.asarray(xest)
measurements = np.asarray(measurements)
# plot data
if DO_PLOT:
plt.plot(xest[:, 0])
plt.plot(xest[:, 1])
plt.plot(measurements)
def update(self, pos, vel):
"""Update object position and filter
Inputs:
pos -- position of object (cartesian)
vel -- veloicty of object (cartesian)
# hist_score -- certainty score based on object history (used as scale factor for measurement covariance) (range 1 - 1.05)
"""
measurement = np.append(pos, vel).astype(np.float32)
self.state, self.covar = kalman.update(x=self.state,
P=self.covar,
z=measurement,
R=self.measurement_covar,
H=self.measurement_trans)
def test_procedure_form():
dt = 1.
std_z = 10.1
x = np.array([[0.], [0.]])
F = np.array([[1., dt], [0., 1.]])
H = np.array([[1., 0.]])
P = np.eye(2)
R = np.eye(1) * std_z**2
Q = Q_discrete_white_noise(2, dt, 5.1)
kf = KalmanFilter(2, 1)
kf.x = x.copy()
kf.F = F.copy()
kf.H = H.copy()
kf.P = P.copy()
kf.R = R.copy()
kf.Q = Q.copy()
measurements = []
results = []
xest = []
ks = []
pos = 0.
for t in range(2000):
z = pos + random.randn() * std_z
pos += 100
# perform kalman filtering
x, P = predict(x, P, F, Q)
kf.predict()
assert norm(x - kf.x) < 1.e-12
x, P, _, _, _, _ = update(x, P, z, R, H, True)
kf.update(z)
assert norm(x - kf.x) < 1.e-12
# save data
xest.append(x.copy())
measurements.append(z)
xest = np.asarray(xest)
measurements = np.asarray(measurements)
# plot data
if DO_PLOT:
plt.plot(xest[:, 0])
plt.plot(xest[:, 1])
plt.plot(measurements)
def kalman_filter(data, sensor_var, process_var):
x0 = (data[0], 1)
velocity = 0
dt = 1.
sensor_var = sensor_var
process_var = process_var
process_model = (velocity*dt, process_var)
x = x0
xs, predictions = [], []
for z in data:
prior = predict(x, process_model)
#print(prior)
likelihood = (z, sensor_var)
x = update(prior, likelihood)
# save results
predictions.append(prior[0])
xs.append(x[0])
return predictions, xs
def proc_form():
""" This is for me to run against the class_form() function to see which,
if either, runs faster. They within a few ms of each other on my machine
with Python 3.5.1"""
dt = 1.
std_z = 10.1
x = np.array([[0.], [0.]])
F = np.array([[1., dt], [0., 1.]])
H = np.array([[1., 0.]])
P = np.eye(2)
R = np.eye(1)*std_z**2
Q = Q_discrete_white_noise(2, dt, 5.1)
pos = 0.
for t in range(2000):
z = pos + random.randn() * std_z
pos += 100
# perform kalman filtering
x, P = predict(x, P, F, Q)
x, P, _ = update(z, R, x, P, H)
def proc_form():
""" This is for me to run against the class_form() function to see which,
if either, runs faster. They within a few ms of each other on my machine
with Python 3.5.1"""
dt = 1.
std_z = 10.1
x = np.array([[0.], [0.]])
F = np.array([[1., dt], [0., 1.]])
H = np.array([[1., 0.]])
P = np.eye(2)
R = np.eye(1) * std_z**2
Q = Q_discrete_white_noise(2, dt, 5.1)
pos = 0.
for t in range(2000):
z = pos + random.randn() * std_z
pos += 100
# perform kalman filtering
x, P = predict(x, P, F, Q)
x, P, _ = update(z, R, x, P, H)
def kalman_filter(z):
global x
global process_model
# -------- PREDICT --------- #
# X is the state of the system
# P is the variance of the system
# u is the movement of the system due to the process
# Q is the noise of the process
x, P = kf.predict(x=x, P=process_model)
# sensor says z with a standard deviation of sensorvar**2
# probability of the measurement given the current state
#likelihood = gaussian(z, mean(z), sensor_var)
# -------- UPDATE --------- #
# X is the state of the system
# P is the variance of the system
# z is the measurement
# R is the measurement variance
x, P = kf.update(x=x, P=P, z=z, R=sensor_var)
# print( "x: ",'%.3f' % x, "var: " '%.3f' % P, "z: ", '%.3f' % z)
#print(x)
return x
V = np.zeros(shape=(25, 2))
Z = np.zeros(shape=(25, 2))
Q = np.array([[0.0001, 0.00002], [0.00002, 0.0001]])
R = np.array([[0.01, 0.005], [0.005, 0.02]])
x = x2 = np.array([[0.,0.]]).T
H = B = P = np.eye(2)
# P = 0.0001*P #use this when estimate case 2
with open('data.csv', newline='') as csvfile:
reader = csv.reader(csvfile, delimiter=' ')
for i, row in enumerate(reader):
a = row[0].split(',')
V[i] = [float(a[0]), float(a[1])]
Z[i] = [float(a[2]), float(a[3])]
result = []
for i in range(25):
u = np.array([[V[i][0]], [V[i][1]]])
z = np.array([[Z[i][0]], [Z[i][1]]])
x, P = predict(x, P, F=1, Q=Q, u=u, B=B)
x, P = update(x, P, z, R, H)
result.append((x[:2]).tolist())
plt.plot(Z.T[0], Z.T[1], 'ro')
x1, x2 = zip(*result)
plt.plot(x1, x2, 'b-')
# plt.plot(x21, x22, '-')
plt.show()
r_noise=r_noise,
R=R,
)
s = Saver(kf)
# ------ RUN FILTER ------
if observe_every > 1:
data.loc[data.index % observe_every != 0, "q_obs"] = np.nan
# Iterate over the Kalman Filter
for time_ix, z in enumerate(np.vstack([data["q_obs"], data["r_obs"]]).T):
kf.predict()
# only make update steps every n timesteps
if time_ix % observe_every == 0:
kf.update(z)
s.save()
s.to_array()
# only observe every n values
# data["q_true_original"] = data["q_true"]
# update data with POSTERIOR estimates
# Calculate the DISCHARGE (measurement operator * \bar{x})
data["q_filtered"] = (s.H @ s.x)[:, 0]
data["q_variance"] = (s.H @ s.P)[:, 0, 0]
return kf, s, data
[x[j] - x[j-1]],
[y[j] - y[j-1]]])
# If near constant acceleration model, measure position, velocity and acceleration.
elif args.model == 'nca':
meas = np.array([[x[j]],
[y[j]],
[x[j]-x[j-1]],
[y[j]-y[j-1]],
[(x[j] - x[j-1]) - (x[j-1] - x[j-2])],
[(y[j] - y[j-1]) - (y[j-1] - y[j-2])]])
# If doing sanity check, use library. Else use provided kalman_step function.
if args.sanity_check:
state, covariance = predict(state, covariance, A, Q_i)
state, covariance = update(state, covariance, meas, R_i, C)
else:
state, covariance, _ , _ = kalman_step(A, C, Q_i, R_i, meas, state, covariance)
# Get position (velocity, acceleration) from deduced state.
sx[j] = state[0]
sy[j] = state[1]
if args.model in {'ncv', 'nca'}:
sx_d[j] = state[2]
sy_d[j] = state[3]
if args.model == 'nca':
sx_dd[j] = state[4]
sy_dd[j] = state[5]
def run_1D_filter(data: pd.DataFrame,
R: float,
Q: float,
P0: float,
S0: float = 5.74) -> pd.DataFrame:
"""Run a simple 1D Kalman Filter on noisy simulated data.
Initialize:
We have an initial estimate of the state variance (`P0`).
We use the first 'measurement' as our initial estimate (`X[0] = z[0]`)
Predict:
We require an initial input storage value - `S0`.
We use the ABC model as our process model to produce a prediction of X (discharge).
We assume the error/variance of this process to be `Q` (process error).
Update:
We use noisy observations (simulated from the ABCModel) as measurements `z`.
We assume the noise on the simulated data to be `R` (measurement error).
Our filtered state (X)
Note:
- The ABC model has an internal state parameter (S[t]) that we are currently not using.
- The ABC model produces a direct estimate of X, rather than the change in X. We
calculate the change in x (`dx`) as `dx = Qsim - X`, i.e. the current estimate
minus the previous (filtered) estimate.
- The improved state (X = discharge) estimated by the filter is not fed back into the
ABC model.
We use the `filterpy.kalman` implementation of the Kalman Filter.
Args:
data (pd.DataFrame): driving data, including precipitation (for driving the ABCmodel) &
the noisy Qsim values. Must have columns: ['precip', 'qsim', 'qsim_noise', 'ssim']
R (float): The measurement noise (because we have simulated data this is known)
Q (float): The process noise (variance / uncertainty on the ABC Model estimate)
P0 (float): The initial estimate for the state noise (variance on the prior / state uncertainty)
S0 (float, optional): [description]. Defaults to 5.74.
Returns:
pd.DataFrame: [description]
"""
assert all(np.isin(["precip", "qsim", "qsim_noise", "ssim"], data.columns))
measured_values = []
predicted_values = []
filtered_values = []
kalman_gains = []
log_likelihoods = []
residuals = []
# initialize step
P = P0
X = data["qsim_noise"][0]
storage = S0
# Kalman Filtered Qsim
for ix, precip in enumerate(data["precip"]):
# predict step
Qsim, storage = abcmodel(storage, precip) # process model (simulate Q)
dx = Qsim - X # convert into a change (dx)
X, P = kf.predict(x=X, P=P, u=dx, Q=Q)
predicted_values.append(X)
# update step
z = data["qsim_noise"][ix] # measurement
X, P, y, K, S, log_likelihood = kf.update(x=X,
P=P,
z=z,
R=R,
return_all=True)
filtered_values.append(X)
measured_values.append(z)
kalman_gains.append(float(K))
log_likelihoods.append(float(log_likelihood))
residuals.append(float(y))
out = pd.DataFrame({
"measured": measured_values,
"predicted": predicted_values,
"filtered": filtered_values,
"unobserved": data["qsim"],
"kalman_gain": kalman_gains,
"log_likelihood": log_likelihoods,
"residual": residuals,
})
return out
