yemian4.m

用递推最小二乘法

T=0.01;                                                                    %采样周期
m=10000;                                                               %估计次数
H=[1,0];                                                               %量测矩阵
A=[1,T;0,1];                                                          %状态转移矩阵
%B=[0,T];                                                               %噪声驱动阵
W=sqrt(0.01)*randn(1,m+1);                                     %状态噪声
V=sqrt(0.02)*randn(1,m+1);                                      %量测噪声
X=zeros(2,m+1);                                                      %存放状态估计值
X(1,1)=0.5;
X(2,1)=0.03;                                                         %初始状态
P=zeros(4,m+1);                                                      %存放一步预测均方误差阵
Pk=zeros(4,m+1);                                                     %存放估计均方误差阵
Pk(1,1)=10000;Pk(2,1)=0;Pk(3,1)=0;Pk(4,1)=10000;              %估计均方误差阵初值
K=zeros(2,m+1);                                                       %存放滤波增益阵
I=[1,0;0,1];
L=zeros(1,m+1);                                                       %存放液位高度
L(1)=0.5;
Q=[0.01*T*T*T/3,0.01*T*T/2;0.01*T*T/2,T*0.01];                     %状态噪声阵
z=[];j=[];p=[];x=[];                                             %中间过渡阵
for i=1:m
    if i<=1333;          %实际系统建模
        L(i+1)=0.5+0.03*i*T;
    elseif i>1333&i<=2933
        L(i+1)=0.9-0.05*(i*T-40/3);
    elseif i>2933&i<=5600
        L(i+1)=0.1+0.03*(i*T-88/3);
    elseif i>5600&&i<=7200
        L(i+1)=0.9-0.05*(i*T-56);
    elseif i>7200&i<=9883
       L(i+1)=0.1+0.03*(i*T-72);
   elseif i>9883&i<=10000
       L(i+1)=0.9-0.05*(i*T-72-80/3);%实际系统建模结束
    end
    z=A*[Pk(1,i),Pk(2,i);Pk(3,i),Pk(4,i)]*A'+Q;        %计算状态一步预测均方误差阵
    j=z*H'*inv(H*z*H'+0.02);                             %计算滤波增益阵
     K(1,i+1)=j(1,1); K(2,i+1)=j(2,1);
    p=(I-[K(1,i+1); K(2,i+1)]*H)*z*(I-[K(1,i+1); K(2,i+1)]*H)'+[K(1,i+1);K(2,i+1)]*0.02*[K(1,i+1);K(2,i+1)]';%计算估计均方误差阵
    Pk(1,i+1)=p(1,1);Pk(2,i+1)=p(1,2);Pk(3,i+1)=p(2,1);Pk(4,i+1)=p(2,2);
    x=A*[X(1,i);X(2,i)]+[K(1,i+1); K(2,i+1)]*(L(1,i+1)+V(i+1)/10-H*[X(1,i);X(2,i)]);%计算估计值
    X(1,i+1)=x(1,1);
    X(2,i+1)=x(2,1);
end
n=1:m;
subplot(2,1,1);
plot(n,X(1,n)); hold on    %滤波后的液面高度值
subplot(2,1,2);
plot(n,X(2,n));hold off     %滤波后的液面上升速度