def predict(self):
self.sp = calculate_sigmas(self.x_hat, self.Nx, self.P, self.kappa)
# Transform sigmas into predicted state space
for i in range(self.num_sp):
self.sp_f[i] = self.F(self.sp[i], self.dt)
# Normalize through unscented transform
self.x_hat, self.P = unscented_transform(self.sp_f, self.weights, self.Q)
def predict(self):
self.sp = calculate_sigmas(self.x_hat, self.Nx, self.P, self.kappa)
# Transform sigmas into predicted state space
for i in range(self.num_sp):
self.sp_f[i] = self.F(self.sp[i], self.dt)
# Normalize through unscented transform
self.x_hat, self.P = unscented_transform(self.sp_f, self.weights,
self.Q)
def update(self, z):
# Transform sigmas into measurement space
for i in range(self.num_sp):
self.sp_h[i] = self.H(self.sp_f[i])
# Mean and covariance of the state in the measurement space
Hx_bar, PHx = unscented_transform(self.sp_h, self.weights, self.R)
# Cross variance of Fx and Hx -- used to calculate K
cross_var = np.zeros((self.Nx, self.Nz))
for i in range(self.num_sp):
cross_var += self.weights[i] * np.outer(self.sp_f[i] - self.x_hat,
self.sp_h[i] - Hx_bar)
# Calculate K and measurement residual
K = np.dot(cross_var, inv(PHx))
residual = self.difference(np.array(z), Hx_bar)
# Update predicted to new values for state and variance
self.x_hat += np.dot(K, residual)
self.P -= np.dot(K, np.dot(PHx, K.T))
def update(self, z):
# Transform sigmas into measurement space
for i in range(self.num_sp):
self.sp_h[i] = self.H(self.sp_f[i])
# Mean and covariance of the state in the measurement space
Hx_bar, PHx = unscented_transform(self.sp_h, self.weights, self.R)
# Cross variance of Fx and Hx -- used to calculate K
cross_var = np.zeros((self.Nx, self.Nz))
for i in range(self.num_sp):
cross_var += self.weights[i] * np.outer(self.sp_f[i] - self.x_hat,
self.sp_h[i] - Hx_bar)
# Calculate K and measurement residual
K = np.dot(cross_var, inv(PHx))
residual = self.difference(z, Hx_bar)
# Update predicted to new values for state and variance
self.x_hat += np.dot(K, residual)
self.P -= np.dot(K, np.dot(PHx, K.T))