kalman.h

来自「this the code of bootstation」· C头文件 代码 · 共 59 行

#ifndef _Kalman_h_
#define _Kalman_h_

#include "global.h"

//Parameter
//-----------------------------------------------------------------------------------------
float Gain_vorne	= 2;	//Kalmangain f黵 die Ultraschallmessungen
float Gain_hinten	= 2;	//
float Gain_oben		= 1;	//
float Gain_unten	= 1;	//
float Gain_rechts	= 3;	//
float Gain_links	= 3;	//
float Q				= 1;	//Systemrauschen einfacher Kalman_Filter (Ultraschallmessung)

float Q_v			= 0.01;	//Modellvarianz Kalman_Datenfusion Geschwindigkeit
float P_v[1]		= {1};	//Fehler-Kovarianz bei Kalman_Datenfusion Geschwindigkeit

float Q_Winkel		= 0.001;	//Modellvarianz Winkelberechnung (GyroIntegration)
float P_Winkel[1]	= {1};	//Fehler-Kovarianz Kalman_Datenfusion Winkelkorrektur

float Kovar_Richtung = 10;	//Kovarianz f黵 die Messwertfusion Richtung

float Kovar_Cam[1] = {1};	//Kovarianz f黵 die Messwertfusion Richtung
float KalmanFixBlob[5]	= {0,0,0,0,0};	//Kalmanfusioniert FixBlob
//-----------------------------------------------------------------------------------------

//temp Variablen
float P_min_vorne[1]	= {1};
float P_min_hinten[1] 	= {1};
float P_min_oben[1]		= {1};
float P_min_unten[1]	= {1};
float P_min_rechts[1] 	= {1};
float P_min_links[1]	= {1};


//Funktionen
//-----------------------------------------------------------------------------------------
//Einfacher Kalmanfilter (Messwertbewertung)
void Kalman_Filter(float Daten[5], float Messwert, float Gain, float P_min[1]);

//Kalman-Datenfusion
//躡ergabewerte: Ergebnis der Fusion[0], Modelldynamik, Messwert, 
//Messwert g黮tig (G),  Fehlervarianz, Rauschen Modell
void Kalman_Datenfusion(float Ergebnis[5], float ModellDynamik, float Messwert, 
						bit G, float P[1], float Q_Modell);
//Datenfusion von Sensormesswerten und Sensormessdynamik
//鋒nlich wie Datenfusion
//躡ergabewerte: Ergebnis der Fusion [0], Messwert, Messdynamik, 
//Rauschen Messwerte, Rauschen Messdynamik, Fehlerkovarianz
//Ergibnis muss ein 5er Feld sein
void Kalman_Messwertfusion(float* Ergebnis,  float Messwert, float Messdynamik, 
						float Q_Messwert, float Q_Messdynamik, float& P);
//-----------------------------------------------------------------------------------------


#endif