基于MLE的自适应卡尔曼滤波器中Q与R发散问题求助

你当前实现的基于MLE的自适应卡尔曼滤波器，通过残差和状态修正量的滑动窗口样本协方差更新Q和R，但存在导致矩阵非正定、无法收敛的核心问题，以下是针对性优化方案：

核心问题分析 #

• 样本协方差计算易受小窗口、噪声波动影响，出现负特征值
• Q/R更新公式未做正定约束，数值误差或不合理估计会直接生成非正定矩阵
• R的计算存在时序错位：用当前时刻的P_predicted对应历史窗口内的残差，违背了公式中「每个残差匹配自身时刻预测协方差」的逻辑

具体改进措施 #

1. 新增正定约束辅助函数 #

通过特征值修正强制矩阵正定，同时做对称化处理消除数值不对称：

function A = make_positive_definite(A)
A = (A + A') / 2; % 对称化消除数值误差
[V, D] = eig(A);
epsilon = 1e-6; % 极小正数阈值，避免特征值为0
D(D < epsilon) = epsilon;
A = V * D * V';
end

2. 修正R的时序匹配问题 #

在内存中存储每个时刻的H*P_predicted*H'，计算R时用对应时刻的值做修正，避免时序错位：

% 内存更新环节新增存储当前时刻的H*P_pred*H'
memory.S_k = [memory.S_k, H*P_predicted*H']; 

% R的计算改为窗口内对应时刻的先验创新协方差平均值
R_est = cov(memory.m') - mean(memory.S_k, 2)';

3. 设置最小样本量阈值 #

当窗口内样本量不足时（建议至少为测量维度的2倍），不更新Q/R，避免小样本导致的不稳定估计：

min_samples = max(2*size(H,1), 5); % 取2倍测量维度或5个样本的较大值
if current_N >= min_samples
    % 执行Q/R更新逻辑
else
    Q_next = Q_prev;
    R_next = R_prev;
end

4. 改进后的完整核心代码 #

function [x_updated, P_updated, Q_next, R_next, memory] = adaptive_kalman_filter(x_prev, P_prev, Q_prev, R_prev, z, memory, Phi, H, N)
% Prediction step
x_predicted = Phi * x_prev;
P_predicted = Phi * P_prev * Phi' + Q_prev;

% Update step
S = H * P_predicted * H' + R_prev;
K = P_predicted * H' / S;
x_updated = x_predicted + K * (z - H * x_predicted);
P_updated = (eye(size(P_predicted)) - K * H) * P_predicted;

% Calculate residual and state correction
m_k = z - H * x_predicted;
delta_x_k = x_updated - x_predicted;
current_S_k = H*P_predicted*H';

% Initialize memory if empty
if isempty(memory)
    memory.m = m_k;
    memory.delta_x = delta_x_k;
    memory.S_k = current_S_k;
else
    % Update memory
    memory.m = [memory.m, m_k];
    memory.delta_x = [memory.delta_x, delta_x_k];
    memory.S_k = [memory.S_k, current_S_k];
end

% Keep the latest N data points
if size(memory.m, 2) > N
    memory.m = memory.m(:, end-N+1:end);
    memory.delta_x = memory.delta_x(:, end-N+1:end);
    memory.S_k = memory.S_k(:, end-N+1:end);
end

% Update Q and R
current_N = size(memory.m, 2);
min_samples = max(2*size(H,1), 5);
if current_N >= min_samples
    % Calculate estimated R and Q
    R_est = cov(memory.m') - mean(memory.S_k, 2)';
    Q_est = cov(memory.delta_x') + P_updated - Phi * P_prev * Phi';
    
    % Enforce positive definiteness
    R_next = make_positive_definite(R_est);
    Q_next = make_positive_definite(Q_est);
else
    R_next = R_prev;
    Q_next = Q_prev;
end
end

% Helper function to ensure matrix positive definiteness
function A = make_positive_definite(A)
A = (A + A') / 2;
[V, D] = eig(A);
epsilon = 1e-6;
D(D < epsilon) = epsilon;
A = V * D * V';
end

额外调试建议 #

• 初始Q/R设置为略大于实际系统噪声的正定矩阵（如Q_prev = 1e-3*eye(n)，n为状态维度），避免初始阶段更新波动过大
• 滑动窗口大小N建议设置为状态维度的3-5倍，平衡跟踪时效性与估计稳定性
• 可加入Q/R更新幅度限制，比如每次更新后矩阵元素变化不超过当前值的±20%，防止突变

内容的提问来源于stack exchange，提问作者diana_zp