function [h,H] = sect_6_2_h_fn(x, ref_pt_1, ref_pt_2)
%
% Inputs: 
%
% Written by:
% -- 
% John L. Weatherwax                2005-08-14
% 
% email: wax@alum.mit.edu
% 
% Please send comments and especially bug reports to the
% above email address.
% 
%-----

n = length(x);

h    = zeros(n,1); % stored the nonlinear measurement mapping
h(1) = (x(1) - ref_pt_1(1))^2 + (x(2) - ref_pt_1(2))^2;
h(2) = (x(1) - ref_pt_2(1))^2 + (x(2) - ref_pt_2(2))^2;
h(3) = sqrt( x(3)^2 + x(4)^2 );

Theta = x(4)/sqrt( x(3)^2 + x(4)^2 );
h(4)  = asin( Theta );

DThetaDx3 = -x(3)*x(4)/( (x(3)^2 + x(4)^2)^(3/2) );
DThetaDx4 = 1/( (x(3)^2 + x(4)^2)^(1/2) ) - x(4)*x(4)/( (x(3)^2 + x(4)^2)^(3/2) );

% Fill H here:
H = zeros(n,n);
H(1,1) = 2*(x(1) - ref_pt_1(1));
H(1,2) = 2*(x(2) - ref_pt_1(2));
H(2,1) = 2*(x(1) - ref_pt_2(1));
H(2,2) = 2*(x(2) - ref_pt_2(2));
H(3,3) = x(3)/sqrt( x(3)^2 + x(4)^2 );
H(3,4) = x(4)/sqrt( x(3)^2 + x(4)^2 );
H(3,4) = DThetaDx3/sqrt( 1 - Theta^2 );
H(4,4) = DThetaDx4/sqrt( 1 - Theta^2 );