Enkf 卡尔曼程序

对于初学卡尔曼者有借鉴作用，结合程序可以更好的理解集合卡尔曼理论

clc;
clear;
 
N=200;
f=50;
ts=1/10000;
t=0:0.02/N:0.2;
w=2*pi*f;

for i=1:1:length（t）
    if t（i）>0.0803 && t（i）<0.1203
        E（i）=0.6;
    else E（i）=1;
    end
    y（i）=E（i）*sin（w*t（i））;
end
figure（1）;
plot（t*10000y）;        %原始电压
xlabel（‘采样点‘）;     

x0=[E（1）*cos（w*ts）;E（1）*sin（w*ts）];
A=[cos（w*ts） -sin（w*ts） ; 
   sin（w*ts） cos（w*ts） ] ;  % 状态转移矩阵  
 H=[0 1];             % 测量转移矩阵 
for k=1:1:length（t）
    z（k）=y（k）;
end

xa=zeros（21length（t））;
xb=zeros（21length（t））; 
xa（::1）=x0;
xa（::2）=x0;
xb（::1）=x0;
xb_mean=zeros（11length（t））;  


% P0=100*[1 1;1 1];    %为滤波误差协方差初始值
 P_xh=zeros（21length（t））;
 P_hh=zeros（11length（t））;
 %P_after（::1）=P0;
 
 R=0.05*norm（A）*norm（A）;   % 系统噪声方差
       % 测量噪声方差
     
 for k=2:1:length（t）-1
     xb（::k）=A*xa（::k）;   
    xb_mean（::k）=（xb（21k）+xb（11k））/2;
    p_xh（::k）=（xb（21k）-xb_mean（::k））*（H*xb（21k）-H*xb_mean（::k））‘+（xb（11k）-xb_mean（::k））*（H*xb（11k）-H*xb_mean（::k））‘;
    p_hh（::k）=（H*xb（21k）-H*xb_mean（::k））*（H*xb（21k）-H*xb_mean（::k））‘+（H*xb（11k）-H*xb_mean（::k-1））*（H*xb（11k）-H*xb_mean（::k））‘;
    Kk（::k）=p_xh（::k）*inv（p_hh（::k）+R）;
    xa（::k+1）=Kk（::k）*（z（k）-H*xb（::k））;
    Z（k）=H*xa（::k+1）;
 end

 for j=1:1:length（t）-1
     Vpp（j）=sqrt（xa（21j）^2+xa（11j）^2）;
%     phase（j）=（atan（xa（21j）/xa（11j）））/（2*pi）*360;
 end
 
 figure（2）
plot（Z‘g‘）;
hold on;
plot（z‘r‘）;
ylim（[-1.51.5]）;
figure（3）
plot（Vpp）;

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       1549  2014-04-16 13:00  Enkf.m

----------- ---------  ---------- -----  ----

                 1549                    1