function_kalman_filter.m

粒子滤波程序

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