Hello, i'm trying to run the program to find the position of an object using ellipsoids intersection. But got the error above. Here is my code. Please explain to me the reason i got error. DOF_ellipsoid is the distance measured from sensors
h_func = @z_fun_ellipsoid;
dh_dx_func = @Hk_fun_ellipsoid;
H_xx = @Hxx_fun;
DOF = k_est_all.*Sound_vel;
% test of function
z_estimate1 = z_fun_ellipsoid( Measurement_size, x_estimate, T_pos, R_pos, Sound_vel, vs_est, DOF ) ;% ellipsoid model for measurement
Hk = Hk_fun_ellipsoid( Measurement_size, x_estimate, T_pos, R_pos, Sound_vel, vs_est, DOF );
%% Estimate with EKF
[M,P] = ekf_predict2(x_estimate,P,A,Q);% kalman predict process
[x_estimate,P,K,MU,S] = ekf_update2(M,P,observe_TOF,dh_dx_func,H_xx,Ez,h_func, Measurement_size, T_pos, R_pos, Sound_vel, vs_est, DOF);% kalman update process
The first subroutine
function yout = z_fun_ellipsoid( Measurement_size, x_estimate, T_pos, R_pos, Sound_vel, vs_est, DOF )
for i = 1:Measurement_size
yout (i) = ellipsoid_gen(i,x_estimate(1:2), T_pos, R_pos(i,:), Sound_vel, vs_est,DOF(i) );
end
yout = yout';
end
Ellipsoid subroutine
function yout = ellipsoid_gen(i, x_estimate, T_pos, R_pos, Sound_vel, vs_est ,DOF_ellipsoid)
ang = acos(( [ T_posR_pos(1,:) ]*[ 1 0 ]')/(norm( T_posR_pos(1,:) )*norm([ 1 0])));
if R_pos(i)T_pos(1)>0
ang = piang;
ang = ang;
end
R = [ cos(ang) sin(ang); sin(ang) cos(ang) ];
% w = [x,y];
alphar = (DOF_ellipsoid(i,:)  norm(R_pos(i,:)))/2;
beta =0.5*sqrt( DOF_ellipsoid(i,:)^2  norm(R_pos(i,:))^2 );
sigma = [alphar^2 0; 0 beta^2];
P = R'*inv(sigma)*R;
yout = (x_estimate'0.5*R_pos(i,:))*P*(x_estimate'0.5*R_pos(i,:))' ;
end
