def addPoseMotionTerms(self, problem, tv, rv):
     wt = 1.0 / tv
     wr = 1.0 / rv
     W = np.diag([wt, wt, wt, wr, wr, wr])
     asp.addMotionErrorTerms(problem, self.poseDv, W, errorOrder)
Exemple #2
0
 def addPoseMotionTerms(self, problem, tv, rv):
     wt = 1.0/tv;
     wr = 1.0/rv
     W = np.diag([wt,wt,wt,wr,wr,wr])
     asp.addMotionErrorTerms(problem, self.poseDv, W, errorOrder)
Exemple #3
0
 def addBiasMotionTerms(self, problem):
     Wgyro = np.eye(3) / (self.gyroRandomWalk * self.gyroRandomWalk)
     Waccel = np.eye(3) / (self.accelRandomWalk * self.accelRandomWalk)
     order = 1
     asp.addMotionErrorTerms(problem, self.gyroBiasDv, Wgyro, order)
     asp.addMotionErrorTerms(problem, self.accelBiasDv, Waccel, order)
Exemple #4
0
 def addBiasMotionTerms(self, problem):
     Wgyro = np.eye(3) / (self.gyroRandomWalk * self.gyroRandomWalk)
     Waccel =  np.eye(3) / (self.accelRandomWalk * self.accelRandomWalk)
     order = 1
     asp.addMotionErrorTerms(problem, self.gyroBiasDv, Wgyro, order)
     asp.addMotionErrorTerms(problem, self.accelBiasDv, Waccel, order)