卡尔曼

gaoxings

卡尔曼

clear clc;
N=600;%采样点的个数  
CON=25;%室内温度的理论值
x=zeros(1,N);%用来记录温度的最优化估算值  
y=randn(1,N)+CON;%温度计的观测值，其中叠加了噪声
' U7 N' w) q: @% C$ ax(1)=20;%为x(k)赋初值
, Y! b; P! |4 J' @p(1)=2;%x(1)对应的协方差
! U9 N) \  J% O) W6 r; G- gQ=cov(randn(1,N));%过程噪声的协方差
5 ~* b/ o! P0 @) u1 iR=cov(randn(1,N));%测量噪声的协方差
' ]0 m0 T# P6 E% {. W" dfor k=2:N%循环里面是卡尔曼滤波的具体过程   
3 F0 Q7 x, j+ F  i8 ]' j. yx(k)=x(k-1);   
2 N  m4 ]( k2 k9 Op(k)=p(k-1)+Q;     
+ ^& b1 m( ~' t9 q3 IKg(k)=p(k)/(p(k)+R);%Kg为Kalman Gain,卡尔曼增益     
x(k)=x(k)+Kg(k)*(y(k)-x(k));     
p(k)=(1-Kg(k))*p(k);
end  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %这个模块起到平滑滤波作用
Filter_Width=10;%滤波器带宽
Smooth_Result=zeros(1,N);%用来存放滤波后的各个采样点的值
/ U1 a+ x- Y) R9 H; D" ^for i=Filter_Width+1:N     
Temp_Sum=0;      
for j=i-Filter_Width: (i-1)      
" N3 K2 P$ x  }; n* T" nTemp_Sum=x(j)+Temp_Sum;   
end      
Smooth_Result(i)=Temp_Sum/Filter_Width;%每一个点的采样值等于这个点之前的filter_width长度的采样点的平均值
end  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
# ?2 V/ i/ X8 {! l: }1 ?# h3 l% pt=1:N;  
6 L: z# I. z1 x1 Lfigure('Name','Kalman Filter Simulation','NumberTitle','off');
7 S* ]9 j9 n% ]1 ^) l& U- m0 Hexpected_Value=zeros(1,N);
for i=1:N      
expected_Value(i)=CON;
. m0 o' Y2 c3 z4 vend
plot(t,expected_Value,'-b',t,y,'-g',t,x,'-k',t,Smooth_Result,'-m');%依次输出理论值，叠加测量噪声的温度计测量值，
9 R' I. ^8 O1 F  j8 {$ Llegend('expected','measure','estimate','smooth result');           %经过kalman滤波后的最优化估算值，平滑滤波后的输出值
xlabel('sample time');
) I. o, {: y/ w) nylabel('temperature');  title('Kalman Filter Simulation');

lilino

程序给力！