1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361
/*! Provides implementations of and related to the g-h and g-h-k filter. */ use num_traits::float::FloatCore; use num_traits::Float; /// A g-h 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 /// * Labbe, "Kalman and Bayesian Filters in Python" http://rlabbe.github.io/Kalman-and-Bayesian-Filters-in-Python /// * Brookner, "Tracking and Kalman Filters Made Easy". John Wiley and Sons, 1998. /// #[derive(Debug)] pub struct GHFilter<T> { /// Filter g gain parameter. pub g: T, /// Filter h gain parameter. pub h: T, /// Timestep (time between sample) pub dt: T, /// State of the filter. pub xt: T, /// Derivative of the filter state. pub dxt: T, /// Predicted filter state. pub x_p: T, /// Predicted derivative of the filter state. pub dx_p: T, } impl<T: FloatCore> GHFilter<T> { /// Returns a g-h filter with the given initialisation parameters. /// /// # Arguments /// /// /// * `x0` - initial value for the filter state. /// * `dx0` - initial value for the derivative of the filter state. /// * `g` - filter g gain parameter. /// * `h` - filter h 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 new(x0: T, dx0: T, g: T, h: T, dt: T) -> GHFilter<T> { GHFilter { g, h, dt, xt: x0, dxt: dx0, x_p: x0, dx_p: dx0, } } /// Performs the g-h filter predict and update step on the given measurement z. /// Returns the new state of x. pub fn update(&mut self, z: T) -> T { // Predict self.dx_p = self.dxt; self.x_p = self.xt + self.dt * self.dxt; // Update let y = z - self.x_p; self.dxt = self.dx_p + self.h * (y / self.dt); self.xt = self.x_p + self.g * y; self.xt } /// Returns the Variance Reduction Factor (VRF) of the state variable /// of the filter (x) and its derivatives (dx, ddx). pub fn vrf(&self) -> (T, T) { let two = T::one() + T::one(); let three = two + T::one(); let four = two + two; let den = self.g * (four - two * self.g - self.h); let vx = (two * self.g.powi(2) + two * self.h - three * self.g * self.h) / den; let vdx = two * self.h.powi(2) / (self.dt.powi(2) * den); (vx, vdx) } /// Returns the Variance Reduction Factor of the prediction step of the filter. /// /// # References /// * Asquith, "Weight Selection in First Order Linear Filters" Report No RG-TR-69-12, /// U.S. Army Missle Command. Redstone Arsenal, Al. November 24, 1970. pub fn vrf_prediction(&self) -> T { let two = T::one() + T::one(); let four = two + two; (two * self.g.powi(2) + two * self.h + self.g * self.h) / (self.g * (four - two * self.g - self.h)) } } /// 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. /// #[derive(Debug)] pub struct GHKFilter<T> { /// Filter g gain parameter. pub g: T, /// Filter h gain parameter. pub h: T, /// Filter k gain parameter. pub k: T, /// Timestep (time between sample) pub dt: T, /// State of the filter. pub xt: T, /// First Derivative of the filter state. pub dxt: T, /// Second derivative of the filter state. pub ddxt: T, /// Predicted filter state. pub x_p: T, /// Predicted first derivative of the filter state. pub dx_p: T, /// Predicted second derivative of the filter state. pub ddx_p: T, } impl<T: FloatCore> 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 new(x0: T, dx0: T, ddx0: T, g: T, h: T, k: T, dt: T) -> GHKFilter<T> { GHKFilter { g, h, k, dt, xt: x0, dxt: dx0, ddxt: ddx0, x_p: x0, dx_p: dx0, ddx_p: ddx0, } } /// Performs the g-h filter predict and update step on the measurement z. /// Returns the new value for x. pub fn update(&mut self, z: T) -> T { let two = T::one() + T::one(); // Predict self.ddx_p = self.ddxt; self.dx_p = self.dxt + self.ddxt * self.dt; self.x_p = self.xt + self.dt * self.dxt + self.ddxt * self.dt * self.dt / two; // Update let y = z - self.x_p; self.ddxt = self.ddx_p + two * self.k * y / (self.dt * self.dt); self.dxt = self.dx_p + self.h * (y / self.dt); self.xt = self.x_p + self.g * y; self.xt } ///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_prediction(&self) -> T { let two = T::from(2).unwrap(); let four = T::from(4).unwrap(); let g = self.g; let h = self.h; let k = self.k; let gh2 = two * g + h; ((g * k * (gh2 - four) + h * (g * gh2 + two * h)) / (two * k - (g * (h + k) * (gh2 - four)))) } /// Returns the Variance Reduction Factor (VRF) of the state variable /// of the filter (x) and its derivatives (dx, ddx). pub fn vrf(&self) -> (T, T, T) { let two = T::from(2).unwrap(); let four = T::from(4).unwrap(); let eight = T::from(8).unwrap(); let g = self.g; let h = self.h; let k = self.k; let hg4 = four - two * g - h; let ghk = g * h + g * k - two * k; let vx = (two * h * (two * g.powi(2) + two * h - two * g * h) - two * g * k * hg4) / (two * k - g * (h + k) * hg4); let vdx = (two * (h.powi(2)) - four * h.powi(2) * k + four * k.powi(2) * (two - g)) / (two * hg4 * ghk); let vddx = eight * h * k.powi(2) / ((self.dt.powi(4)) * hg4 * ghk); (vx, vdx, vddx) } /// 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. pub fn bias_error(&self, dddx: T) -> T { -self.dt.powi(2) * dddx / (T::from(2.0).unwrap() * self.k) } } /// Returns g, h, k parameters for optimal smoothing of noise for a given value of g. /// This is due to Polge and Bhagavan. /// /// # References /// * Polge and Bhagavan. "A Study of the g-h-k Tracking Filter". Report No. RE-CR-76-1. /// University of Alabama in Huntsville. July, 1975 pub fn optimal_noise_smoothing<T: Float>(g: T) -> (T, T, T) { let one = T::one(); let two = T::from(2).unwrap(); let four = T::from(4).unwrap(); let eight = T::from(8).unwrap(); let sixty_four = T::from(64).unwrap(); let h = ((two * g.powi(3) - four * g.powi(2)) + (four * g.powi(6) - sixty_four * g.powi(5) + sixty_four * g.powi(4)).sqrt()) / (eight * (one - g)); let k = (h * (two - g) - g.powi(2)) / g; (g, h, k) } /// An order 1 least squared filter can be computed by a g-h filter by varying g and h over time /// according to the formulas below, where the first measurement is at n=0, the second is /// at n=1, and so on: pub fn least_squares_parameters<T: FloatCore>(n: T) -> (T, T) { let one = T::one(); let two = T::from(2).unwrap(); let six = T::from(6).unwrap(); let den = (n + two) * (n + one); let g = (two * (two * n + one)) / den; let h = six / den; (g, h) } /// Computes values for g and h for a critically damped filter. /// The idea here is to create a filter that reduces the influence of old data as new data comes in. /// This allows the filter to track a moving target better. This goes by different names. /// It may be called the discounted least-squares g-h filter, a fading-memory polynomal filter /// of order 1, or a critically damped g-h filter. /// /// # References /// * Brookner, "Tracking and Kalman Filters Made Easy". John Wiley and Sons, 1998. /// * Polge and Bhagavan. "A Study of the g-h-k Tracking Filter". Report No. RE-CR-76-1. /// University of Alabama in Huntsville. July, 1975 pub fn critical_damping_parameters_order_two<T: FloatCore>(theta: T) -> (T, T) { let one = T::one(); (one - theta.powi(2), (one - theta).powi(2)) } /// Computes values for g, h and k for a critically damped filter. /// The idea here is to create a filter that reduces the influence of old data as new data comes in. /// This allows the filter to track a moving target better. This goes by different names. /// It may be called the discounted least-squares g-h filter, a fading-memory polynomal filter /// of order 1, or a critically damped g-h filter. /// /// # References /// * Brookner, "Tracking and Kalman Filters Made Easy". John Wiley and Sons, 1998. /// * Polge and Bhagavan. "A Study of the g-h-k Tracking Filter". Report No. RE-CR-76-1. /// University of Alabama in Huntsville. July, 1975 pub fn critical_damping_parameters_order_three<T: FloatCore>(theta: T) -> (T, T, T) { let one = T::one(); let two = T::from(2).unwrap(); let three = T::from(2).unwrap(); ( one - theta.powi(3), (three / two) * (one - theta.powi(2)) * (one - theta), (one / two) * (one - theta).powi(3), ) } /// Computes the g,h constants for a Benedict-Bornder filter, which minimizes transient errors /// for a g-h filter. Returns the values g,h for a specified g. Strictly speaking, only h /// is computed, g is returned unchanged. The default formula for the Benedict-Bordner allows ringing. /// We can "nearly" critically damp it; ringing will be reduced, but not entirely eliminated at /// the cost of reduced performance. /// /// # References /// * Brookner, "Tracking and Kalman Filters Made Easy". John Wiley and Sons, 1998. pub fn benedict_bornder_constants<T: Float>(g: T, critical: bool) -> (T, T) { let g_sqr = g.powi(2); if critical { ( g, T::from(0.8).unwrap() * (T::from(2).unwrap() - g_sqr - T::from(2).unwrap() * (T::one() - g_sqr).sqrt()) / g_sqr, ) } else { (g, g_sqr / (T::from(2).unwrap() - g)) } }