卡尔曼滤波实现多项式拟合Matlab

Posted 2020-09-27 Jarvis_Xu

 

%%%%%%%%%%%%%Q3:多项式系数估计%%%%%%%%%%%%%%%%
%%%%%%%%%%2016/07/21%%%%%%%%%%%%%%%%%%%

clc;clear;
N=10;%样本个数输入
Order=1;%函数阶次输入
M=5;%绘制每M分之1个过程的观测结果曲线
X=linspace(1,N,N);%时间向量
 
for i=1:(Order+1)
    %构造以N/2为对称轴的Order阶函数，计算各阶次系数
    X_0(i)=nchoosek(Order,i-1)*(-N/2)^(i-1);
end
 
C=cell(N,1);
X_noise=(N/10)^Order*randn(1,N);%白噪声
 
%%%%%%%%%%%构造离散点%%%%%%%%%%%%%%%%%%%%
for i=1:N
    temp=0;
    for j=1:(Order+1)
        temp(j)=X(i)^(Order-j+1);
    end
    C{i}=temp;
    Y(i)=C{i}*X_0‘+X_noise(i);
end
%%%%%%%%%%%状态估计初始值%%%%%%%%%%%%%%%%
X_estimate=cell(N,1);
X_estimate1=0;
for i=1:(Order+1)
    X_estimate{1}(i)=0;
end
X_estimate{1}=X_estimate{1}‘;
P_estimate=cell(N,1);
P_estimate1=0;
P_estimate{1}=eye(Order+1);
 
temp=P_estimate{1};
for i=1:(Order+1)
    std{i}(1)=temp(i,i);%std为多项式Order+1个系数方差数组，由协方差矩阵对角线元素（自相关系数）取得
end
 
%%%%%%%%%%%%过程误差及测量误差方差%%%%%%%%%%%%%%%%
R=1;
Q=0;
 
%%%%%%%%%%%%%%迭代过程%%%%%%%%%%%%%%%%%
for k=2:N
    
    X_estimate1=X_estimate{k-1};
    P_estimate1=P_estimate{k-1}+Q;
    Kk=P_estimate1*C{k}‘*[C{k}*P_estimate1*C{k}‘+R].^-1;
    X_estimate{k}=X_estimate1+Kk*(Y(k)-C{k}*X_estimate1);
    P_estimate{k}=(eye(Order+1)-Kk*C{k})*P_estimate1;
    
    
    %%%计算各系数方差%%%
    temp=P_estimate{k};
    for i=1:(Order+1)
        std{i}(k)=temp(i,i);
    end
end
 
 
%%%cell结构的转化，得各阶系数计算结果%%%
legend_str1=cell(1,Order+1);
legend_str1{1}=[‘Measured Value‘];
figure(1);hold on
estimate=cell(M,1);
plot(X,Y,‘v‘,‘linewidth‘,1);
for  z=1:M
    result=X_estimate{N-N/10*(M-z)}
    
    %%%以各阶系数最终计算结果计算多项式估计值%%%
    for i=1:N
        temp=0;
        for j=1:(Order+1)
            temp(j)=X(i)^(Order-j+1);
        end
        C{i}=temp;
        estimate{z}(i)=C{i}*result;
    end
    %%%绘制测量值与估计值%%% 
    legend_str1{z+1}=[‘Estimate(‘,num2str(N-N/10*(M-z)),‘)‘];
    
    if  z==M
        plot(X,estimate{z},‘linewidth‘,2);
    else
        plot(X,estimate{z},‘-.‘,‘linewidth‘,1.5);
    end
end
 
legend(legend_str1);
title(‘Kalman Filter for Polynomial Coefficient‘,‘fontsize‘,16);
str=[‘Order=‘,num2str(Order),‘;N=‘,num2str(N)];
text(N/10,Y(N/10)*1.5,str,‘fontsize‘,16);
hold off
 
%%%各阶系数方差变化曲线%%%
figure(2);
hold on
legend_str2=cell(1,Order+1);
for i=1:(Order+1)
    legend_str2{i}=[‘Std of a(‘,num2str(i),‘)‘];
    plot(X,std{i},‘-.‘,‘linewidth‘,2);
end
title(‘Std of Polynomial Coefficient with Estimating‘,‘fontsize‘,16);
legend(legend_str2);
hold off