首页 > 代码库 > 卡尔曼滤波实现多项式拟合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
声明:以上内容来自用户投稿及互联网公开渠道收集整理发布,本网站不拥有所有权,未作人工编辑处理,也不承担相关法律责任,若内容有误或涉及侵权可进行投诉: 投诉/举报 工作人员会在5个工作日内联系你,一经查实,本站将立刻删除涉嫌侵权内容。