Android Open Source - Flight-Computer-Android-Flightradar24 Kalman Filter

• Back to Map/map  ↑

From Project

Back to project page Flight-Computer-Android-Flightradar24.

License

The source code is released under:

GNU General Public License

Java Source Code

package com.flightcomputer.utilities;
// w  w w.jav a  2s . c o m
public class KalmanFilter {
  // The state we are tracking, namely:
  private double x_abs_;  // The absolute value of x.
  private double x_vel_;  // The rate of change of x.
  
  // Covariance matrix for the state.
  private double p_abs_abs_;
  private double p_abs_vel_;
  private double p_vel_vel_;
  
  // The variance of the acceleration noise input in the system model.
  private double var_accel_;
  
  // Constructor. Assumes a variance of 1.0 for the system model's
  // acceleration noise input, in units per second squared.
  public KalmanFilter() {
    setAccelerationVariance(1.);
    reset();
  }
  
  // Constructor. Caller supplies the variance for the system model's
  // acceleration noise input, in units per second squared.
  public KalmanFilter(double var_accel) {
    setAccelerationVariance(var_accel);
    reset();
  }
    
  // The following three methods reset the filter. All of them assign a huge
  // variance to the tracked absolute quantity and a var_accel_ variance to
  // its derivative, so the very next measurement will essentially be
  // copied directly into the filter. Still, we provide methods that allow
  // you to specify initial settings for the filter's tracked state.

  public void reset() {
    reset(0., 0.);
  }
  
  public void reset(double abs_value) {
    reset(abs_value, 0.);
  }
  
  public void reset(double abs_value, double vel_value) {
    x_abs_ = abs_value;
    x_vel_ = vel_value;
    p_abs_abs_ = 1.e10;
    p_abs_vel_ = 0.;
    p_vel_vel_ = var_accel_;
  }
  
  // Sets the variance for the acceleration noise input in the system model,
  // in units per second squared.
  public void setAccelerationVariance(double var_accel) {
    var_accel_ = var_accel;
  }
  
  // Updates state given a sensor measurement of the absolute value of x,
  // the variance of that measurement, and the interval since the last
  // measurement in seconds. This interval must be greater than 0; for the
  // first measurement after a reset(), it's safe to use 1.0.
  public void update(double z_abs, double var_z_abs, double dt) {
    // Note: math is not optimized by hand. Let the compiler sort it out.
    // Predict step.
    // Update state estimate.
    x_abs_ += x_vel_ * dt;
    // Update state covariance. The last term mixes in acceleration noise.
    p_abs_abs_ += 2.*dt*p_abs_vel_ + dt*dt*p_vel_vel_ + var_accel_*dt*dt*dt*dt/4.;
    p_abs_vel_ +=                       dt*p_vel_vel_ + var_accel_*dt*dt*dt/2.;
    p_vel_vel_ +=                                     + var_accel_*dt*dt;
    
    // Update step.
    double y = z_abs - x_abs_;  // Innovation.
    double s_inv = 1. / (p_abs_abs_ + var_z_abs);  // Innovation precision.
    double k_abs = p_abs_abs_*s_inv;  // Kalman gain
    double k_vel = p_abs_vel_*s_inv;
    // Update state estimate.
    x_abs_ += k_abs * y;
    x_vel_ += k_vel * y;
    // Update state covariance.
    p_vel_vel_ -= p_abs_vel_*k_vel;
    p_abs_vel_ -= p_abs_vel_*k_abs;
    p_abs_abs_ -= p_abs_abs_*k_abs;
  }
  
  // Getters for the state and its covariance.
  public double getXAbs() { return x_abs_; }
  public double getXVel() { return x_vel_; }
  public double getCovAbsAbs() { return p_abs_abs_; }
  public double getCovAbsVel() { return p_abs_vel_; }
  public double getCovVelVel() { return p_vel_vel_; }
}

Java Source Code List

• Back to Map/map  ↑