k_ud.m

function [X,P] = k_ud(X,P,H,Y,R)
%  K_UD   Kalman update, one measurement per call
%	  Observation covariance R

%  Written by Kai Borre and C.C. Goad
%  November 24, 1996

     omc = Y-H'*X;
     HP = H'*P;
     innovation_variance = HP*H+R;
     K = HP'/innovation_variance;
     X = X+K*omc;
     P = P-K*HP;

%%%%%%%% end k_ud.m  %%%%%%%%%%%%%%%%%%