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)
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)
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)
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)