卡尔曼

gaoxings

卡尔曼
/ @! W( F: d4 r$ [
/ c" Q7 o8 I7 Oclear clc;
6 f9 L7 u8 l- w! Y/ ]N=600;%采样点的个数  
CON=25;%室内温度的理论值
. t; y6 w/ ?% |+ l9 }x=zeros(1,N);%用来记录温度的最优化估算值  
y=randn(1,N)+CON;%温度计的观测值，其中叠加了噪声
x(1)=20;%为x(k)赋初值
3 X7 _0 ?1 r$ K+ P3 Ip(1)=2;%x(1)对应的协方差
Q=cov(randn(1,N));%过程噪声的协方差
R=cov(randn(1,N));%测量噪声的协方差
for k=2:N%循环里面是卡尔曼滤波的具体过程   
4 E1 M0 A3 ~  nx(k)=x(k-1);   
p(k)=p(k-1)+Q;     
Kg(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  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %这个模块起到平滑滤波作用
% b7 v7 o- O/ j9 \  a' h. FFilter_Width=10;%滤波器带宽
Smooth_Result=zeros(1,N);%用来存放滤波后的各个采样点的值
. q2 \- }! R* z, U; j% rfor i=Filter_Width+1:N     
3 j. x$ y& e* I! DTemp_Sum=0;      
for j=i-Filter_Width: (i-1)      
$ Q2 Z/ r( \& E5 K' o* [; w5 DTemp_Sum=x(j)+Temp_Sum;   
end      
0 |2 a: ?% w1 e, nSmooth_Result(i)=Temp_Sum/Filter_Width;%每一个点的采样值等于这个点之前的filter_width长度的采样点的平均值
end  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
2 Y3 ]8 s0 n& f. s& t: r! S9 ~* Ht=1:N;  
) n# m/ ~% r$ L3 kfigure('Name','Kalman Filter Simulation','NumberTitle','off');
expected_Value=zeros(1,N);
! Z1 a7 r1 C. w* R4 Z7 h) pfor i=1:N      
expected_Value(i)=CON;
end
5 G0 E* q* {/ ]' @9 {% f0 c# c% Iplot(t,expected_Value,'-b',t,y,'-g',t,x,'-k',t,Smooth_Result,'-m');%依次输出理论值，叠加测量噪声的温度计测量值，
& Z' z' A; z8 {% y- C+ h& xlegend('expected','measure','estimate','smooth result');           %经过kalman滤波后的最优化估算值，平滑滤波后的输出值
xlabel('sample time');
& x% H7 Y- a; v# y, {5 uylabel('temperature');  title('Kalman Filter Simulation');

lilino

程序给力！