Shuto Naruse
Eurobot_2012_Secondary
Kalman/Kalman.h@0:fbfafa6bf5f9, 2012-04-20 (annotated)
- Committer:
- narshu
- Date:
- Fri Apr 20 21:32:24 2012 +0000
- Revision:
- 0:fbfafa6bf5f9
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| narshu | 0:fbfafa6bf5f9 | 1 | #include "rtos.h" |
| narshu | 0:fbfafa6bf5f9 | 2 | //#include "Matrix.h" |
| narshu | 0:fbfafa6bf5f9 | 3 | #include "motors.h" |
| narshu | 0:fbfafa6bf5f9 | 4 | #include "RFSRF05.h" |
| narshu | 0:fbfafa6bf5f9 | 5 | |
| narshu | 0:fbfafa6bf5f9 | 6 | #include <tvmet/Matrix.h> |
| narshu | 0:fbfafa6bf5f9 | 7 | #include <tvmet/Vector.h> |
| narshu | 0:fbfafa6bf5f9 | 8 | using namespace tvmet; |
| narshu | 0:fbfafa6bf5f9 | 9 | |
| narshu | 0:fbfafa6bf5f9 | 10 | |
| narshu | 0:fbfafa6bf5f9 | 11 | class Kalman { |
| narshu | 0:fbfafa6bf5f9 | 12 | public: |
| narshu | 0:fbfafa6bf5f9 | 13 | enum measurement_t {SONAR1 = 0, SONAR2, SONAR3, IR1, IR2, IR3}; |
| narshu | 0:fbfafa6bf5f9 | 14 | static const measurement_t maxmeasure = IR3; |
| narshu | 0:fbfafa6bf5f9 | 15 | |
| narshu | 0:fbfafa6bf5f9 | 16 | Kalman(Motors &motorsin); |
| narshu | 0:fbfafa6bf5f9 | 17 | |
| narshu | 0:fbfafa6bf5f9 | 18 | void predict(); |
| narshu | 0:fbfafa6bf5f9 | 19 | void runupdate(measurement_t type, float value, float variance); |
| narshu | 0:fbfafa6bf5f9 | 20 | |
| narshu | 0:fbfafa6bf5f9 | 21 | //State variables |
| narshu | 0:fbfafa6bf5f9 | 22 | Vector<float, 3> X; |
| narshu | 0:fbfafa6bf5f9 | 23 | Matrix<float, 3, 3> P; |
| narshu | 0:fbfafa6bf5f9 | 24 | Mutex statelock; |
| narshu | 0:fbfafa6bf5f9 | 25 | |
| narshu | 0:fbfafa6bf5f9 | 26 | float SonarMeasures[3]; |
| narshu | 0:fbfafa6bf5f9 | 27 | float IRMeasures[3]; |
| narshu | 0:fbfafa6bf5f9 | 28 | |
| narshu | 0:fbfafa6bf5f9 | 29 | private: |
| narshu | 0:fbfafa6bf5f9 | 30 | |
| narshu | 0:fbfafa6bf5f9 | 31 | //Matrix<float, 3, 3> Q; //perhaps calculate on the fly? dependant on speed etc? |
| narshu | 0:fbfafa6bf5f9 | 32 | |
| narshu | 0:fbfafa6bf5f9 | 33 | RFSRF05 sonararray; |
| narshu | 0:fbfafa6bf5f9 | 34 | Motors& motors; |
| narshu | 0:fbfafa6bf5f9 | 35 | |
| narshu | 0:fbfafa6bf5f9 | 36 | Thread predictthread; |
| narshu | 0:fbfafa6bf5f9 | 37 | void predictloop(); |
| narshu | 0:fbfafa6bf5f9 | 38 | static void predictloopwrapper(void const *argument){ ((Kalman*)argument)->predictloop(); } |
| narshu | 0:fbfafa6bf5f9 | 39 | RtosTimer predictticker; |
| narshu | 0:fbfafa6bf5f9 | 40 | |
| narshu | 0:fbfafa6bf5f9 | 41 | // Thread sonarthread; |
| narshu | 0:fbfafa6bf5f9 | 42 | // void sonarloop(); |
| narshu | 0:fbfafa6bf5f9 | 43 | // static void sonarloopwrapper(void const *argument){ ((Kalman*)argument)->sonarloop(); } |
| narshu | 0:fbfafa6bf5f9 | 44 | // RtosTimer sonarticker; |
| narshu | 0:fbfafa6bf5f9 | 45 | |
| narshu | 0:fbfafa6bf5f9 | 46 | struct measurmentdata{ |
| narshu | 0:fbfafa6bf5f9 | 47 | measurement_t mtype; |
| narshu | 0:fbfafa6bf5f9 | 48 | float value; |
| narshu | 0:fbfafa6bf5f9 | 49 | float variance; |
| narshu | 0:fbfafa6bf5f9 | 50 | } ; |
| narshu | 0:fbfafa6bf5f9 | 51 | |
| narshu | 0:fbfafa6bf5f9 | 52 | Mail <measurmentdata, 16> measureMQ; |
| narshu | 0:fbfafa6bf5f9 | 53 | |
| narshu | 0:fbfafa6bf5f9 | 54 | Thread updatethread; |
| narshu | 0:fbfafa6bf5f9 | 55 | void updateloop(); |
| narshu | 0:fbfafa6bf5f9 | 56 | static void updateloopwrapper(void const *argument){ ((Kalman*)argument)->updateloop(); } |
| narshu | 0:fbfafa6bf5f9 | 57 | |
| narshu | 0:fbfafa6bf5f9 | 58 | |
| narshu | 0:fbfafa6bf5f9 | 59 | }; |