Why isn't my code iterating more than once?

Hello Everyone

I'm developing a code for a kalman filter in a mobile robot. The idea of this code is that updates the robot position x,y, theta every 0.05seconds up to 40 seconds. There should be 800 iterations but not sure why my code only iterates once?. I have put it in a while loop so it should work?. can someone help me why the code isn't working?. i'll send my code and mat files down below . any help would be really appreciated

Many Thanks

Daniel

[Rik]

Set a breakpoint just before your loop and step through your code. Are all variables what you expect?

[Daniel Pitigala]

Hi Rik

I'm not sure what you mean by that?. I have tried implementing what you suggested but not sure how to since i'm relatively new to MATLAB . could you show what you mean if that is possible?

many thanks

Daniel

[Rik]

You can click on the line number to make Matlab pause execution at that line. That way you can follow step by step what is happening. Using breakpoints is a very valuable tool to debug code.

If you can't find a way to enable them, you can put the line below in your code before the line where you want to add a breakpoint:

ans=dbstack;dbstop('in',strrep(mfilename,'.m',''),'at',num2str(ans(1).line+1)),drawnow %#ok<NOANS>

You can also use the simpler command

keyboard

which should have a similar effect.

But the easiest way is to click on the line number (or on the small dash next to the line number on older releases).

[Fifteen12]

I'm not sure why it's not running 800 times, but I would recommend using a for loop instead of a while loop. It's more robust.

for K = 1:800
    % run your loop here
end

That way, you don't have to increment K each time.

[Daniel Pitigala]

Hi thanks for the advice

I have implemented a for loop but not sure why it's not working. does anyone else have more suggestions?. Always when i run the code it comes with an error saying that "Index in position 2 exceeds array bounds). I'm preety certain that i have incremented K correctly and all?. any additional help is greatly appreciated. I have my code files referenced in my first initial question

Many thanks

Daniel

Answer 1

[Walter Roberson]

r1_real=me(1,K);
r2_real=me(2,K);
b1_real=me(3,K);
b2_real=me(4,K);

Those are all scalars.

x=(r1_real(1,K).^2-r2_real(1,K).^2)/2;
y= (16+sqrt(4-8*(14-r1_real(1,K).^2-r2_real(1,K).^2)))/4;

But there you try to index r1_real at location K in 3 spots, and r2_real at location K as well. Which is a problem because they are scalars.

[Walter Roberson]

x_pos=x(K)+(V(K).*cos(theta(K)));
y_pos=y(K)+(V(K).*sin(theta(K)));

x and y are scalars; you should not be indexing them.

[Walter Roberson]

delta_x=[ones(1,K) zeros(1,K) -(V(1,K)).*sin(theta(1,K));zeros(1,K) ones(1,K) (V(1,K)).*cos(theta(1,K)) ;zeros(1,K) zeros(1,K) ones(1,K)];%% delta x matrix

For reasons that are not at all clear, you construct vectors that are larger and larger as you iterate over K. When K = 2, then ones(1,K) zeros(1,K) is going to be length 2*K between them; likewise for the case where you put zeros(1,K) ones(1,K) the length is going to be 2*K between them. Then since K is scalar, V(1,K) and sin(theta(1,K)) are scalars and so you calculate a scalar at the end of the first row, likewise the V(1,K) and cos are scalars so you calculate scalars for the first two rows. The length of the first two rows is then 2*K+1 . But on the third row, you have zeros(1,K) twice and ones(1,K) so the total length of that row is going to be 3*K . Which is not going to equal 2*K+1 when K > 1.

[Daniel Pitigala]

Hi Walter

Thankyou for your reply. I see what the problem is now but i'm unaware on how to fix the issue?. I'm relatively new and still very much a beginner to matlab . Would you be aware on how to not make these values a scalar quantity and make the code count 800 interations sucessfully?

Many Thanks

Daniel

[Walter Roberson]

Sometimes it helps to first write the code in terms of "current" values. You might have matrices, and you might be looping over them, but it can help to extract the particular values you are interested in right now into scalars, and perform calculations on the scalars, getting a "current" result... which you then might store at the appropriate location in some output array.

Doing this will not necessarily result in the fastest possible code -- but what it does do is make it easier to debug the code. And many professional developers recommend that the highest priority is in first coding correct code, and only then consider optimizing that code... while keeping the correct implementation around for side-by-side testing to be sure that the "optimized" version of the code is producing reasonable answers.

Getting the code right first (instead of getting the code fast first) can make it a lot easier to figure out the "corner-cases". And oddly enough, when you have correctly functioning code up to a point and you start working through all of the "corner-cases", then it is common to realize that the "corner-cases" follow a pattern that can lead to more compact code. Whereas when you are working on making the code fast first, then you are often too busy trying to do mental gymnatistics worrying about whether the "fast" version of the code is correct, making it difficult to figure out efficient ways to handle the "corner-cases"