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@4:9ffbf9101992, 2015-05-08 (annotated)

Committer:
joe4465
Date:
Fri May 08 09:07:38 2015 +0000
Revision:
4:9ffbf9101992
Parent:
2:969dfa4f2436 
End of FYP

Who changed what in which revision?

UserRevisionLine numberNew contents of line
joe4465 2:969dfa4f2436 1 #include "Kalman.h"
joe4465 2:969dfa4f2436 2
joe4465 2:969dfa4f2436 3 Kalman::Kalman(double q, double r, double p, double intialValue)
joe4465 2:969dfa4f2436 4 {
joe4465 2:969dfa4f2436 5 _kalmanState = KalmanState();
joe4465 2:969dfa4f2436 6 _kalmanState.q = q;
joe4465 2:969dfa4f2436 7 _kalmanState.r = r;
joe4465 2:969dfa4f2436 8 _kalmanState.p = p;
joe4465 2:969dfa4f2436 9 _kalmanState.x = intialValue;
joe4465 2:969dfa4f2436 10 }
joe4465 2:969dfa4f2436 11
joe4465 2:969dfa4f2436 12 Kalman::~Kalman(){}
joe4465 2:969dfa4f2436 13
joe4465 2:969dfa4f2436 14 double Kalman::update(double predicted, double measurement)
joe4465 2:969dfa4f2436 15 {
joe4465 2:969dfa4f2436 16 //prediction
joe4465 2:969dfa4f2436 17 _kalmanState.x = predicted * _kalmanState.x;
joe4465 2:969dfa4f2436 18 _kalmanState.p = _kalmanState.p + _kalmanState.q;
joe4465 2:969dfa4f2436 19
joe4465 2:969dfa4f2436 20 //measurement
joe4465 2:969dfa4f2436 21 _kalmanState.k = _kalmanState.p / (_kalmanState.p + _kalmanState.r);
joe4465 2:969dfa4f2436 22 _kalmanState.x = _kalmanState.x + _kalmanState.k * (measurement - _kalmanState.x);
joe4465 2:969dfa4f2436 23 _kalmanState.p = (1 - _kalmanState.k) * _kalmanState.p;
joe4465 2:969dfa4f2436 24
joe4465 2:969dfa4f2436 25 return _kalmanState.x;
joe4465 2:969dfa4f2436 26 }
joe4465 2:969dfa4f2436 27
joe4465 2:969dfa4f2436 28 double Kalman::getEstimated()
joe4465 2:969dfa4f2436 29 {
joe4465 2:969dfa4f2436 30 return _kalmanState.x;
joe4465 2:969dfa4f2436 31 }