设计卡尔曼滤波器

因此,我被要求设计一个卡尔曼滤波器,并进行一些仿真以查看结果。 给定方程:

x(k + 1)= -0.8 * x(k)+ sin(0.1k)+ w(k) y(k)= x(k)+ v(k) 其中w(k)和v(k)是处理和测量噪声 w(k)〜N(0,0.01) v(k)〜N(0,1)

我想确保一切正常,并在我的代码中有意义。

A = -0.8;
B = 1;
C = 1;

% Covariance matrices
% Processing noise
W = 0.01;
Q=W;

% Measurement noise
V = 1;
R=V;

% Initial conditions
x0 = 0;
P0 = 0;
xpri = x0;
Ppri = P0;
xpost = x0;
Ppost = P0;

% State
Y = zeros(1, size(t,2));
X = Y;
X(1) = x0;

% xpri - x priori
% xpost - x posteriori
% Ppri - P priori
% Ppost - P posteriori

for i = 1:size(t,2)
    %Simulation
    Y(i) = C*sin((i-1)*0.1)+normrnd(0,sqrt(V));

    if i > 1

        % Prediction
        xpri = A*xpost+B*sin((i-1)*0.1);
        Ppri = A*Ppost*A' + Q;

        eps = Y(i) - C*xpri;
        S = C*Ppri*C' + R;
        K = Ppri*C'*S^(-1);
        xpost = xpri + K*eps;
        Ppost = Ppri - K*S*K';
        X(i) = xpost;
    end
end

plot(t, Y, 'b', t, X, 'r')
title('Kalman's Filter')
xlabel('Time')
ylabel('Value')
legend('Measured value', 'Estimated value')

此KF是否正常工作?如果没有,那是什么问题?

enter image description here