josephb.m

来自「Kalman Filtering Theory and Practice, Us」· M 代码 · 共 15 行

function [K,Pout] = josephb(z,R,H,P)
%
% Joseph "stabilized" Kalman filter measurement
% update as modified by Bierman.
%
T1 = sqrt(R);
T2 = H/T1;
T4 = P*T2';
T5 = T2*T4 + 1;
K  = T4/T5;
T7 = P - K*T4';
T8 = T7*T2';
Pout = T7 - T8*K' + K*K';