首页 > 代码库 > 卡尔曼滤波实现多项式拟合Matlab

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

 

%%%%%%%%%%%%%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
 
 

 

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