PF.zip_卡尔曼改进_改进卡尔曼

clear; clc; x = 0; % 初始状态 R = 15;%input('请输入过程噪声方差R的值: ');; % 测量噪声协方差 Q = 15;%input('请输入观测噪声方差Q的值: '); % 过程状态协方差 tf = 100; % 模拟长度 N = 100; % 粒子滤波器中的粒子个数 xhat = x; P = 2; xhatPart = x; %粒子滤波器初期 for i = 1 : N xpart(i) = x + sqrt(P) * randn; end xArr = [x]; yArr = [x^2 / 20 + sqrt(R) * randn]; xhatArr = [x]; PArr = [P]; xhatPartArr = [xhatPart]; close all; for k = 1 : tf % 模拟系统 x = 0.5 * x + 25 * x / (1 + x^2) + 8 * cos(1.2*(k-1)) + sqrt(Q) * randn; y = x^2 / 20 + sqrt(R) * randn; % 粒子滤波器 for i = 1 : N xpartminus(i) = 0.5 * xpart(i) + 25 * xpart(i) / (1 + xpart(i)^2) + 8 * cos(1.2*(k-1)) + sqrt(Q) * randn; ypart = xpartminus(i)^2 / 20; vhat = y - ypart;% 观测和预测的差 q(i) = (1 / sqrt(R) / sqrt(2*pi)) * exp(-vhat^2 / 2 / R); end % 平均每一个估计的可能性 qsum = sum(q); for i = 1 : N q(i) = q(i) / qsum;%归一化权值 end % 重采样 for i = 1 : N u = rand; qtempsum = 0; for j = 1 : N qtempsum = qtempsum + q(j); if qtempsum >= u xpart(i) = xpartminus(j); break; end end end xhatPart = mean(xpart); % 在图片中表示出数据 xArr = [xArr x]; yArr = [yArr y]; xhatArr = [xhatArr xhat]; PArr = [PArr P]; xhatPartArr = [xhatPartArr xhatPart]; end t = 0 : tf; figure; plot(t, xArr, 'b', t, xhatPartArr, 'k-'); set(gca,'FontSize',10); set(gcf,'Color','yellow'); xlabel('time step'); ylabel('state'); legend('真实值', 'PF估计值'); xhatRMS = sqrt((norm(xArr - xhatArr))^2 / tf); xhatPartRMS = sqrt((norm(xArr - xhatPartArr))^2 / tf); disp(['PF估计误差均方值 = ', num2str(xhatPartRMS)]);