[英]Designing Kalman Filter
所以,我被要求設計一個卡爾曼濾波器並運行一些模擬來查看結果。 給定方程:
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 工作正常嗎? 如果不是,有什么問題?
代碼看起來不錯,結果也不錯。 是什么讓你懷疑?
這是作為 function 的通用卡爾曼濾波器實現,如果您想仔細檢查,可以查看。 但是使用濾波器,它的全部內容是協方差矩陣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
無論如何,我建議將此問題轉移到codereview-pages ,因為您顯然沒有問題,只是想查看您的代碼,對嗎?
聲明:本站的技術帖子網頁,遵循CC BY-SA 4.0協議,如果您需要轉載,請注明本站網址或者原文地址。任何問題請咨詢:yoyou2525@163.com.