kalman滤波matlab代码demo

kalman滤波其实是一个自回归，自我调整的预测系统，对有白噪声干扰的信号有很好的去除效果

网上找了些maltab代码，修改了一下

帮助自己理解：

clear
clc;
N = 600;
t = 1:N;
CON = 25;
%x = zeros(1,N);
w = randn(1,N);
x(1) = 0;
for i = 2:N
    x(i) = x(i-1)+w(i-1);
end
subplot(311);
plot(t,x);
v = randn(1,N);
q1 = std(v);
q2 = std(w);
c = 0.2;
y = c*x+v;
% im = imread('lake.bmp');
% [z j] = size(im);
% y = im;
% x(1) = 20;
p(1) = 2;
Q = q2.^2;
R = q1.^2;
% x = zeros(512,512);
% x(1,1) = y(1,1);
c = 0.2
for k = 2:N
    x(k) = x(k-1);
    p(k) = p(k-1)+Q;
    Kg(k) = c*p(k)/(c^2*p(k)+R);
    x(k) = x(k)+Kg(k)*(y(k)-c*x(k));
    p(k) = (1-c*Kg(k))*p(k);
    k
end

Filter_Width = 10;
Smooth_Result = zeros(1,N);
for i = Filter_Width+1:N
    Temp_Sum = 0;
    for j = i-Filter_Width:(i-1)
        Temp_Sum = x(j)+Temp_Sum;
    end
    Smooth_Result(i) = Temp_Sum/Filter_Width;
end

t = 1:N;
% figure('Name','Kalman Filter Simulation','NumberTitle','off');
expected_Value = zeros(1,N);
for i = i:N
    expected_Value(i) = CON;
end
t = 1:N;
subplot(312);
plot(t,y);
subplot(313);
plot(t,x);
% plot(t,expected_Value,'-b',t,y,'-g',t,x,'-k',t,Smooth_Result,'-m');
% legend('expected','measure','estimate','smooth_result');
% xlabel('sample time');
% ylabel('temperature');
% title('Kalman Filter Simulation');