@@ -0,0 +1,552 @@
+package me.drton.jmavsim;%0A%0Apublic final class Filter %7B%0A    private double tc = 0.0;%0A    private double z1 = 0.0;%0A%0A    /**%0A     * First-order fixed-timestep filter.%0A     * larger tau, smoother filter%0A     * %0A     */%0A    public void filterInit(double dt, double tau, double setpoint) %7B%0A        this.tc = dt / tau;%0A        filterReset(setpoint);%0A    %7D%0A    %0A    public void filterReset(double setpoint) %7B%0A        this.z1 = setpoint;%0A    %7D%0A%0A    double filter(double signal) %7B%0A        this.z1 += (signal - this.z1) * this.tc;%0A        return this.z1;%0A    %7D%0A%7D%0A

Add Filter utility class.