卡尔曼滤波中矩阵求逆算法的改进

卡尔曼滤波中矩阵求逆算法的改进

在对惯导位置误差进行量测的Kalman滤波中，由于经纬度采用弧度表示，而高度采用米表示，量测方差阵的数值在量级上差别很大，采用Matlab求逆，有时会遇到警告。

比如假设：
Rk=[ 0.01/6378000,    0,           0;
     0,               0.1/6378000, 0;
     0,               0,           1  ]^2;
求逆：
Rk^-1
会出现如下警告：
     Warning: Matrix is close to singular or badly scaled.
              Results may be inaccurate. RCOND = 2.458278e-018.
也许这并不影响计算结果的精度，但老出现类似警告还是挺烦人的。

    解决办法：求逆时通过将小的元素适当放大，可避免求逆警告。由于求逆的方差矩阵是正定对称的，对角线元素必定都是正数，因此可以比较对角线元素的平方根相对大小，根据相对大小先将矩阵放大后求逆，可避免条件数过大，求逆之后再放大同样倍数即可。

    用Matlab表示，新的求逆算法函数如下：
function invM = myinv(M)
    D = sqrt(diag(M));
    K = max(D)./D;
    K = diag(K);
    invM = K/(K*M*K)*K;

    或采用以下等效算法，计算量稍少一些：
function invM = myinv(M)
    D = sqrt(diag(M));
    K = max(D)./D;
    K = K*K';
    invM = K.*inv(K.*M);

    当前的导航计算机普遍使用double双精度，位数已经很高了，在实际系统应用中，在数值解算上我尚未发现采用平方根或UDU分解Kalman滤波的必要性。