Filter For Beginners With Matlab Examples Download Top — Kalman

for k = 1:T w = mvnrnd(zeros(4,1), Q)'; v = mvnrnd(zeros(2,1), R)'; x = A*x + w; z = H*x + v; % Predict xhat_p = A*xhat; P_p = A*P*A' + Q; % Update K = P_p*H'/(H*P_p*H' + R); xhat = xhat_p + K*(z - H*xhat_p); P = (eye(4) - K*H)*P_p; true_traj(:,k) = x; meas(:,k) = z; est(:,k) = xhat; end

MATLAB code:

T = 100; pos_true = zeros(1,T); pos_meas = zeros(1,T); pos_est = zeros(1,T); for k = 1:T w = mvnrnd(zeros(4,1), Q)';

T = 200; true_traj = zeros(4,T); meas = zeros(2,T); est = zeros(4,T); v = mvnrnd(zeros(2