function_kalman_filter.m

卡尔曼滤波估计雅可比的matlab代码,用于计算机视觉的在线估计

function [X_new,P_new] =Function_kalman_filter( m,n,X,P,Z,H,W,Q,R);

 	%----------kalman迭代方程式----------
 	X1=X+W;                 % kalman formula ------1
 	P1=P+Q;                  %kalman formula  ------2
 	K=(P1*H')/(H*P1*H'+R);  %kalman formula  ------3
 	X_new=X1+K*(Z-H*X1);    %kalman formula  ------4
 	P_new=(eye(m*n)-K*H)*P1;  %kalman formula  ------5