基于matlab仿真对卡尔曼滤波的理解

clear
clc;
N = 200;           
t = 1:N;
w = randn(1,N);          %噪声w
x(1) = 0;
for i = 2:N
x(i) = x(i-1)+w(i-1);    
end
subplot(311);
plot(t,x);             %输出带噪声函数
xlabel('原始数据');
v = randn(1,N);        %噪声v
c = 0.2;               %同样一个干扰系数
y = c*x+v;             %输出带噪声函数

q1 = std(v);          
q2 = std(w);
p(1) = 2;           %协方差初始化
Q = q2.^2;          %预测（过程）噪声方差
R = q1.^2;          %测量（观测）噪声方差

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);           %修正 协方差为下次计算
end
subplot(312);
plot(t,x);
xlabel('卡尔曼滤波结果');



Filter_Width = 10;                 %对x滑动滤波处理
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