卡尔曼滤波程序
// Kalman Filter
float EstimateErrorLast;
float EstimateValueLast;
void KalmanFilterInit(void)
{
EstimateValueLast=80;
EstimateErrorLast=10;
}
int KalmanFilter(int MeasureValue)
{
float MeasureError;
float EstimateValue;
float EstimateError;
float KalmanGain;
int KalmanFilterData;
MeasureValue = MeasureValue;
MeasureError=MeasureValue*0.2;
KalmanGain = EstimateErrorLast/(EstimateErrorLast + MeasureError);
EstimateValue = EstimateValueLast + KalmanGain*(MeasureValue-EstimateValueLast);
EstimateError = (1-KalmanGain)*EstimateErrorLast;
EstimateValueLast = EstimateValue;
EstimateErrorLast = EstimateError;
KalmanFilterData = (int)EstimateValue;
return KalmanFilterData;
}
MATLAB仿真
function main
N = 300;
Realillumin = 82;
MeasureVaration = 0.2;
Measuremin = Realillumin*(1 - MeasureVaration);
Measuremax = Realillumin*(1 + MeasureVaration);
Real_illumin = Realillumin*ones(1,N);
Measure_Value = Measuremin + (Measuremax-Measuremin)*rand(1,N);
Estimate_Value = zeros(1,N);
Measure_error = zeros(1,N);
Estimate_error = zeros(1,N);
KalmanGain = zeros(1,N);
SumValue = zeros(1,N);
MeanValue = zeros(1,N);
Estimate_Value(1) = Measure_Value(1);
Measure_error(1) = Measure_Value(1)*MeasureVaration;
Estimate_error(1) = Measure_Value(1)*MeasureVaration;
for k=2:N
Measure_error(k) = Measure_Value(k)*MeasureVaration;
KalmanGain(k)= Estimate_error(k-1)/(Estimate_error(k-1)+Measure_error(k));
Estimate_error(k) = (1-KalmanGain(k))*Estimate_error(k-1);
Estimate_Value(k) = Estimate_Value(k-1) + KalmanGain(k)*(Measure_Value(k)-Estimate_Value(k-1));
end
SumValue(1)=Measure_Value(1);
MeanValue(1)=SumValue(1)/1;
for k=2:N
SumValue(k) = SumValue(k-1)+Measure_Value(k);
MeanValue(k) = SumValue(k)/k;
end
t=1:N;
figure('Name','Kalman Filter Simulation','NumberTitle','off');
plot(t,Measure_Value,'-b',t,Estimate_Value,'-r',t,MeanValue,'-k',t,Real_illumin,'-g');
legend('MeasureValue','EstimateValue','MeanValue','Realillumin');
xlabel('sample time');
ylabel('illumin');
title('Kalman Filter Simulation');