9.5 filter(): 量测更新

9.5.1 filter()：函数主体

filter_() 主要是 Kalman 的量测更新，主要是按照公式来的，这里不再赘述。

1. 参数列表

/* args */
double       *x     IO    n阶参数向量，就是参数，不再是最小二乘中的参数的增量 
double       *P     IO    nn阶协方差阵。X、P在filter()函数中既是传入参数，也是传出参数，迭代。
const double *H     I     nm阶设计矩阵的转置
const double *v     I     m阶新息向量
const double *R     I     mm阶量测噪声协方差阵
int           n     I     n阶参数向量的维度
int           m     I     m阶新息向量的维度
/* return */
int           info  -     0：成功，<0：失败

2. 执行流程

• 非零状态索引：
  • 创建索引数组 ix (长度 ），记录非零状态位置（ 且 ）。
  • 计数非零状态数 。
• 内存分配与压缩：
  • 分配压缩数组：
    • x_：压缩状态向量 (长度 ）。
    • xp_：更新状态向量 (长度 ）。
    • P_：压缩协方差矩阵 (大小 ）。
    • Pp_：更新协方差矩阵 (大小 ）。
    • H_：压缩量测矩阵 (大小 ）。
  • 填充压缩数组：
    • ：复制非零状态。
    • ：复制非零状态的协方差。
    • ：复制对应量测矩阵行。
• 卡尔曼滤波更新：
  • 调用 filter_ 执行更新：
    • 输入： (维度 ）。
    • 输出： xp_, Pp_ 。
  • info 记录更新状态。
• 解压与回填：
  • 将更新结果复制回原始数组：
    • ：更新状态。
    • ：更新协方差。

3. 源码注释

extern int filter(double *x, double *P, const double *H, const double *v,
                  const double *R, int n, int m)
{
    double *x_, *xp_, *P_, *Pp_, *H_; /* 压缩状态、更新状态、压缩/更新协方差、压缩量测矩阵 */
    int i, j, k, info, *ix; /* 索引、计数器、更新状态、非零状态索引 */

    /* 创建非零状态索引列表 */
    ix = imat(n, 1); 
    for (i = k = 0; i < n; i++) if (x[i] != 0.0 && P[i + i * n] > 0.0) ix[k++] = i;

    /* 分配并压缩数组，移除零元素以优化计算 */
    x_ = mat(k, 1); xp_ = mat(k, 1); P_ = mat(k, k); Pp_ = mat(k, k); H_ = mat(k, m);
    for (i = 0; i < k; i++) {
        x_[i] = x[ix[i]]; /* 复制非零状态 */
        for (j = 0; j < k; j++) P_[i + j * k] = P[ix[i] + ix[j] * n]; /* 复制非零协方差 */
        for (j = 0; j < m; j++) H_[i + j * k] = H[ix[i] + j * n]; /* 复制量测矩阵 */
    }

    /* 在压缩数组上执行卡尔曼滤波更新 */
    info = filter_(x_, P_, H_, v, R, k, m, xp_, Pp_);

    /* 将更新结果解压回原始数组 */
    for (i = 0; i < k; i++) {
        x[ix[i]] = xp_[i]; /* 更新状态 */
        for (j = 0; j < k; j++) P[ix[i] + ix[j] * n] = Pp_[i + j * k]; /* 更新协方差 */
    }

    /* 释放内存 */
    free(ix); free(x_); free(xp_); free(P_); free(Pp_); free(H_);

    return info; /* 返回更新状态 */
}