Python 卡尔曼滤波器的设计

因此，我被要求设计一个卡尔曼滤波器，并进行一些模拟以查看结果。 给定方程式：

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工作正常吗？如果没有，有什么问题

---

代码看起来正常，结果也正常。什么使你怀疑

这里是一个通用的卡尔曼滤波器实现，作为一个函数，如果您想再次检查，可以检查它。但对于滤波器，它的全部功能是调整协方差矩阵

P

，

Q

，

R

function [v_f,xyz] = KF(u,z,A,B,C,D,x0,P0,Q,R)
%% initialization
persistent x P
if isempty(x)||isempty(P)
   % state vector
   x = reshape(x0,numel(x0),1);  % ensure vector size

   if nargin < 8 || isempty(P0)
       P0 = eye(length(x)); %default initialization
   emd
   P = P0;    % covariance matrix
end


%% covariance matrices 
if nargin < 9 || isempty(Q)
    Q = diag( ones(length(x) )*1e1;  % proess-noise-covariance
end
% % Q = 0 -> perfect model. Q =/= 0 -> pay more attention to the measurements.
%
if nargin < 10
     R = diag( ones(length(z)) ) *1e1;     % measurement-noise-covariance
end
% The higher R, the less trusting the measurements


%% prediction
x_P = A*x + B*u;
P = A*P*A.' + Q;   % covariance matrix

%% update
H = C;

K = P*H.'*(H*P*H.' +R)^-1;
x = x_P + K*(z - H*x_P);
P = (eye(length(x)) - K*H)*P;

%% Output
y = C*x;
end

函数[v_f，xyz]=KF（u，z，A，B，C，D，x0，P0，Q，R）
%%初始化
持久性xp
如果isempty（x）| | isempty（P）
%状态向量
x=重塑（x0，努梅尔（x0），1）；%确保向量大小
如果nargin<8 | |为空（P0）
P0=眼睛（长度（x））；%默认初始化
emd
P=P0；%协方差矩阵
结束
%%协方差矩阵
如果nargin<9 | |是空的（Q）
Q=diag（长度（x））*1e1；%proess噪声协方差
结束
%%Q=0->完美模型。Q=/=0->更多注意测量值。
%
如果nargin<10
R=diag（长度（z））*1e1；%测量噪声协方差
结束
%R越高，对测量结果的信任度越低
%%预测
x_P=A*x+B*u；
P=A*P*A.+Q；%协方差矩阵
%%更新
H=C；
K=P*H.*（H*P*H.+R）^-1；
x=x_P+K*（z-H*x_P）；
P=（眼睛（长度（x））-K*H）*P；
%%输出
y=C*x；
结束

---

在任何情况下，我建议将这个问题转移到，因为您显然没有问题，只是想查看您的代码，对吗？

我认为我不能使用mine实现过滤其他数据？它只在一种情况下有效，对吗？例如，如果我想过滤除正弦波以外的线性信号的噪声数据？嗯，等等，您需要d，以便在重新运行时清除持久变量。可以通过从函数外部调用

clear KF

来执行此操作。否则，它将保留上一示例中的变量