卡爾曼濾波實現多項式擬合Matlab
阿新 • • 發佈：2017-07-08
nom and kalman ffi 樣本 矩陣 協方差 數組 fontsize
%%%%%%%%%%%%%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