kalman.cpp

this the code of bootstation

#include "stdafx.h"		//in MFC einkommentieren, sonst raus!

#include "Kalman.h"
#include "global.h"


//躡ergabewerte: Ergebnis, Messwert, Kalmangain (K), Fehlervarianz
void Kalman_Filter(float Daten[5], float Messwert, float Gain, float P_min[1])
{
	float K;
	float P;

	if (Daten[0] == 0)
	{
		P_min[0] = 0;
		K = P_min[0]/(P_min[0] + Gain);
		Daten[0] = Messwert;		
		P = (1 - K) * P_min[0];
		P_min[0] = P + Q;
	}
	else
	{
		Daten[4] = Daten[3];
		Daten[3] = Daten[2];
		Daten[2] = Daten[1];
		Daten[1] = Daten[0];

		K = P_min[0]/(P_min[0] + Gain);
		Daten[0] = Daten[1] + K*(Messwert - Daten[1]);
		P = (1 - K)*P_min[0];
		P_min[0] = P + Q;
	}
}


//Datenfusion von Sensormesswerten und Systemmodell
//躡ergabewerte: Ergebnis der Fusion[0], Modelldynamik, Messwert, Messwert g黮tig,  Fehlervarianz, Rauschen Modell
void Kalman_Datenfusion(float Ergebnis[5],  float ModellDynamik, float Messwert, 
						bit G, float P[1], float Q_Modell)
{

	float P_pre;		//gesch鋞zte Kovarianz
	float K;			//Kalmangain
	float R;			//Varianz des Messwertes
	float Ergebnis_pre;//gesch鋞zes Ergebnis
	float temp;

	Ergebnis[4] = Ergebnis[3];
	Ergebnis[3] = Ergebnis[2];
	Ergebnis[2] = Ergebnis[1];
	Ergebnis[1] = Ergebnis[0];

	//Vorraussagen des neuen Wertes und sch鋞zen des Fehlers mit Modellvarianz Q
	Ergebnis_pre = Ergebnis[1] + ModellDynamik;	//sch鋞zen des neuen Ergebnisses (Vohersage (Xn-))
	P_pre = P[0] + Q_Modell;			//sch鋞zen der Kovarianz

	//Vorraussage  durch bewerteten Messwert korrigieren,
	//wenn Messwert g黮tig
	if (G == true)
	{
		temp = (Ergebnis_pre - Messwert);	//Messwertabweichung vom Voraussagewert (Xn-)
		R = temp * temp;			//Varianz^2 des Messwertes
		K = P_pre/(P_pre + R);		//Kalmangain					
	}
	//kein Kalmangain (Messwertkorrektur), wenn Messweert nicht g黮tig
	else
	{
		K = 0;
	}
	//gefilteter (fusionierter) Datenwert
	Ergebnis[0] = Ergebnis_pre + K*(Messwert - Ergebnis_pre);
	
	//neue Fehlerkovarianz
	P[0] = (1 - K)*P_pre;
}			
//--------------------------------------------------------------------------------
//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)
{

	float P_pre;		//gesch鋞zte Kovarianz
	float K;			//Kalmangain
//	float R;			//Varianz des Messwertes
	float Ergebnis_pre; //gesch鋞zes Ergebnis

	DatenShift(Ergebnis, 5);
		
	//Vorraussagen des neuen Wertes und sch鋞zen des Fehlers mit Modellvarianz Q
	Ergebnis_pre = Ergebnis[1] + Messdynamik;	//sch鋞zen des neuen Ergebnisses (Vohersage (Xn-))
	P_pre = P + Q_Messdynamik;					//sch鋞zen der Kovarianz

	//Vorraussage  durch bewerteten Messwert korrigieren

	K = P_pre/(P_pre + Q_Messwert);		//Kalmangain					


	//gefilteter (fusionierter) Datenwert
	Ergebnis[0] = Ergebnis_pre + K*(Messwert - Ergebnis_pre);
	
	//neue Fehlerkovarianz
	P = (1 - K)*P_pre;
}