卡尔曼

gaoxings

卡尔曼

clear clc;
+ Q! b- W, T. R% e5 i$ CN=600;%采样点的个数  
CON=25;%室内温度的理论值
% z; t* e/ k- Z; m7 N$ Dx=zeros(1,N);%用来记录温度的最优化估算值  
y=randn(1,N)+CON;%温度计的观测值，其中叠加了噪声
x(1)=20;%为x(k)赋初值
p(1)=2;%x(1)对应的协方差
Q=cov(randn(1,N));%过程噪声的协方差
6 g8 E0 x& {8 j9 K) A6 \1 `4 w; cR=cov(randn(1,N));%测量噪声的协方差
for k=2:N%循环里面是卡尔曼滤波的具体过程   
x(k)=x(k-1);   
" c6 I9 C5 k' f" F. vp(k)=p(k-1)+Q;     
Kg(k)=p(k)/(p(k)+R);%Kg为Kalman Gain,卡尔曼增益     
9 K: \- M+ G9 @x(k)=x(k)+Kg(k)*(y(k)-x(k));     
p(k)=(1-Kg(k))*p(k);
end  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %这个模块起到平滑滤波作用
+ t) {, i3 G5 `1 J5 |1 A$ I0 p! QFilter_Width=10;%滤波器带宽
Smooth_Result=zeros(1,N);%用来存放滤波后的各个采样点的值
) R' U4 D- d. H. d9 i! u6 {+ bfor i=Filter_Width+1:N     
Temp_Sum=0;      
% A+ m$ k+ i6 Z; v' Z: n1 ^& tfor j=i-Filter_Width: (i-1)      
+ p/ o, {, k4 x& Z) d& O+ q, C' tTemp_Sum=x(j)+Temp_Sum;   
end      
Smooth_Result(i)=Temp_Sum/Filter_Width;%每一个点的采样值等于这个点之前的filter_width长度的采样点的平均值
$ E2 p# @  E. @/ n% \. R$ b" Pend  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
t=1:N;  
' `' d1 L2 z# f" qfigure('Name','Kalman Filter Simulation','NumberTitle','off');
- k5 c$ ?7 q: ^  Mexpected_Value=zeros(1,N);
for i=1:N      
expected_Value(i)=CON;
end
2 U2 X6 x) ?+ B5 g" ]1 Z9 [plot(t,expected_Value,'-b',t,y,'-g',t,x,'-k',t,Smooth_Result,'-m');%依次输出理论值，叠加测量噪声的温度计测量值，
" j* i) z( g5 s4 R. ]legend('expected','measure','estimate','smooth result');           %经过kalman滤波后的最优化估算值，平滑滤波后的输出值
xlabel('sample time');
7 n! v' i+ r+ {; Y* Jylabel('temperature');  title('Kalman Filter Simulation');

lilino

程序给力！