laermanlvbo.zip_LAERMANLVBO_almostprp_everybodyw55_卡尔曼滤波_测量 温度

N= 120; CON = 25; Xexpect = CON*ones(1,N); X = zeros(1,N); Xkf = zeros(1,N); Z = zeros(1,N); P = zeros(1,N); X(1) = 25.1; Z(1) = 24.9; P(1) = 0.01; %初始这个值为何这么小？ Xkf(1) = Z(1); Q = 0.01; R = 0.25; W = sqrt(Q)*rand(1,N); V = sqrt(R)*rand(1,N); F = 1; H = 1; G = 1; I = eye(1); Err = zeros(1,N); Err_messure= zeros(1,N); Err_Kaerman= zeros(1,N); %for k = 2:N % X(k) = F*X(k-1)+G*W(k-1); % Z(k) = H*X(k)+V(k); % Xpre(k) = F*Xzkf(k-1); % Ppre = F*P(k-1)*F'+Q; % K = Ppre*inv(H*Ppre*H'+R); % Xzkf(k) = Z(k)-K*(Z(k)-H*Xpre(k)); % P(k) = (I-K*H)*Ppre; %end for k = 2:N % Err(k) = abs(Xkf(k-1)-X(k-1)); % a(k) = Err(k)+V1(k); % V(k) =(V1(k)*V1(k)/a(k))+(Err(k)*Err(k)/a(k)); %W(k-1)= 2*W1(k-1)*Err(k)/a(k); X(k) = F*X(k-1)+G*W(k-1); %真实值 Z(k) = H*X(k)+V(k); %观测值 Xpp(k) = F*Xkf(k-1); %测量值 Ppre = F*P(k-1)*F'+Q; K = Ppre*inv(H*Ppre*H'+R); % m = 1-K; Xkf(k) = Xpp(k)+K*(Z(k)-H*Xpp(k)); % xxx(k) = Xpp(k)+m*(Z(k)-H*Xpp(k)); P(k) = (I-K*H)*Ppre; %W = (Err_Kaerman+W)/2; end for k =1:N Err_messure(k)=abs(Z(k)-X(k)); Err_Kaerman(k) = abs(Xkf(k)-X(k)); % Err_xlaerman(k) = abs(xxx(k)-X(k)); end t = 1:N; figure; hold on;box on; plot(t,V,'r'); plot(t,V1,'g'); %plot(t,Err_xlaerman,'k'); figure hold on;box on; plot(t,Xkf,'-k',t,X,'r');