Struct filter::gh::GHKFilter

pub struct GHKFilter<T> {
    pub g: T,
    pub h: T,
    pub k: T,
    pub dt: T,
    pub xt: T,
    pub dxt: T,
    pub ddxt: T,
    pub x_p: T,
    pub dx_p: T,
    pub ddx_p: T,
}

A g-h-k filter.

Example

use filter::gh::GHFilter;
use assert_approx_eq::assert_approx_eq;

let mut fgh: GHFilter<f32> = GHFilter::new(0.0, 0.0, 0.8, 0.2, 1.0);
assert_approx_eq!(0.8, fgh.update(1.0));
assert_approx_eq!(0.2, fgh.dxt);

fgh.g = 1.0;
fgh.h = 0.01;
assert_approx_eq!(2.0, fgh.update(2.0));

References

• Brookner, "Tracking and Kalman Filters Made Easy". John Wiley and Sons, 1998.

Fields

g: T

Filter g gain parameter.

h: T

Filter h gain parameter.

k: T

Filter k gain parameter.

dt: T

Timestep (time between sample)

xt: T

State of the filter.

dxt: T

First Derivative of the filter state.

ddxt: T

Second derivative of the filter state.

x_p: T

Predicted filter state.

dx_p: T

Predicted first derivative of the filter state.

ddx_p: T

Predicted second derivative of the filter state.

Methods

impl<T: FloatCore> GHKFilter<T>

pub fn new(x0: T, dx0: T, ddx0: T, g: T, h: T, k: T, dt: T) -> GHKFilter<T>

Returns a g-h-k filter with the given initialisation parameters.

Arguments

• x0 - initial value for the filter state.
• dx0 - initial value for the first derivative of the filter state.
• ddx0 - initial value for the second derivative of the filter state.
• g - filter g gain parameter.
• h - filter h gain parameter.
• k - filter k gain parameter.
• dt - time between samples.

Example

use filter::gh::GHFilter;
let fgh = GHFilter::new(0.0, 0.0, 0.2 ,0.2, 0.01);

pub fn update(&mut self, z: T) -> T

Performs the g-h filter predict and update step on the measurement z. Returns the new value for x.

pub fn vrf_prediction(&self) -> T

Returns the Variance Reduction Factor for x of the prediction step of the filter.

References

• Asquith and Woods, "Total Error Minimization in First and Second Order Prediction Filters" Report No RE-TR-70-17, U.S. Army Missle Command. Redstone Arsenal, Al. November 24, 1970.

pub fn vrf(&self) -> (T, T, T)

Returns the Variance Reduction Factor (VRF) of the state variable of the filter (x) and its derivatives (dx, ddx).

pub fn bias_error(&self, dddx: T) -> T

Returns the bias error given the specified constant jerk(dddx).

References

• Asquith and Woods, "Total Error Minimization in First and Second Order Prediction Filters" Report No RE-TR-70-17, U.S. Army Missle Command. Redstone Arsenal, Al. November 24, 1970.

Trait Implementations

impl<T: Debug> Debug for GHKFilter<T>

fn fmt(&self, f: &mut Formatter) -> Result

Formats the value using the given formatter.