Joseph Roberts / Mbed 2 deprecated Quadcopter_mk2

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

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

Global/Kalman/Kalman.cpp

Committer:
joe4465
Date:
2015-04-01
Revision:
2:969dfa4f2436

File content as of revision 2:969dfa4f2436:

#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;
}