Line data    Source code

       1             : /*
       2             : Extended Kalman Filter class by Sam Tabor, 2013.
       3             : * http://diydrones.com/forum/topics/autonomous-soaring
       4             : * Set up for identifying thermals of Gaussian form, but could be adapted to other
       5             : * purposes by adapting the equations for the jacobians.
       6             : */
       7             : 
       8             : #pragma once
       9             : 
      10             : #include <AP_Math/matrixN.h>
      11             : 
      12             : class ExtendedKalmanFilter {
      13             : public:
      14         692 :     ExtendedKalmanFilter(void) {}
      15             : 
      16             :     static constexpr const uint8_t N = 4;
      17             : 
      18             :     VectorN<float,N> X;
      19             :     MatrixN<float,N> P;
      20             :     MatrixN<float,N> Q;
      21             :     float R;
      22             :     void reset(const VectorN<float,N> &x, const MatrixN<float,N> &p, const MatrixN<float,N> q, float r);
      23             :     void update(float z, float Px, float Py, float driftX, float driftY);
      24             : 
      25             : private:
      26             :     float measurementpredandjacobian(VectorN<float,N> &A, float Px, float Py);
      27             : };