卡尔曼

gaoxings

卡尔曼
2 z1 v: t; H# A. e$ |" f% L
clear clc;
N=600;%采样点的个数  
# x- ^  I8 G$ o$ D6 U+ Y9 i/ x! g8 VCON=25;%室内温度的理论值
x=zeros(1,N);%用来记录温度的最优化估算值  
6 M5 V0 J9 O4 K; d! F. by=randn(1,N)+CON;%温度计的观测值，其中叠加了噪声
x(1)=20;%为x(k)赋初值
p(1)=2;%x(1)对应的协方差
8 w7 Y, d, z7 ]' _Q=cov(randn(1,N));%过程噪声的协方差
R=cov(randn(1,N));%测量噪声的协方差
for k=2:N%循环里面是卡尔曼滤波的具体过程   
3 S! G+ ~& Q& N( \x(k)=x(k-1);   
3 `: ?3 _( J3 H- f% Rp(k)=p(k-1)+Q;     
( p$ y/ k/ @$ {/ l+ V, HKg(k)=p(k)/(p(k)+R);%Kg为Kalman Gain,卡尔曼增益     
1 }) q& y7 B( S( e5 fx(k)=x(k)+Kg(k)*(y(k)-x(k));     
! f, G; j- {3 l0 `( wp(k)=(1-Kg(k))*p(k);
$ _, W9 Q: z5 r" u0 D3 Vend  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %这个模块起到平滑滤波作用
Filter_Width=10;%滤波器带宽
: X2 X, ?/ O3 X$ [Smooth_Result=zeros(1,N);%用来存放滤波后的各个采样点的值
for i=Filter_Width+1:N     
% P5 ^( b. p1 E  eTemp_Sum=0;      
for j=i-Filter_Width: (i-1)      
  p  n& d1 p( ]  W9 q) RTemp_Sum=x(j)+Temp_Sum;   
# B7 ~: W3 V& \2 f) Iend      
3 Z3 i/ V: O/ USmooth_Result(i)=Temp_Sum/Filter_Width;%每一个点的采样值等于这个点之前的filter_width长度的采样点的平均值
3 ^5 n: v1 ~1 U/ |# _end  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
9 Q6 R8 z0 x% F2 Q+ ^0 c, ft=1:N;  
figure('Name','Kalman Filter Simulation','NumberTitle','off');
expected_Value=zeros(1,N);
7 ]6 `* d4 ^" \: ifor i=1:N      
expected_Value(i)=CON;
# J* Y" V) S  x) L/ bend
9 L5 l/ |1 h# {; ~: u8 [/ O4 Rplot(t,expected_Value,'-b',t,y,'-g',t,x,'-k',t,Smooth_Result,'-m');%依次输出理论值，叠加测量噪声的温度计测量值，
legend('expected','measure','estimate','smooth result');           %经过kalman滤波后的最优化估算值，平滑滤波后的输出值
xlabel('sample time');
ylabel('temperature');  title('Kalman Filter Simulation');

lilino

程序给力！