卡尔曼序贯滤波方法与误差分析

来自严老师博客：http://blog.sina.cn/dpool/blog/s/blog_40edfdc90102vhmw.html?vt=4
在卡尔曼滤波组合导航中，如果量测噪声方差阵是严格对角矩阵，则可以改成序贯滤波的处理方式（理论上完全成立，但数值上由计算顺序不同可能会引起一些误差），省去矩阵求逆运算（即简化为标量除法运算，在C语言编程时特别方便），以下是Matlab流程示意： function kf = kfupdatesq(kf, yk, measflag)
% KF sequential filtering for measurement update.
%
% See also kfupdate.
% Copyright(c) 2009-2015, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 22/02/2015
if measflag~='M', error('kfupdatesq only used for KF measurement update!'); end
Hk = kf.Hk; Rk = kf.Rk;
for k=1:length(yk)
kf.Hk = Hk(k,:); kf.Rk = Rk(k,k);
kf = kfupdate(kf, yk(k), '