Joseph Roberts / Mbed 2 deprecated Quadcopter_mk2

New version of quadcopter software written to OO principles

Dependencies:   mbed MODSERIAL filter mbed-rtos ConfigFile PID PPM FreeIMU_external_magnetometer TinyGPS

Global/Kalman/Kalman.cpp

Committer:
joe4465
Date:
2015-05-08
Revision:
4:9ffbf9101992
Parent:
2:969dfa4f2436

File content as of revision 4:9ffbf9101992:

#include "Kalman.h"

Kalman::Kalman(double q, double r, double p, double intialValue)
{
    _kalmanState = KalmanState();
    _kalmanState.q = q;
    _kalmanState.r = r;
    _kalmanState.p = p;
    _kalmanState.x = intialValue;
}

Kalman::~Kalman(){}

double Kalman::update(double predicted, double measurement)
{
  //prediction
  _kalmanState.x = predicted * _kalmanState.x;
  _kalmanState.p = _kalmanState.p + _kalmanState.q;

  //measurement
  _kalmanState.k = _kalmanState.p / (_kalmanState.p + _kalmanState.r);
  _kalmanState.x = _kalmanState.x + _kalmanState.k * (measurement - _kalmanState.x);
  _kalmanState.p = (1 - _kalmanState.k) * _kalmanState.p;
  
  return _kalmanState.x;
}

double Kalman::getEstimated()
{
    return _kalmanState.x;
}