public SecondOrderFilteredYoVariable(String name, YoVariableRegistry registry, double dt, double naturalFrequencyInHz, double dampingRatio, SecondOrderFilterType filterType) { this(name, registry, dt, new SecondOrderFilteredYoVariableParameters(name, registry, naturalFrequencyInHz, dampingRatio, filterType), null); }
private void computeCoefficients() { double omega = 2 * Math.PI * parameters.getNaturalFrequencyInHz().getDoubleValue(); double xi = parameters.getDampingRatio().getDoubleValue(); switch (parameters.getFilterType()) { case LOW_PASS: b[0] = omega * omega; b[1] = 2.0 * omega * omega; b[2] = omega * omega; break; case NOTCH: b[0] = 4.0 / (dt * dt) + omega * omega; b[1] = 2.0 * omega * omega - 8.0 / (dt * dt); b[2] = 4.0 / (dt * dt) + omega * omega; break; case HIGH_PASS: b[0] = 4.0 / (dt * dt); b[1] = -8.0 / (dt * dt); b[2] = 4.0 / (dt * dt); break; } a[0] = 4.0 / (dt * dt) + 4.0 / dt * xi * omega + omega * omega; a[1] = 2.0 * omega * omega - 8.0 / (dt * dt); a[2] = 4.0 / (dt * dt) - 4.0 / dt * xi * omega + omega * omega; } }
public void setDampingRatio(double dampingRatio) { parameters.getDampingRatio().set(Math.max(dampingRatio, 0.0)); computeCoefficients(); }
public void setNaturalFrequencyInHz(double naturalFrequencyInHz) { parameters.getNaturalFrequencyInHz().set(Math.min(Math.max(naturalFrequencyInHz, 0.0), 1.0 / (2.0 * dt))); computeCoefficients(); }
public SecondOrderFilteredYoVariable(String name, YoVariableRegistry registry, double dt, double naturalFrequencyInHz, double dampingRatio, SecondOrderFilterType filterType, YoDouble inputVariable) { this(name, registry, dt, new SecondOrderFilteredYoVariableParameters(name, registry, naturalFrequencyInHz, dampingRatio, filterType), inputVariable); }
public static SecondOrderFilteredYoFrameVector createSecondOrderFilteredYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, double dt, double naturalFrequencyInHz, double dampingRatio, SecondOrderFilterType filterType, YoFrameVector3D unfilteredVector) { SecondOrderFilteredYoVariableParameters parameters = new SecondOrderFilteredYoVariableParameters(namePrefix + nameSuffix, registry, naturalFrequencyInHz, dampingRatio, filterType); return createSecondOrderFilteredYoFrameVector(namePrefix, nameSuffix, registry, dt, parameters, unfilteredVector); }
public static SecondOrderFilteredYoFrameVector createSecondOrderFilteredYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, double dt, double naturalFrequencyInHz, double dampingRatio, SecondOrderFilterType filterType, ReferenceFrame referenceFrame) { SecondOrderFilteredYoVariableParameters parameters = new SecondOrderFilteredYoVariableParameters(namePrefix + nameSuffix, registry, naturalFrequencyInHz, dampingRatio, filterType); return createSecondOrderFilteredYoFrameVector(namePrefix, nameSuffix, registry, dt, parameters, referenceFrame); }