00001 #ifndef KALMAN_H 00002 #define KALMAN_H 00003 00004 #include <inttypes.h> 00005 #include <mav_common/comm_packets.h> 00006 #include <mav_common/comm_util.h> 00007 #include <mav_common/comm_types.h> 00008 00009 #include "matrices.h" 00010 #include "LL_HL_comm.h" 00011 #include "sdk.h" 00012 #include "ssp.h" 00013 #include "uart.h" 00014 #include "comm_util_LL.h" 00015 00016 #define HEIGHT_PKT_TIMEOUT 200000 // 200 ms 00017 00018 typedef struct 00019 { 00020 float A[2][2]; 00021 float At[2][2]; 00022 float K[2][2]; 00023 float P[2][2]; 00024 float T[2][2]; 00025 float C[2][2]; 00026 float I[2][2]; 00027 float Q[2][2]; 00028 float R[2][2]; 00029 float Sigma2Q1; 00030 float Sigma2Q2; 00031 float Sigma2R1; 00032 float Sigma2R2; 00033 float B[2]; 00034 float Input; 00035 float State[2]; 00036 float Correction[2]; 00037 float Res[2]; 00038 } KalPos; 00039 00040 typedef struct 00041 { 00042 float K; 00043 float P; 00044 float T; 00045 float Q; 00046 float R; 00047 float Sigma2Q; 00048 float Sigma2R; 00049 float Input; 00050 float State; 00051 float Correction; 00052 } KalYaw; 00053 00054 typedef struct 00055 { 00056 float pos_filtered[3]; // Position 00057 float vel_filtered[3]; // linear velocity 00058 float yaw_filtered; //yaw angle 00059 } KalOut; 00060 00061 typedef struct 00062 { 00063 float sigma2Q1x; 00064 float sigma2Q2x; 00065 float sigma2R1x; 00066 float sigma2R2vx; 00067 00068 float sigma2Q1y; 00069 float sigma2Q2y; 00070 float sigma2R1y; 00071 float sigma2R2vy; 00072 00073 float sigma2Q1z; 00074 float sigma2Q2z; 00075 float sigma2R1z; 00076 float sigma2R2vz; 00077 00078 float sigma2Qyaw; 00079 float sigma2Ryaw; 00080 } Covariance; 00081 00082 KalPos kal_x, kal_y, kal_z; 00083 KalYaw kal_yaw; 00084 KalOut kal_out; 00085 Covariance covariance; 00086 00087 void KFilter(void); 00088 00089 void InitKalPos(KalPos *k); 00090 void PredictPos(KalPos *k, float dt); 00091 void CorrectPos(KalPos *k); 00092 00093 void InitKalYaw(KalYaw *k); 00094 void PredictYaw(KalYaw *k, float dt); 00095 void CorrectYaw(KalYaw *k); 00096 00097 void resetKalmanFilter(void); 00098 00099 #endif // KALMAN_H 00100