navboxplus.py 文件源码

python
阅读 20 收藏 0 点赞 0 评论 0

项目:navboxplus 作者: jnez71 项目源码 文件源码
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)
评论列表
文章目录


问题


面经


文章

微信
公众号

扫码关注公众号