kf_update_w_simple.m

来自「用粒子滤波算法进行跟踪的matlab代码。」· M 代码 · 共 18 行

function [x,P,w]= KF_update_w_simple(x,P,v,R,H, logflag)
%function [x,P,w]= KF_update_w_simple(x,P,v,R,H, logflag)
%
% Calculate the Kalman Filter update given the prior state [x,P], the innovation, v, the 
% observe uncertainty R, and the (linearised) observation model H. The weight, w, is the
% update normalising constant. 
%
% This simple implementation is to check the validity of KF_update_w().
%
% Tim Bailey 2005.

S = H*P*H' + R;  % innovation covariance
W = P*H'*inv(S); % Kalman gain

x = x + W*v;
P = P - W*S*W';
w = gauss_likelihood(v,S,logflag);