卡尔曼

gaoxings

卡尔曼
8 s) k& ^! P1 F5 |2 x; W
clear clc;
N=600;%采样点的个数  
7 U; ], Z; G$ Z1 X1 B) cCON=25;%室内温度的理论值
x=zeros(1,N);%用来记录温度的最优化估算值  
y=randn(1,N)+CON;%温度计的观测值，其中叠加了噪声
% p# j. l9 n" x0 _/ Ex(1)=20;%为x(k)赋初值
p(1)=2;%x(1)对应的协方差
$ s& |4 l% s# o& ]. G3 I7 MQ=cov(randn(1,N));%过程噪声的协方差
9 \( R, [$ D  ^. ZR=cov(randn(1,N));%测量噪声的协方差
) `% i% r0 \$ Y% Vfor k=2:N%循环里面是卡尔曼滤波的具体过程   
: m) T$ c  |- m7 ~) D9 R/ wx(k)=x(k-1);   
$ G8 C" t, W0 m3 K1 }8 Y. b0 Tp(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);
+ y' b4 ~0 {, u& i9 Vend  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %这个模块起到平滑滤波作用
Filter_Width=10;%滤波器带宽
Smooth_Result=zeros(1,N);%用来存放滤波后的各个采样点的值
5 X( }6 O/ x7 Z/ N! x9 v3 yfor i=Filter_Width+1:N     
) p$ C% s9 v* z7 G, {5 RTemp_Sum=0;      
for j=i-Filter_Width: (i-1)      
Temp_Sum=x(j)+Temp_Sum;   
end      
Smooth_Result(i)=Temp_Sum/Filter_Width;%每一个点的采样值等于这个点之前的filter_width长度的采样点的平均值
end  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
t=1:N;  
figure('Name','Kalman Filter Simulation','NumberTitle','off');
expected_Value=zeros(1,N);
for i=1:N      
expected_Value(i)=CON;
7 v: x5 w! U- U, ~end
* ^/ x$ s) q4 Z$ p) j. u3 l8 z' oplot(t,expected_Value,'-b',t,y,'-g',t,x,'-k',t,Smooth_Result,'-m');%依次输出理论值，叠加测量噪声的温度计测量值，
legend('expected','measure','estimate','smooth result');           %经过kalman滤波后的最优化估算值，平滑滤波后的输出值
* x! n& D! }6 s: I, o' xxlabel('sample time');
ylabel('temperature');  title('Kalman Filter Simulation');

lilino

程序给力！