作者beatitude (python fan)
站內MATLAB
標題[心得] kalman filter in 2 dimension
時間Sat Jun 22 23:54:33 2013
之前聽說 kalman filter 在金融風管部門有一些特別的應用，
最近參考維基網頁試著把 kalman filter 公式以 matlab 呈現，
請板上前輩多多指教。
維基網頁
http://zh.wikipedia.org/wiki/%E5%8D%A1%E5%B0%94%E6%9B%BC%E6%BB%A4%E6%B3%A
%% kalman filter in 2 dimension
%
% x: state
% F: state transition matrix
% u: external motion
% P: prediction covariance matrix
%
% y: measurement residual
% z: measurement
% H: measurement function
% S: measurement covariance matrix
% R: measurement noise
%
% I: identity matrix
%
%
% PREDICT X'
% Predicted (a priori) state estimate
%
% x' = Fx + u
%
% UPDATE P'
% Predicted (a priori) estimate covariance
%
% P' = F*P*(trans F)
%
% UPDATE Y (measurement residual)
% difference between the measurement and what we expected:
%
% y = z - Hx'
%
% UPDATE S (measurement covariance matrix)
%
% S = H*P*(transp H) + R
%
% CALCULATE K (optimal kalman gain)
% it doesn't depend on u or z. We're computing how much of the difference
% between the observed move and the expected move to add to x.
%
% K = P*(transp H)*(inv S)
%
% UPDATE X'
% Updated (a posteriori) state estimate
%
% x' = x + Ky
%
% SUBTRACT P
% Updated (a posteriori) estimate covariance,
% not depending on u or z, input doesn't affect how fast we converge:
%
% P' = (I - K*H)*P
measurements = linspace(1, 10, 10);
% add some noise
measurements = measurements + randn(length(measurements), 1)' * 0.5
x = [0; 0]; % initial model state (location and velocity)
F = [1 1; 0 1]; % state transition matrix
u = [0; 0]; % external motion
P = [1000 0; 0 1000]; % initial uncertainty
H = [1 0]; % measurement function
R = [1]; % measurement uncertainty
I = [1 0; 0 1]; % identity matrix
estimate = [];
for i = 1:length(measurements)
% predicted (a priori) state estimate
x = F*x + u;
% predicted (a priori) estimate covariance
P = F*P*F';
% measurement residual
y = measurements(i) - H*x;
% measurement covariance matrix
S = H*P*H' + R;
% kalman gain
K = P*H'*inv(S);
% updated (a posteriori) state estimate
x = x + K*y;
% updated (a posteriori) estimate covariance
P = (I - K*H)*P;
estimate(:,i) = x;
end
--
1. Write down the problem.
2. Think real hard.
3. Write down the solution.
“The Feynman Algorithm”
as described by Murray Gell-Mann
--
※ 發信站: 批踢踢實業坊(ptt.cc)
◆ From: 111.251.141.79