扩展卡尔曼滤波算法

想请教一下各位大神 for k=2:len
        X_hat=zeros(len,6);
        X_hat(k+1|k)=T*(H(X_hat(k|k))+B(X_hat(k|k)).*u(k));
        X_hat(k+1|k+1)=X_hat(k+1|k)+L(k+1)*r(k+1);
        r(k+1)=y(k+1)-X_hat(k+1|k);
        X_hat(0|0)=X_hat_0;
    end

    %计算增益
    for k=2:len
        P(k+1|k)=phi(k)*P(k)*inverse(phi(k))+Gamma(X_hat(k|k))*Q(k)*inverse(Gamma(X_hat(k|k)));
        R_r_k=P(k+1|k)+R(k);
        L(k+1)=P(k+1|k)*(R_r_k)^(-1);
        P(k+1)=(I-L(k+1))*P(k+1|k);
        P(0|0)=P_0;
    end

这一部分程序写的对么？
被这玩意搞坏了。。。。。。