[未答复] 扩展卡尔曼滤波算法

[复制链接]
顺心平安 发表于 2022-5-13 15:20:13
想请教一下各位大神 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

这一部分程序写的对么?
被这玩意搞坏了。。。。。。
您需要登录后才可以回帖 登录 | 注册

本版积分规则

相关帖子
相关文章
热门教程
站长推荐
快速回复 返回顶部 返回列表