卡尔曼

gaoxings

卡尔曼

1 ~, P1 d! L; Z1 i* aclear clc;
( d, l2 d4 K( c" X4 EN=600;%采样点的个数  
6 |1 A/ \% M! i8 D5 L* E/ v- c  M+ fCON=25;%室内温度的理论值
x=zeros(1,N);%用来记录温度的最优化估算值  
y=randn(1,N)+CON;%温度计的观测值，其中叠加了噪声
, A8 e" G  D4 {& s9 Q+ [x(1)=20;%为x(k)赋初值
0 @, P, v/ g4 F7 Q" X8 I) _p(1)=2;%x(1)对应的协方差
5 i1 `& c9 j# z" d: l4 ?9 UQ=cov(randn(1,N));%过程噪声的协方差
4 M% |) Z6 i, o+ UR=cov(randn(1,N));%测量噪声的协方差
/ q8 _% `$ [) dfor k=2:N%循环里面是卡尔曼滤波的具体过程   
0 c* T. F+ w* ]- Lx(k)=x(k-1);   
p(k)=p(k-1)+Q;     
4 I( H, l1 b+ iKg(k)=p(k)/(p(k)+R);%Kg为Kalman Gain,卡尔曼增益     
+ l9 f  l6 h9 v  F; X' ^- o! `x(k)=x(k)+Kg(k)*(y(k)-x(k));     
) Z, Z$ o  [$ H- gp(k)=(1-Kg(k))*p(k);
, N: z/ o" B. q* Z! d4 C. Mend  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %这个模块起到平滑滤波作用
Filter_Width=10;%滤波器带宽
1 a; @) D$ X7 C- X( zSmooth_Result=zeros(1,N);%用来存放滤波后的各个采样点的值
for i=Filter_Width+1:N     
4 b! z% W) }6 bTemp_Sum=0;      
for j=i-Filter_Width: (i-1)      
Temp_Sum=x(j)+Temp_Sum;   
end      
1 C8 I( L0 G6 D3 JSmooth_Result(i)=Temp_Sum/Filter_Width;%每一个点的采样值等于这个点之前的filter_width长度的采样点的平均值
: f  X( B( @2 L% H0 V# t4 v% o. J0 |end  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
t=1:N;  
figure('Name','Kalman Filter Simulation','NumberTitle','off');
3 `9 X# z) N: A. C* B5 a8 xexpected_Value=zeros(1,N);
9 u3 T0 r* ~" n% Cfor i=1:N      
9 [$ `/ E" q( @, n4 a) g. m0 Cexpected_Value(i)=CON;
4 J' Y, }- c7 d6 `/ @- B, d3 qend
) a  u$ _& {  _8 c+ f1 Q  Jplot(t,expected_Value,'-b',t,y,'-g',t,x,'-k',t,Smooth_Result,'-m');%依次输出理论值，叠加测量噪声的温度计测量值，
legend('expected','measure','estimate','smooth result');           %经过kalman滤波后的最优化估算值，平滑滤波后的输出值
xlabel('sample time');
2 D3 K0 S# e; Y: n  j  W; y1 Xylabel('temperature');  title('Kalman Filter Simulation');

lilino

程序给力！