Previous | Next | nonlinear_ok_box_h.m |
function [hk, Hk] = nonlinear_ok_box_h(k, xk)
global nonlinear_ok_box_s1
global nonlinear_ok_box_s2
s1 = nonlinear_ok_box_s1;
s2 = nonlinear_ok_box_s2;
r1 = sqrt( (xk(2) - s1(1))^2 + (xk(4) - s1(2))^2 );
r2 = sqrt( (xk(2) - s2(1))^2 + (xk(4) - s2(2))^2 );
hk = [ r1 ; r2 ];
dr1 = [ 0, xk(2) - s1(1), 0, xk(4) - s1(2) ] / r1;
dr2 = [ 0, xk(2) - s2(1), 0, xk(4) - s2(2) ] / r2;
Hk = [ dr1 ; dr2 ];
return
end