卡尔曼滤波房间温度例子mathlab源代码,带注释

卡尔曼滤波房间温度例子mathlab源代码,带注释 clear N=200; %w(1)=0; %w=randn(1,N); w=0; %系统控制矩阵 x(1)=0; a=1; %温度模拟A为1 V=randn(1,N); q1=std(V); %Rvv=q1.^2; Rvv=0.1; %测量过程协方差 温度模拟R为1e-1 q2=std(x); Rxx=q2.^2; q3=std(w); %Rww=q3.^2; Rww = 0.000001; %温度模拟Q为1e-6 %c=0.6; c=1; %温度模拟H为1 for k=1:N; Y(k)=25+sqrt(0.1)*randn(1); %温度模拟平均温度为25度 方差—————————————————————————————————————————————————— ——— (协方差)为0.1的温度输入 测量方程，其中V为测量系统的噪声,c为测量系统的参数 end p(1)=10; %协方差 初始值 s(1)=1; %最优估计 初始值 for t=2:N; s(t)=a*s(t-1)+w; %先验估计 求当前时刻的估计值 p1(t)=a.^2*p(t-1)+Rww; %协方差估计 求当前时刻的估计值的偏差，a为系统参数，没有控制量，所以没有参数b，Rww为噪声 b(t)=c*p1(t)/(c.^2*p1(t)+Rvv); %求Kg,b(t)为Kg，即Kalman增量 s(t)=a*s(t)+b(t)*(Y(t)-a*c*s(t));%后验估计 求t时刻的最优值，即当前时刻的最优值 p(t)=p1(t)-c*b(t)*p1(t); %后验协方差 求当前最状态最优值的偏差，即式子5:(1-c*b(t))*p1(t) end figure(1); plot(Y,'g--');hold on; plot(s,'r--');hold on; —————————————————————————————————————————————————————