kalmanfltr.m

来自「toa代码仿真」· M 代码 · 共 15 行

%---------------------------------
% Discreat Kalman Filter Function
% Khodjaev Jasurbek
% MCL 2007
%---------------------------------

function []=kalmanfltr(x,y)
k = 0;
X(k) = [x y 0 0];

Fi = [1 0 delta_t 0; 0 1 0 delta_t; 0 0 1 0; 0 0 0 1];
G = [0 0; 0 0; delta_t 0; 0 delta_t];
M = [1 0 0 0; 0 1 0 0];

X(k+1) = Fi * X(k) + G*W(k);