卡尔曼

gaoxings

卡尔曼
( z: d( p3 W- {. e* }& p
9 v0 P* h' F5 Y. w* Fclear clc;
$ k$ z3 u) V" |+ s& v% U8 G. q$ EN=600;%采样点的个数  
CON=25;%室内温度的理论值
9 l6 T# T) t$ S* k' P- H+ xx=zeros(1,N);%用来记录温度的最优化估算值  
, S- i+ G5 \0 q( n% Hy=randn(1,N)+CON;%温度计的观测值，其中叠加了噪声
x(1)=20;%为x(k)赋初值
+ Z0 W% J/ o; @p(1)=2;%x(1)对应的协方差
( I+ P6 o( r8 jQ=cov(randn(1,N));%过程噪声的协方差
R=cov(randn(1,N));%测量噪声的协方差
9 O$ t4 u* r8 `2 R8 ?/ D4 Tfor k=2:N%循环里面是卡尔曼滤波的具体过程   
; f1 _! V" o# f+ v+ C4 Qx(k)=x(k-1);   
p(k)=p(k-1)+Q;     
7 O6 I- z/ t. b% t' {: cKg(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  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %这个模块起到平滑滤波作用
Filter_Width=10;%滤波器带宽
; j0 q4 z/ {5 h4 ^8 E8 W- z) ^Smooth_Result=zeros(1,N);%用来存放滤波后的各个采样点的值
- I- z0 h1 A! L6 \; I9 r2 |for i=Filter_Width+1:N     
& f& j, k; f$ o) Z' V2 JTemp_Sum=0;      
for j=i-Filter_Width: (i-1)      
" I2 {8 N* x1 @8 J* o; P6 @) fTemp_Sum=x(j)+Temp_Sum;   
1 \% q8 \" b0 W( }" h9 a0 E+ |end      
9 _8 z) m9 G- T0 e2 LSmooth_Result(i)=Temp_Sum/Filter_Width;%每一个点的采样值等于这个点之前的filter_width长度的采样点的平均值
8 q6 V0 l3 G# kend  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
2 m5 S7 m5 M! F; Ot=1:N;  
4 c. K" k& b8 Y; W% ofigure('Name','Kalman Filter Simulation','NumberTitle','off');
expected_Value=zeros(1,N);
for i=1:N      
+ q& t: @' s( k$ q  sexpected_Value(i)=CON;
end
/ a0 }: l  _9 I" \1 d# Hplot(t,expected_Value,'-b',t,y,'-g',t,x,'-k',t,Smooth_Result,'-m');%依次输出理论值，叠加测量噪声的温度计测量值，
& J! |' _, q4 u  Ylegend('expected','measure','estimate','smooth result');           %经过kalman滤波后的最优化估算值，平滑滤波后的输出值
, @. V  b- b5 w5 `! C& G) Nxlabel('sample time');
ylabel('temperature');  title('Kalman Filter Simulation');

lilino

程序给力！