KalmanSq.m function[xtn, oRxn, ll] [xtn, oRxn, ll] function KalmanSq(xt,yt,A,C,oRw,oRv,oRx) dimx = size(A,1) dimy dimy size(C,1) M M [oRv C*oRx zeros(dimy,dimx) al al qr(M') r(1:2,end) be be r(3:4,end) oRxn oRxn r(dimy+1:dimy+dimx, dimy+1:dimy+dimx) xtn xtn oRxn'*be ll ll 0 zeros (dimx, dimy) A *oRx oRw zeros dimx dimy 1, dimx 1,dimx zeros () zeros function[xtn,oRxn,ll]=KalmanSq(xt,yt,A,C,oRw,oRv,oRx) dimx=size(A,1); dimy=size(C,1); M=[oRvC*oRxzeros(dimy,dimx); zeros(dimx,dimy)A*oRxoRw; -yt'*inv(oRv')xt'*inv(oRx')zeros(1,dimx)]; [q,r]=qr(M'); al=r(1:2,end); be=r(3:4,end); oRxn=r(dimy+1:dimy+dimx,dimy+1:dimy+dimx); xtn=oRxn'*be; ll=0; %keyboard