卡尔曼

gaoxings

卡尔曼

% ?. f& K4 o4 `! i" |7 Z1 \clear clc;
! D, ~3 n0 Y7 L. |N=600;%采样点的个数  
CON=25;%室内温度的理论值
3 H( n3 N/ C# I6 D- `x=zeros(1,N);%用来记录温度的最优化估算值  
% f# S2 f. T( r: Zy=randn(1,N)+CON;%温度计的观测值，其中叠加了噪声
- Q& I8 e% F$ |! B7 ?! r) }x(1)=20;%为x(k)赋初值
- R0 ^4 y  w9 E2 q2 B9 ]5 qp(1)=2;%x(1)对应的协方差
; D8 ~. `, \5 Z9 |+ O; bQ=cov(randn(1,N));%过程噪声的协方差
R=cov(randn(1,N));%测量噪声的协方差
for k=2:N%循环里面是卡尔曼滤波的具体过程   
x(k)=x(k-1);   
p(k)=p(k-1)+Q;     
Kg(k)=p(k)/(p(k)+R);%Kg为Kalman Gain,卡尔曼增益     
. e6 H) Z% Q  \) q6 v5 B  yx(k)=x(k)+Kg(k)*(y(k)-x(k));     
p(k)=(1-Kg(k))*p(k);
end  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %这个模块起到平滑滤波作用
5 R9 B2 y! b+ m9 z. k- AFilter_Width=10;%滤波器带宽
Smooth_Result=zeros(1,N);%用来存放滤波后的各个采样点的值
5 a5 E+ s( a/ |$ Yfor i=Filter_Width+1:N     
4 B: S1 s- ~, h. y7 \Temp_Sum=0;      
for j=i-Filter_Width: (i-1)      
Temp_Sum=x(j)+Temp_Sum;   
- x  e! D/ w3 I* aend      
% X, k/ ^" O9 X8 u* ^Smooth_Result(i)=Temp_Sum/Filter_Width;%每一个点的采样值等于这个点之前的filter_width长度的采样点的平均值
# o1 l3 R# W) dend  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
: I$ H' s7 s2 H- ^6 B+ E% p( u* vt=1:N;  
& \) P( S2 I2 B% hfigure('Name','Kalman Filter Simulation','NumberTitle','off');
* l$ ~0 b- l* ]2 H$ t( z7 nexpected_Value=zeros(1,N);
for i=1:N      
% V; v7 C% L' O' Kexpected_Value(i)=CON;
end
" E+ n5 ^0 U% Y8 @4 s9 @plot(t,expected_Value,'-b',t,y,'-g',t,x,'-k',t,Smooth_Result,'-m');%依次输出理论值，叠加测量噪声的温度计测量值，
legend('expected','measure','estimate','smooth result');           %经过kalman滤波后的最优化估算值，平滑滤波后的输出值
" ?1 C' y9 g/ E7 a: s3 f2 Rxlabel('sample time');
ylabel('temperature');  title('Kalman Filter Simulation');

lilino

程序给力！