This is a very useful script. Thanks for sharing. I noticed that you are storing a lot of memory for items you don't need. As an example you never use the full Lp, set to zeros(N1,N2). Only the highest order is needed. If I comment out all but the last occurrence of Lp the script generates the same results. You could do the same for L(N1,N2) since you only need 3 values of k at any step in the calculation. Just a thought.
I don't know whether i have misunderstood or not, but some of the important codes are wrong.
In the line 51 of file kalman.m, x(i,:) = (xp + K*([cc(i),cr(i)]' - H*xp))', you just predicte the current position with the current real position and plot them in the same time.
That's why you get such a perfect result.
And I take a reference to other version of Kalman code like this one.
Just change it to the following code:
x(i+1,:) = (xp + K*([cc(i),cr(i)]' - H*xp))';
Correct me if i'm wrong.