kopia lustrzana https://github.com/DanInvents/Rockit
21 wiersze
1.1 KiB
C++
21 wiersze
1.1 KiB
C++
// This program performs a Kalman filter of the flight data. It smoothens the data and ignores transitory events.
|
|
|
|
// Q = process noise covariance
|
|
// R = measurement noise covariance. Larger R means large measurement uncertainty. Larger Q means larger estimation uncertainty. Thus increasing Q corrects more.
|
|
// Xpe0 = prior estimation of signal X at time t=0 (current state)
|
|
// Xe1 = estimation of X at time t=1 (previous state)
|
|
// Ppe0 = prior estimation of "error covariance" at t=0,
|
|
// P1 = error covariance at t=1, P0 = error covariance at t=0
|
|
// K = Kalman gain, Xe0 = estimation of signal at t=0, Z = measured signal at t=0;
|
|
|
|
float kalmanFilter(float Z){
|
|
Xpe0 = Xe1; // Assumption of prediction 1
|
|
Ppe0 = P1 + Q; // Update of prior estimation of "error covariance"
|
|
K = Ppe0/(Ppe0 + R); // Measurement update or correction of "Kalman gain"
|
|
Xe0 = Xpe0 + K * (Z - Xpe0); // Measurement update or correction of "estimated signal"
|
|
P0 = (1 - K) * Ppe0; // Measurement update or correction of "error covariance";
|
|
Xe1 = Xe0;
|
|
P1 = P0;
|
|
return Xe0;
|
|
}
|