卡尔曼

gaoxings

卡尔曼

) j4 {2 p  K. Y0 S' p% wclear clc;
4 [3 V7 k" d- I1 y" G+ U) VN=600;%采样点的个数  
CON=25;%室内温度的理论值
6 S% U, |; h4 N! o, L) }% k: ]x=zeros(1,N);%用来记录温度的最优化估算值  
y=randn(1,N)+CON;%温度计的观测值，其中叠加了噪声
x(1)=20;%为x(k)赋初值
! g" ?! j2 Z8 r1 u& [p(1)=2;%x(1)对应的协方差
4 V' C, W4 W: `7 l6 {5 x' oQ=cov(randn(1,N));%过程噪声的协方差
R=cov(randn(1,N));%测量噪声的协方差
for k=2:N%循环里面是卡尔曼滤波的具体过程   
x(k)=x(k-1);   
, U2 i4 u2 [1 ~  q) y. r$ d' Xp(k)=p(k-1)+Q;     
Kg(k)=p(k)/(p(k)+R);%Kg为Kalman Gain,卡尔曼增益     
: _9 K, }6 u  M& R5 F0 `x(k)=x(k)+Kg(k)*(y(k)-x(k));     
p(k)=(1-Kg(k))*p(k);
2 ]* ~5 o$ d0 f: n! _/ Uend  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %这个模块起到平滑滤波作用
8 @2 A: ^0 v! U$ Z  W6 pFilter_Width=10;%滤波器带宽
Smooth_Result=zeros(1,N);%用来存放滤波后的各个采样点的值
8 I$ y! E( }3 e0 E3 a; _for i=Filter_Width+1:N     
Temp_Sum=0;      
for j=i-Filter_Width: (i-1)      
% c; ?& z  Z, z  `$ o7 R" N  OTemp_Sum=x(j)+Temp_Sum;   
end      
+ e4 U+ U# Z: q, Y& Z  [( `% J' A9 VSmooth_Result(i)=Temp_Sum/Filter_Width;%每一个点的采样值等于这个点之前的filter_width长度的采样点的平均值
5 z0 Q5 q: c- L/ [; C: J! Send  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
7 |$ B1 J0 |, ?0 Qt=1:N;  
4 P. l3 K: w; [) dfigure('Name','Kalman Filter Simulation','NumberTitle','off');
! v/ x  v- d8 B7 W7 }& B! t( g( C! k- ]expected_Value=zeros(1,N);
" q, J! p0 q2 K$ @for i=1:N      
5 @! d7 }* n5 K) Lexpected_Value(i)=CON;
  o3 i7 S5 ~2 S5 [8 W  _: Dend
plot(t,expected_Value,'-b',t,y,'-g',t,x,'-k',t,Smooth_Result,'-m');%依次输出理论值，叠加测量噪声的温度计测量值，
legend('expected','measure','estimate','smooth result');           %经过kalman滤波后的最优化估算值，平滑滤波后的输出值
% m8 o; _" Y, mxlabel('sample time');
ylabel('temperature');  title('Kalman Filter Simulation');

lilino

程序给力！