For loops in MATLAB producing NaNs

Greetings! I am in need of some debugging help in trying to figure out what is wrong with my for loop in this program. For some background: The idea is to write a program to estimate the location and frequency of an object. In order to do that, I first used a grid search to produce an estimate of the object's location and then an iterative least squares (ILS) loop to try and produce the more accurate estimate.

Here is the code:

clear
close all
clc

% Example 2 Lecture 8

load('Lecture8_example2_Fall2018')

% Engagement Paramters
time_between_measurements = 10 % seconds
collect_duration = 5*60 % seconds
aircraft_speed = 150 * 0.51444 % meters per second
aircraft_altitude = 10000 / 3.281 % meters
signal_transmit_frequency = 300e6 % Hz
initial_transmit_frequency_error = -50 %Hz
FOA_sigma = 1 % Hz
c = 299792.458e3

% Number of measurements
n = 31

% frequency estimate
f_est = signal_transmit_frequency + initial_transmit_frequency_error

% location
aircraft_pos_m = distdim(aircraft_pos_nmi','nm','m')
z = z_meas
a_dot_aircraft = [aircraft_speed 0 0]'

% set R value
R = eye(31)*diag(FOA_sigma^2)

% grid search to obtain original estimate
% grid search should be from -10:1:10 nmi in both the x and y space
x_grid = distdim(-10,'nm','m'):distdim(1,'nm','m'):distdim(10,'nm','m')
y_grid = distdim(-10,'nm','m'):distdim(1,'nm','m'):distdim(10,'nm','m')
for i = 1:length(x_grid)
    for j = 1:length(y_grid)
        for k = 1:n
                p = [x_grid(i);x_grid(j);0];
                a = aircraft_pos_m(k,:)';
                r = norm(p - aircraft_pos_m(k,:));
                r_dot = -(1/r)*(p -a)'*(a_dot_aircraft);
                h_grid(k,:) = (1 - r_dot/c)*f_est;
        end
        g(i,j) = sum((z_meas - h_grid).^2);
    end
end

[q_hat_x q_hat_y] = find(g==min(min(g)))
a_aircraft = aircraft_pos_m
x_old = [x_grid(q_hat_y);y_grid(q_hat_x);signal_transmit_frequency]

% ILS Loop
for i = 1:100
    for j = 1:n

          x_target = [x_old(1); x_old(2); 0]

          r_loop = norm(x_target - aircraft_pos_m(j,:).')
          r_dot_loop = -(1/r_loop)*(x_target - aircraft_pos_m(j,:).')'*(a_dot_aircraft)

          % measurements

          h(j,1) = (1 - r_loop/c)*f_est

          % partial derivatives

          H_x1_1 = -x_old(3)/c 
          H_x1_2 = -(1/r_loop)^3*a_dot_aircraft.'*(r_loop^2*eye(3)-(x_old-aircraft_pos_m(j,:))*(x_old-aircraft_pos_m(j,:))')
          H_x1_3 = [1;0;0]
          H_x1 = H_x1_1*H_x1_2*H_x1_3
          H_x2 = H_x1_1*H_x1_2*[0;1;0]
          H_x3 = 1-(r_dot_loop/c)

          H(j,:) = [H_x1;H_x2;H_x3]

      end

      x_new = x_old + inv(H.'*inv(R)*H)*H.'*inv(R)*(z_meas-h)

      if(norm(x_new - x_old)) < 1e-3
          break
      end

      x_old = x_new

end

It relies on a .mat file I attached to the question.

However, it appears on about the 8th iteration, the ILS loop begins to break down and produce NaNs. I can't quite figure out where the error is. Would someone be able to help me? I think there is a lot of valuable MATLAB learning that could be acquired.

Thanks for your help in advanced!

[Kevin Chng]

Which variable? x_old?

[billyjthorton]

yes, that is correct. My apologies.

Answer 1

[Star Strider]

The problem is that ‘x_new’ comes close to overflowing just before it becomes NaN in the next iteration:

x_new = x_old + inv(H.'*inv(R)*H)*H.'*inv(R)*(z_meas-h)

x_new =
    10.4757e+114
   -44.6963e+114
   134.5030e+078

I cannot follow your code, so I also cannot suggest a solution. The multiple matrix inversions could contribute to this. The solution to that is usually mldivide,\ (link), because it avoids the instabilities of inverting poorly-conditioned matrices. Poorly-conditioned matrices could be causing the instability in your calculations.

Also, I don’t have ‘distdim’, so I created a version of it to do what what I assume it does:

distdim = @(x,a,b) x * 1852;