ELCT 302 / Mbed 2 deprecated Sandbox

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

kalman.h@1:cbd32e9800fb, 2018-04-22 (annotated)

Committer:
KDrainEE
Date:
Sun Apr 22 13:22:06 2018 +0000
Branch:
test
Revision:
1:cbd32e9800fb
branch test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
KDrainEE 1:cbd32e9800fb 1
KDrainEE 1:cbd32e9800fb 2 class Kalman{
KDrainEE 1:cbd32e9800fb 3 public:
KDrainEE 1:cbd32e9800fb 4 void statePriori();
KDrainEE 1:cbd32e9800fb 5 void statePostPriori();
KDrainEE 1:cbd32e9800fb 6 void kCompute(float observation, float variance, float angle);
KDrainEE 1:cbd32e9800fb 7
KDrainEE 1:cbd32e9800fb 8 private:
KDrainEE 1:cbd32e9800fb 9 float mean;
KDrainEE 1:cbd32e9800fb 10 float covariance;
KDrainEE 1:cbd32e9800fb 11 float dt;
KDrainEE 1:cbd32e9800fb 12 float PredictionVariance;
KDrainEE 1:cbd32e9800fb 13 };
KDrainEE 1:cbd32e9800fb 14
KDrainEE 1:cbd32e9800fb 15 float[2] Kalman::statePriori(float speed, float angle){
KDrainEE 1:cbd32e9800fb 16 float returns[2];
KDrainEE 1:cbd32e9800fb 17 returns[1] = this->mean + this->dt*speed*sin(angle);
KDrainEE 1:cbd32e9800fb 18 returns[2] = this->covariance * pow(this->dt*speed*cos(angle),2) + PredictionVariance;
KDrainEE 1:cbd32e9800fb 19 return returns;
KDrainEE 1:cbd32e9800fb 20 }
KDrainEE 1:cbd32e9800fb 21
KDrainEE 1:cbd32e9800fb 22 void Kalman::statePostPriori(float observation, float measVariance, float angle, float covariance_hat, float mean_hat){
KDrainEE 1:cbd32e9800fb 23 float h = observation*cos(angle);
KDrainEE 1:cbd32e9800fb 24 float s = 1/(pow(h,2)*covariance_hat + measVariance);
KDrainEE 1:cbd32e9800fb 25 float k = covariance_hat*h*s;
KDrainEE 1:cbd32e9800fb 26 float z = observation*sin(angle);
KDrainEE 1:cbd32e9800fb 27 this->mean = mean_hat+k*(z-h*mean_hat);
KDrainEE 1:cbd32e9800fb 28 this->covariance = (1-k*h)*covariance_hat;
KDrainEE 1:cbd32e9800fb 29 }
KDrainEE 1:cbd32e9800fb 30
KDrainEE 1:cbd32e9800fb 31 float Kalman::kCompute(float speed, float turnAngle, float observation[2], float variance[2], float angle[2]){
KDrainEE 1:cbd32e9800fb 32 float[2] predictions = statePriori(speed, turnAngle);
KDrainEE 1:cbd32e9800fb 33 statePostPriori(observation[0], variance[0], angle[0], predictions[1], predictions[0]);
KDrainEE 1:cbd32e9800fb 34 statePostPriori(observation[0], variance[0], angle[0], this->covariance, this->mean);
KDrainEE 1:cbd32e9800fb 35 return this->mean;
KDrainEE 1:cbd32e9800fb 36 }
KDrainEE 1:cbd32e9800fb 37
KDrainEE 1:cbd32e9800fb 38
KDrainEE 1:cbd32e9800fb 39