卡爾曼濾波實現多項式擬合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