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.
Good work! Could you send me a link or a document with the explanation of the algorithm?
I don´t know why you include some variables that not are included in Matlab examples. For example: Bu.
The matrix dimensions are also different...
Link to this code: http://codeviewer.org/view/code:2b99
bocatasinpan (at) hotmail.com