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.
Please correct me if I miss something.
I think your code is wrong, though it is working.
The state prediction:
xp=A*x(i-1,:)' + Bu
will give as a result the same vector x with the g in the last element.
xp = [MC/2,MR/2,0,0]'.
It is a bit pointless, since the dt elements will always be cancel out by the last zeros. Then you do correctly the observation step and the algorithm is working, but the prediction practically doesn't exist.