09_kfupdate 逐行注释 Wiki (PSINS)¶

x_k = Φ_{k,k-1}·x_{k-1} + w_{k-1}        状态方程
y_k = H_k·x_k + v_k                       量测方程
E[w]=0, E[w·wᵀ]=Q;  E[v]=0, E[v·vᵀ]=R;  E[w·vᵀ]=0

b = kf.beta     （遗忘因子，指数衰减权重，初始≈0.9~0.99）
R̂ₖ = b·R̂ₖ₋₁ + (1−b)·(rₖ·rₖᵀ − H·Pₖ⁻·Hᵀ)

kf.beta = kf.beta / (kf.beta + kf.b)

常见错误1：以为 kfupdate(kf) 是"什么都不做初始化"
          实际 kfupdate(kf) = 只做'T'时间更新！xk已经被Φ乘过了！
常见错误2：量测更新写成 kfupdate(kf, yk) 不写第三个参数
          → 实际默认'B' = 先Time再Meas
          → 如果之前循环里已经单独做过kfupdate(kf)（Time），
             就会重复做一次Time，等效于"多走了一步"
             → Φ被多乘一次 → P比正常情况下大很多 → 估计发散
正确写法2选1，不要混用：
  A) 高频: kf=kfupdate(kf); 低频: kf=kfupdate(kf,yk,'M');  （test_SINS_GPS_153写法）
  B) 高频: kffk求Φ存好;   低频: kf=kfupdate(kf,yk,'B');    （"一步到位"写法）

有人在自定义量测时手动套公式写成：
  r = H*xkk_1 - yk;    % ❌ 预测减实际=负残差
然后 xk = xkk_1 + K*r  → 等效 xk = xkk_1 - K*(正确残差)
校正方向完全反了！状态误差越"校正"越大。

诊断方法：断L56，看rk符号——如果某通道真实误差应该是正的（位置偏东GPS显示偏东），
        rk应该是正的但你看到是负的→99%是残差写反了
牢记：残差 = "实际看到什么" 减 "我们预测该看到什么"
      即   r = 观测 减 预测观测   （y − Hx）

场景：有人把L78-80改成 kf.Pxk = kf.Pxkk_1 - kf.Kk*kf.Hk*kf.Pxkk_1;
     （即所谓"标准形式" (I-KH)P⁻，并去掉了(P+P')/2）
     跑了1000多步仿真一切正常，突然在第1835步报错：
       Error using chol
       Matrix must be positive definite.

原因：浮点加减不满足严格分配律
     (I-KH)P⁻ = P⁻ - KHP⁻ 数学上= P⁻ - KSKᵀ，但数值上有1e-15级差异
     每步积累一点点不对称(P-P'≈1e-14)，累积几千步后P出现微小负特征值≈-1e-12
     下一步求S=HP⁻Hᵀ+R，S也非正定，invbc/chol直接崩

PSINS作者的保险三件套（缺一不可）：
  ① L75用Joseph形式 P ← P⁻ − K·S·Kᵀ （对称保持性最好）
  ② L80强制 (P+P')/2 每步对称化 （1e-15级误差被抹除）
  ③ L91-102 Pconstrain 防坍防炸 （数值病态兜底）

典型翻车：Pmin=[1e-20, ...] 设太小
         → P坍缩到1e-18时还没触发Pmin
         → K=P⁻HᵀS⁻¹ → K→0 → 状态死锁，再准的量测也"吸收不动"
反向翻车：Pmax=[1e20, ...] 设太大
         → P爆炸到1e15还没触发Pmax
         → K→∞ → x⁺=x⁻+K·r 对量测噪声过敏，估计抖成筛子
经验值（15状态SINS/GPS）：
  Pmin(φ) ≈ (0.001°)² = (1e-5 rad)² ≈ 1e-10
  Pmin(dv)≈ (0.001m/s)² = 1e-6
  Pmin(dp)≈ (0.1m)² = 1e-2
  Pmax ≈ 1e6 × Pmin 比较稳妥

% 在 test_SINS_GPS_153.m 中 kfinit 之后添加/修改：
kf.pconstrain = 0;   % 关闭P约束（默认应该是1，注释掉就是0或手动写0）
kf.adaptive   = 0;   % 顺便关掉自适应R，观察更纯粹的坍缩

% 同时修改仿真时间拉长点：
% 找到NN/totalTime参数，改成1000秒（原默认可能是短时间）

% 在仿真结束后的kfplot调用之前，手动画：
figure;
subplot(3,1,1);
plot(kf.Pxk_log(:,1:3));  title('姿态误差P对角 (rad^2)'); grid on;
subplot(3,1,2);
plot(kf.Pxk_log(:,4:6));  title('速度误差P对角 (m^2/s^2)'); grid on;
subplot(3,1,3);
plot(kf.Pxk_log(:,7:9));  title('位置误差P对角 (m^2)'); grid on;
xlabel('仿真步数');
% 观察现象：
% 某些对角元素会单调下降到 1e-25 ~ 1e-30 之间的极小值并卡住
% 对应状态的 K(k,:) 增益也变成≈0——KF对该状态已经"死锁"，不再相信任何量测

% 假设你把kf.Pmini/kf.Pmaxi也记录到log里了
figure; bar([kf.Pmini; kf.Pmaxi]'); 
legend('Pmin触发次数','Pmax触发次数');
xlabel('状态编号(1~15)'); 
% 看看哪个状态最容易坍缩（一般是零偏10~15维，因为Q最小）、哪个最容易炸

%% 参数
b = 0.001;       % PSINS默认b，查kfinit.m（通常很小）
beta_init = 0.99;% 初始beta
N = 2000;        % 迭代步数（模拟2000次量测更新≈2000s仿真）

%% 迭代
beta_hist = zeros(N,1);
beta = beta_init;
for k = 1:N
    beta_hist(k) = beta;
    beta = beta / (beta + b);   % 对应 L66
end

%% 绘图
figure; 
semilogy(1:N, beta_hist, 'b-', 'LineWidth', 1.5);
xlabel('量测更新步数 k'); ylabel('beta'); grid on;
title('Sage-Husa变遗忘因子beta衰减曲线');

%% 验证渐近公式: beta ≈ beta0 / (1 + k*b*beta0)  近似解
hold on;
k_vec = 1:N;
beta_approx = beta_init ./ (1 + k_vec * b * beta_init);
semilogy(k_vec, beta_approx, 'r--', 'LineWidth', 1.5);
legend('精确迭代值', '近似公式 beta0/(1+k*b*beta0)');
% 结论：b越小，beta衰减越慢，自适应R的"学习时间"越长但越稳
%       b越大，beta衰减越快，初期R剧烈变化，后期迅速冻结

%% 再看 R 的实际变化：假设ry恒定=1，R从10开始
b = 0.001; beta = 0.99;
R_hist = zeros(N,1); R = 10; ry = 1;  % 真实R应该=1，初始R误设为10（太保守）
for k = 1:N
    R_hist(k) = R;
    R = (1-beta)*R + beta*ry;
    beta = beta / (beta + b);
end
figure; plot(1:N, R_hist, 'LineWidth',1.5); grid on;
xlabel('步数'); ylabel('自适应R估计值'); title('Sage-Husa从R=10收敛到真R=1的过程');
% 观察：初期因beta大→R快速下降；后期beta→0→R停在接近1处，不再变