Line data    Source code

       1             : #include "ExtendedKalmanFilter.h"
       2             : #include "AP_Math/matrixN.h"
       3             : 
       4             : 
       5       13618 : float ExtendedKalmanFilter::measurementpredandjacobian(VectorN<float,N> &A, float Px, float Py)
       6             : {
       7             :     // This function computes the Jacobian using equations from
       8             :     // analytical derivation of Gaussian updraft distribution
       9             :     // This expression gets used lots
      10       13618 :     float expon = expf(- (powf(X[2]-Px, 2) + powf(X[3]-Py, 2)) / powf(X[1], 2));
      11             :     // Expected measurement
      12       13618 :     float w = X[0] * expon;
      13             : 
      14             :     // Elements of the Jacobian
      15       13618 :     A[0] = expon;
      16       13618 :     A[1] = 2 * X[0] * ((powf(X[2]-Px,2) + powf(X[3]-Py,2)) / powf(X[1],3)) * expon;
      17       13618 :     A[2] = -2 * (X[0] * (X[2]-Px) / powf(X[1],2)) * expon;
      18       13618 :     A[3] = -2 * (X[0] * (X[3]-Py) / powf(X[1],2)) * expon;
      19       13618 :     return w;
      20             : }
      21             : 
      22             : 
      23           1 : void ExtendedKalmanFilter::reset(const VectorN<float,N> &x, const MatrixN<float,N> &p, const MatrixN<float,N> q, float r)
      24             : {
      25           1 :     P = p;
      26           1 :     X = x;
      27           1 :     Q = q;
      28           1 :     R = r;
      29           1 : }
      30             : 
      31             : 
      32       13618 : void ExtendedKalmanFilter::update(float z, float Px, float Py, float driftX, float driftY)
      33             : {
      34       13618 :     MatrixN<float,N> tempM;
      35       13618 :     VectorN<float,N> H;
      36       13618 :     VectorN<float,N> P12;
      37       13618 :     VectorN<float,N> K;
      38             :     
      39             :     // LINE 28
      40             :     // Estimate new state from old.
      41       13618 :     X[2] += driftX;
      42       13618 :     X[3] += driftY;
      43             : 
      44             :     // LINE 33
      45             :     // Update the covariance matrix
      46             :     // P = A*ekf.P*A'+ekf.Q;
      47             :     // We know A is identity so
      48             :     // P = ekf.P+ekf.Q;
      49       13618 :     P += Q;
      50             : 
      51             :     // What measurement do we expect to receive in the estimated
      52             :     // state
      53             :     // LINE 37
      54             :     // [z1,H] = ekf.jacobian_h(x1);
      55       13618 :     float z1 = measurementpredandjacobian(H, Px, Py);
      56             : 
      57             :     // LINE 40
      58             :     // P12 = P * H';
      59       13618 :     P12.mult(P, H); //cross covariance 
      60             :     
      61             :     // LINE 41
      62             :     // Calculate the KALMAN GAIN
      63             :     // K = P12 * inv(H*P12 + ekf.R);                     %Kalman filter gain
      64       13618 :     K = P12 * 1.0 / (H * P12 + R);
      65             : 
      66             :     // Correct the state estimate using the measurement residual.
      67             :     // LINE 44
      68             :     // X = x1 + K * (z - z1);
      69       13618 :     X += K * (z - z1);
      70             : 
      71             :     // Make sure X[1] stays positive.
      72       13618 :     X[1] = X[1]>40.0 ? X[1]: 40.0;
      73             : 
      74             :     // Correct the covariance too.
      75             :     // LINE 46
      76             :     // NB should be altered to reflect Stengel
      77             :     // P = P_predict - K * P12';
      78       13618 :     tempM.mult(K, P12);
      79       13618 :     P -= tempM;
      80             :     
      81       13618 :     P.force_symmetry();
      82       13618 : }