Quadrocopter
/
2014_12_12_quadro_01
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
Fork of
2014_12_12_quadro_01
by 
Szewc Michał
Diff: KalmanFilter.h
- Revision:
- 8:7a22b8294c5d
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/KalmanFilter.h Mon Dec 08 13:21:33 2014 +0000
@@ -0,0 +1,34 @@
+#ifndef KALMANFILTER_H
+#define KALMANFILTER_H
+
+#include <stdlib.h>
+#include <math.h>
+
+class KalmanFilter
+{
+ public:
+ KalmanFilter();
+ ~KalmanFilter(void);
+
+ void reset();
+ float update(float gyroRaw, float accelAngle);
+ float getEstimatedData();
+
+ private:
+ float A[2][2];
+ float B[2][1];
+ float C[1][2];
+ float Sz[1][1];
+ float Sw[2][2];
+
+ float xhat[2][1];
+ float P[2][2];
+
+ void matrix_multiply(float* A, float* B, int m, int p, int n, float* C);
+ void matrix_addition(float* A, float* B, int m, int n, float* C);
+ void matrix_subtraction(float* A, float* B, int m, int n, float* C);
+ void matrix_transpose(float* A, int m, int n, float* C);
+ int matrix_inversion(float* A, int n, float* AInverse);
+};
+
+#endif