def correct(self, sensor, z, wh0, Ch):
"""
sensor: String of the sensor key name
z: Value of the measurement
wh0: Mean of that sensor's noise
Ch: Covariance of that sensor's noise
Updates the state estimate with the given sensor measurement.
"""
# Store relevant functions and parameters
h, zminus, n_z, _, utp = self.hDict_full[sensor]
# Compute sigma points and emulate their measurement error
M = spl.cholesky(utp[0]*spl.block_diag(self.Cx, Ch))
V = np.vstack((M, -M))
s0 = np.append(self.x, wh0)
hE = [zminus(z, self.sh(s0, h))]
for i, Vi in enumerate(V):
hE.append(zminus(z, self.sh(self.sxplus(s0, Vi), h)))
hE = np.array(hE)
# Determine the mean of the sigma measurement errors
e0 = utp[1]*hE[0] + utp[2]*np.sum(hE[1:], axis=0)
# Determine the covariance and cross-covariance
hV = hE - e0
hPi_sum = np.zeros((n_z, n_z))
hPci_sum = np.zeros((self.n_m, n_z))
for Vqi, hVi in zip(V[:, :self.n_m], hV[1:]):
hPi_sum += np.outer(hVi, hVi)
hPci_sum += np.outer(Vqi, hVi)
Pzz = utp[3]*np.outer(hV[0], hV[0]) + utp[2]*hPi_sum
Pxz = utp[2]*hPci_sum
# Kalman update the state estimate and covariance
K = Pxz.dot(npl.inv(Pzz))
self.x = self.xplus(self.x, -K.dot(e0))
self.Cx = self.Cx - K.dot(Pzz).dot(K.T)
评论列表
文章目录
