卡尔曼

gaoxings

卡尔曼
, c/ r8 C3 z0 @1 Y' R: S2 Y
clear clc;
N=600;%采样点的个数  
3 T3 ~( |8 K$ BCON=25;%室内温度的理论值
/ q# u( G; C; H+ i2 sx=zeros(1,N);%用来记录温度的最优化估算值  
y=randn(1,N)+CON;%温度计的观测值，其中叠加了噪声
& j, e: O# U5 c6 s" L7 O& Wx(1)=20;%为x(k)赋初值
" A0 E' l+ G( a5 ~; I4 W4 ~- mp(1)=2;%x(1)对应的协方差
% F# ^( w2 X& `+ }, ]Q=cov(randn(1,N));%过程噪声的协方差
# Q1 T7 w3 R# `. RR=cov(randn(1,N));%测量噪声的协方差
; W* c+ c( \6 {. ^+ u4 Xfor k=2:N%循环里面是卡尔曼滤波的具体过程   
x(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));     
6 j  l) Y+ Q7 P) d- K% A, Pp(k)=(1-Kg(k))*p(k);
  O) U% `* A# [. J: Lend  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %这个模块起到平滑滤波作用
. ?/ [, B3 v) s4 z: \" x/ {  E/ FFilter_Width=10;%滤波器带宽
Smooth_Result=zeros(1,N);%用来存放滤波后的各个采样点的值
& S$ e: U: ]7 h  {  U/ lfor i=Filter_Width+1:N     
Temp_Sum=0;      
for j=i-Filter_Width: (i-1)      
! ^8 Z0 k0 K/ h( [* V2 B7 F8 e  K6 RTemp_Sum=x(j)+Temp_Sum;   
" p1 P. b& `/ N7 N: A3 Uend      
Smooth_Result(i)=Temp_Sum/Filter_Width;%每一个点的采样值等于这个点之前的filter_width长度的采样点的平均值
+ ]9 D: b! T* n- _* b- w$ eend  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
t=1:N;  
figure('Name','Kalman Filter Simulation','NumberTitle','off');
expected_Value=zeros(1,N);
1 h5 u, G3 w+ ^" m1 Efor i=1:N      
expected_Value(i)=CON;
. r9 x/ u9 _: w2 K5 Zend
) c" K5 \0 i- J* J, L  i1 j3 Qplot(t,expected_Value,'-b',t,y,'-g',t,x,'-k',t,Smooth_Result,'-m');%依次输出理论值，叠加测量噪声的温度计测量值，
$ w3 M0 T5 `2 {legend('expected','measure','estimate','smooth result');           %经过kalman滤波后的最优化估算值，平滑滤波后的输出值
! I9 i' X6 W' O. Fxlabel('sample time');
ylabel('temperature');  title('Kalman Filter Simulation');

lilino

程序给力！