卡尔曼滤波器始终预测线性结果

问题描述

我是卡尔曼滤波器的新手，现在我正在实现该滤波器以预测运动目标。但是，无论我做什么，滤波器都将始终预测直线，而不是实际跟踪目标的运动。 actual vs noisy coordinates

您可以从图表中看到，蓝点是实际值，而红色是噪声坐标。但是，在使用卡尔曼滤波器预测结果之后，它看起来像这样。estimated vs actual coordinates

我正在使用Matlab来实现它，这些是过滤器的功能。

 %kalman predict
    function [xp,Pp] = kalmanPredict(x,P,F,Q)
    % Prediction step of Kalman filter.
    % x: state vector
    % P: covariance matrix of x
    % F: matrix of motion model
    % Q: matrix of motion noise
    % Return predicted state vector xp and covariance Pp
    xp = F * x; % predict state
    Pp = F * P * F' + Q; % predict state covariance

    end 

%kalman update
function [xe,Pe] = kalmanUpdate(x,H,R,z)
% Update step of Kalman filter.
% x: state vector
% P: covariance matrix of x
% H: matrix of observation model
% R: matrix of observation noise
% z: observation vector
% Return estimated state vector xe and covariance Pe

S = H * P * H' + R;  % innovation covariance
K = P * H' * inv(S); % Kalman gain
zp = H * x;          % predicted observation


xe = x + K * (z - zp); %estimated state
Pe = P - K * S * K'; % estimated covariance

end




%kalman tracking
function [px py] = KalmanTracking(z)
% Track a target with a Kalman filter
% z: observation vector
% Return the estimated state position coordinates (px,py)

dt = 0.1; % time interval
N = length(z); % number of samples

F = [1 dt 0 0; 0 1 0 0; 0 0 1 dt; 0 0 0 1]; % CV motion model
Q = [0.1 0 0 0; 0 0.2 0 0; 0 0 0.1 0; 0 0 0 0.2]; % motion noise

H = [1 0 0 0; 0 0 1 0]; % Cartesian observation model
R = [0.3 0; 0 0.3]; % observation noise

x = [0 0 0 0]'; % initial state
P = Q; % initial state covariance

s = zeros(4,N);

 for i=1 : N
     [xp Pp] = kalmanPredict(x,Q);
     [x P] = kalmanUpdate(xp,Pp,z(:,i));
     s(:,i) = x; % save current state
 end

 px = s(1,:); % NOTE: s(2,:) and s(4,:),not considered here,py = s(3,:); % contain the veLocities on x and y respectively
end 

我在这里做错了什么？请帮忙谢谢你

解决方法

暂无找到可以解决该程序问题的有效方法，小编努力寻找整理中！

如果你已经找到好的解决方法，欢迎将解决方案带上本链接一起发送给小编。

小编邮箱:dio#foxmail.com (将#修改为@）