calculate.m

ekf在无源导航中的应用

%calculate H
function H=calculate(X1,X2)
X=X1-X2;
r=X(1)^2+X(2)^2;
H=[X(2)/r X(1)/r 0 0;-X(4)/r-(2*X(1)*X(2)*X(3)-2*X(1)^2*X(4))/(r^2) X(3)/r+(2*X(2)^2*X(3)-2*X(1)*X(2)*X(4))/(r^2) X(2)/r -X(1)/r];