EKF.rar_keystone_keystone matlab_keystone matlab

%在解决预测方程非线性的问题时，EKF运用的方法是将非线性关系线性化， %这样线性化后的预测值的方差仍然可以按原来的方法进行线性运算，具体 %的实现方法是将预测方程用泰勒公式展开，舍弃展开式中的高次项。 %EKF适用于非线性化不是太强的情况。 N=150; %一共有150个值用来估计 Q=0.00001; %过程激励噪声协方差(w对应的)，假设为恒定值 v=0.5*randn(1,N); %v用来保存测量误差，服从正态分布 E=zeros(1,150); %E用来保存理论值 E(1)=100; for k=2:150; E(k)=E(k-1)^0.9; %用来构造理论值，这就是已知的非线性关系 end Daoshu=diff(E); %对理论值进行求导运算,进行线性化处理 z=v+E; %z用来保存测量值，测量值与理论值总是有一个偏差 x=zeros(1,150); x(1)=99; %初始估计 p1=2; %初始估计 R=std(v)^2; % std标准偏差，R是v对应的协方差,也可以是R1=cov(v); for k=2:N; x(k)=x(k-1)+Daoshu(1,k-1); %方程一：向前推算状态变量 p2=p1+Q; %方程二：向前推算误差协方差 g=p2/(p2+R); %方程三：计算卡尔曼增益 x(k)=x(k)+g*(z(k)-x(k)); %方程四：由观测变量更新估计，得到最优值 p1=(1-g)*p2; %方程五：更新误差协方差 end t=1:150; figure(1) plot(t,E,'k',t,x,'*',t,z,'.'); %E(实线)是电压真值, x(*)是最优化结果 %z(黑点)是测量值 %axis([0 150 0 10]); legend('电压真值', '最优化结果', '测量值'); title('扩展卡尔曼滤波结果'); xlabel('t'); ylabel('value'); %%%%%%%%%%%%%%半对数坐标绘图%%%%%%%%%%% figure(2) semilogy(t,E,'k',t,x,'*',t,z,'.'); %E(实线)是电压真值, x(*)是最优化结果 %z(黑点)是测量值 %axis([0 150 0 100]); legend('电压真值', '最优化结果', '测量值'); title('扩展卡尔曼滤波半对数坐标下结果'); xlabel('t'); ylabel('value');