/** * Sets the value of this tuple to the sum of tuple1 and tuple2 (this = tuple1 + tuple2). * @param tuple1 the first Tuple3d * @param tuple2 the second Tuple3d */ public final void add(Tuple3d tuple1, Tuple3d tuple2) { tuple.add(tuple1, tuple2); }
/** * Sets the value of this tuple to the sum of itself and tuple1. * @param tuple1 the other Tuple3d */ public final void add(Tuple3d tuple1) { tuple.add(tuple1); }
public void corrupt(Tuple3d signal) { double std = standardDeviation.getDoubleValue(); double noiseX = std * random.nextGaussian(); double noiseY = std * random.nextGaussian(); double noiseZ = std * random.nextGaussian(); noise.set(noiseX, noiseY, noiseZ); signal.add(noise); }
public void corrupt(Tuple3d signal) { double std = standardDeviation.getDoubleValue(); double biasUpdateX = std * random.nextGaussian() * squareRootOfUpdateDT; double biasUpdateY = std * random.nextGaussian() * squareRootOfUpdateDT; double biasUpdateZ = std * random.nextGaussian() * squareRootOfUpdateDT; biasYoFrameVector.add(biasUpdateX, biasUpdateY, biasUpdateZ); biasYoFrameVector.get(biasVector); signal.add(biasVector); }
public static void integrate(Matrix3d rotationToPack, Tuple3d positionToPack, double dt, Twist twist) { twist.changeFrame(twist.getBodyFrame()); Vector3d dPosition = new Vector3d(); twist.getLinearPart(dPosition); // velocity in body frame rotationToPack.transform(dPosition); // velocity in base frame dPosition.scale(dt); // translation in base frame positionToPack.add(dPosition); Vector3d axis = new Vector3d(); twist.getAngularPart(axis); axis.scale(dt); double angle = axis.length(); if (angle > 0.0) axis.normalize(); else axis.set(1.0, 0.0, 0.0); AxisAngle4d axisAngle = new AxisAngle4d(axis, angle); Matrix3d dRotation = new Matrix3d(); dRotation.set(axisAngle); rotationToPack.mul(dRotation); }