def predict(self, r, rnext, wf0, Cf, dt):
"""
r: Desired reference state at time t
rnext: Desired reference state at time t+dt
wf0: Mean of the process noise
Cf: Covariance of the process noise
dt: Timestep to predict forward
Progresses the state estimate one dt into the future.
Returns the control effort u.
"""
# Compute control action
self.u = self.g(r, rnext, self.x, self.Cx, dt)
# Store relevant parameters
utp = self.utpDict['_f']
# Compute sigma points, propagate through process, and store tangent-space representation
M = spl.cholesky(utp[0]*spl.block_diag(self.Cx, Cf))
s0 = np.append(self.x, wf0)
fS = [self.sf(s0, dt)]
fT_sum = np.zeros(self.n_m)
for Vi in np.vstack((M, -M)):
fS.append(self.sf(self.sxplus(s0, Vi), dt))
fT_sum += self.xminus(fS[-1], fS[0])
# Determine the mean of the propagated sigma points from the tangent vectors
self.x = self.xplus(fS[0], utp[2]*fT_sum)
# Determine the covariance from the tangent-space deviations from the mean
fv0 = self.xminus(fS[0], self.x)
fPi_sum = np.zeros((self.n_m, self.n_m))
for fSi in fS[1:]:
fv = self.xminus(fSi, self.x)
fPi_sum += np.outer(fv, fv)
self.Cx = utp[3]*np.outer(fv0, fv0) + utp[2]*fPi_sum
# Over and out
return np.copy(self.u)
评论列表
文章目录