kf.m

来自「程序描述了电压为-0.377V的情况下」· M 代码 · 共 160 行

clear all;
close all;


% 初始化参数
n_iter = 50;%迭代次数
sz = [n_iter, 1]; % 一个大小为n_iter的一维数组
x = -0.37727; % 真实值 truth value (typo in example at top of p. 13 calls this z)
z = x + sqrt(0.01)*randn(sz); %观测值 observations (normal about x, sigma=0.1)

Q = 1e-5; % 过程噪声方差强度 process variance

% allocate space for arrays
xhat=zeros(sz);      % 状态后验估计值a posteri estimate of x
P=zeros(sz);         % 误差协方差后验估计值a posteri error estimate
xhatminus=zeros(sz); % 状态先验估计值a priori estimate of x
Pminus=zeros(sz);    % 误差协方差先验估计值a priori error estimate
K=zeros(sz);         % 卡尔曼增益或叫平衡因子gain or blending factor

R = 0.01; % 观测噪声方差强度 可以进行值的变换以观察效果estimate of measurement variance, change to see effect
%R=0.0;
% intial guesses
xhat(1) =0;
P(1) = 1.0;

disp(['--------------------------当R为0.01时：----------------------------------']);
disp(['Iteration # ', num2str(1), ', xhat = ', num2str(xhat(1)),', P = ', num2str(P(1))]);

for k = 2:n_iter
    % 时间更新方程 time update
    xhatminus(k) = xhat(k-1);
    Pminus(k) = P(k-1)+Q;

    % 测量更新方程 measurement update
    K(k) = Pminus(k)/( Pminus(k)+R );
    xhat(k) = xhatminus(k)+K(k)*(z(k)-xhatminus(k));
    P(k) = (1-K(k))*Pminus(k);
    disp(['Iteration # ', num2str(k), ', xhat = ', num2str(xhat(k)), ', P = ', num2str(P(k))]);
end

figure(); 
plot(z,'k+');
hold on;
plot(xhat,'b-')
hold on;
plot(x*ones(sz),'g-');
legend('noisy measurements', 'a posteri estimate', 'truth value');
xlabel('Iteration');
ylabel('Voltage');
hold off;

figure();
valid_iter = [2:n_iter]; % Pminus not valid at step 1
plot(valid_iter,Pminus([valid_iter]));
legend('a priori error estimate');
xlabel('Iteration');
ylabel('$(Voltage)^2$');
ylim([0,.01]);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Q = 1e-5; % 过程噪声方差强度 process variance

% allocate space for arrays
xhat=zeros(sz);      % 状态后验估计值a posteri estimate of x
P=zeros(sz);         % 误差协方差后验估计值a posteri error estimate
xhatminus=zeros(sz); % 状态先验估计值a priori estimate of x
Pminus=zeros(sz);    % 误差协方差先验估计值a priori error estimate
K=zeros(sz);         % 卡尔曼增益或叫平衡因子gain or blending factor

R = 1; % 观测噪声方差强度 可以进行值的变换以观察效果estimate of measurement variance, change to see effect

% intial guesses
xhat(1) =0;
P(1) = 1.0;

disp(['--------------------------当R为1时：----------------------------------']);
disp(['Iteration # ', num2str(1), ', xhat = ', num2str(xhat(1)),', P = ', num2str(P(1))]);

for k = 2:n_iter
    % 时间更新方程 time update
    xhatminus(k) = xhat(k-1);
    Pminus(k) = P(k-1)+Q;

    % 测量更新方程 measurement update
    K(k) = Pminus(k)/( Pminus(k)+R );
    xhat(k) = xhatminus(k)+K(k)*(z(k)-xhatminus(k));
    P(k) = (1-K(k))*Pminus(k);
    disp(['Iteration # ', num2str(k), ', xhat = ', num2str(xhat(k)), ', P = ', num2str(P(k))]);
end

figure(); 
plot(z,'k+');
hold on;
plot(xhat,'b-')
hold on;
plot(x*ones(sz),'g-');
legend('noisy measurements', 'a posteri estimate', 'truth value');
xlabel('Iteration');
ylabel('Voltage');
hold off;

figure();
valid_iter = [2:n_iter]; % Pminus not valid at step 1
plot(valid_iter,Pminus([valid_iter]));
legend('a priori error estimate');
xlabel('Iteration');
ylabel('$(Voltage)^2$');
ylim([0,.01]);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Q = 1e-5; % 过程噪声方差强度 process variance

% allocate space for arrays
xhat=zeros(sz);      % 状态后验估计值a posteri estimate of x
P=zeros(sz);         % 误差协方差后验估计值a posteri error estimate
xhatminus=zeros(sz); % 状态先验估计值a priori estimate of x
Pminus=zeros(sz);    % 误差协方差先验估计值a priori error estimate
K=zeros(sz);         % 卡尔曼增益或叫平衡因子gain or blending factor

R = 0.0001; % 观测噪声方差强度 可以进行值的变换以观察效果estimate of measurement variance, change to see effect

% intial guesses
xhat(1) =0;
P(1) = 1.0;

disp(['--------------------------当R为0.0001时：----------------------------------']);
disp(['Iteration # ', num2str(1), ', xhat = ', num2str(xhat(1)),', P = ', num2str(P(1))]);

for k = 2:n_iter
    % 时间更新方程 time update
    xhatminus(k) = xhat(k-1);
    Pminus(k) = P(k-1)+Q;

    % 测量更新方程 measurement update
    K(k) = Pminus(k)/( Pminus(k)+R );
    xhat(k) = xhatminus(k)+K(k)*(z(k)-xhatminus(k));
    P(k) = (1-K(k))*Pminus(k);
    disp(['Iteration # ', num2str(k), ', xhat = ', num2str(xhat(k)), ', P = ', num2str(P(k))]);
end

figure(); 
plot(z,'k+');
hold on;
plot(xhat,'b-')
hold on;
plot(x*ones(sz),'g-');
legend('noisy measurements', 'a posteri estimate', 'truth value');
xlabel('Iteration');
ylabel('Voltage');
hold off;

figure();
valid_iter = [2:n_iter]; % Pminus not valid at step 1
plot(valid_iter,Pminus([valid_iter]));
legend('a priori error estimate');
xlabel('Iteration');
ylabel('$(Voltage)^2$');
ylim([0,.01]);