EKF 与因子图核心概念一一对应表

EKF 概念	因子图中的对应概念	详细解释
状态变量 $\mathbf{x}_k$	图中的节点（变量节点 $x_k$）	节点表示系统在时刻 $k$ 的状态。两者一一对应。
预测模型 $f(x_{k-1}, u_k)$	运动因子（Motion Factor）	用于连接 $x_{k-1} \rightarrow x_k$ 的边，残差为 $r_f = x_k - f(x_{k-1}, u_k)$
观测模型 $h(x_k)$	观测因子（Measurement Factor）	残差为 $r_h = z_k - h(x_k)$，表示观测与预测的差距
过程噪声协方差 $\mathbf{Q}_k$	运动因子的协方差 $\Sigma_f$	用来加权运动因子的误差大小（“惩罚”大误差）
观测噪声协方差 $\mathbf{R}_k$	观测因子的协方差 $\Sigma_h$	用来加权观测因子的误差大小
协方差传播	信息融合内隐在因子之间的残差结构中	因子图不显式传播协方差，而是通过联合优化自动融合多步误差信息
卡尔曼增益 $K_k$	隐含在优化中协方差加权的最小二乘解结构中	因子图没有明确的“增益”，但它通过残差加权自动得出最优融合值
状态更新公式	全局最小化残差平方和：MAP 估计	状态是优化变量，不是逐步更新；一次性联合优化获得所有状态变量的最优值
初始状态分布（先验）	先验因子（Prior Factor）	对初始状态添加高斯分布先验，例如 $x_0 \sim \mathcal{N}(\mu_0, P_0)$
残差（如 $z_k - h(x_k)$）	每个因子的误差函数（Residual function）	用于构建代价函数，衡量模型预测与实际之间的误差
误差协方差加权项 $\Sigma^{-1}$	误差惩罚（loss penalty），残差的权重项	在优化目标函数中体现为：$\Vert r \Vert^2_{\Sigma^{-1}} = r^\top \Sigma^{-1} r$
状态估计不确定性 $\mathbf{P}_k$	优化后可由海森矩阵近似协方差（$P \approx (H^\top \Sigma^{-1} H)^{-1}$）	因子图本身不显示协方差传播，但可由线性化后提取
递归更新过程	增量优化（iSAM）或批量优化（Batch MAP）	因子图可以递归优化，也可以批量优化，更灵活

举个栗子

那么，状态估计模型如何从 EKF 转为因子图呢？举个栗子：
假设我们有以下系统：

状态：位置+速度 $x_k = [p_k, v_k]^\top$
控制输入：加速度 $a_k$
测量：位置观测 $z_k = p_k + \text{noise}$

▶ EKF 中的模型：

状态转移：$\hat{x}_k = f(x_{k-1}, a_k) = \begin{bmatrix} p_{k-1} + v_{k-1} \Delta t + 0.5 a_k \Delta t^2 \ v_{k-1} + a_k \Delta t \end{bmatrix}$
状态协方差预测：$P_k^- = F P_{k-1} F^\top + Q$
观测模型：$z_k = H x_k + \eta_k$，其中 $H = [1, 0]$

▶ 因子图表示（统一为最小化目标）：

状态变量：

节点变量：$x_0, x_1, x_2, \dots$

1. 初始先验因子（Prior Factor）：

$\text{残差： } r_0 = x_0 - \mu_0,\quad \Sigma_0 = P_0$

惩罚项为：$r_0^\top \Sigma_0^{-1} r_0$

2. 运动因子（IMU 因子）：

$r_{k}^{\text{motion}} = x_k - f(x_{k-1}, a_{k-1})$
$\text{惩罚项： } r_{\text{motion}}^\top Q^{-1} r_{\text{motion}}$

3. 观测因子：

$kr_k^{\text{obs}} = z_k - h(x_k) = z_k - H x_k$

惩罚项为：

R−1 $robsr_{\text{obs}}^\top R^{-1} r_{\text{obs}}$

▶ 最终目标函数（因子图）：

整体优化目标为最小化所有残差平方和：

$\min_{x_0, x_1, \dots, x_N} \left[ \underbrace{(x_0 - \mu_0)^\top P_0^{-1} (x_0 - \mu_0)}_{\text{先验因子}} + \sum_{k=1}^{N} \underbrace{(x_k - f(x_{k-1}, a_{k-1}))^\top Q^{-1} (x_k - f(x_{k-1}, a_{k-1}))}_{\text{运动因子}} + \sum_{k=1}^{N} \underbrace{(z_k - H x_k)^\top R^{-1} (z_k - H x_k)}_{\text{观测因子}} \right]$

✅ 总结思维图：EKF vs 因子图 ✅

EKF	因子图
状态变量 x_k	变量节点 x_k
预测函数 f(x,u)	运动因子（残差函数）
观测函数 h(x)	观测因子（残差函数）
协方差矩阵 P_k	Hessian 的近似逆
过程噪声 Q	运动因子的协方差
观测噪声 R	观测因子的协方差
协方差传播	多因子耦合优化，自然传播
卡尔曼增益 K	解优化问题后的隐含加权融合
状态更新	一次性求解最小残差路径（MAP估计）

—-

EKF 与因子图的各要素映射（数值方法 + 概率建模角度）

1. 状态变量（State Variable）

EKF	因子图	说明
状态向量 $\mathbf{x}_k$	隐变量节点 $x_k$	表示系统在时刻 $k$ 的状态，例如位置、速度、姿态等。EKF 是时间步递推估计，因子图则将所有时刻的状态显式建图，一次联合优化。

2. 初始先验（Initial Prior）

EKF	因子图	说明
初始估计：$\mathbf{x}_0 \sim \mathcal{N}(\mu_0, \Sigma_0)$	先验因子：$\phi_0(x_0) \propto \exp\left( -\frac{1}{2}(x_0 - \mu_0)^T \Sigma_0^{-1} (x_0 - \mu_0) \right)$	EKF 使用均值+协方差初始化，因子图通过先验因子显式表达初始置信度。两者在概率意义下等价。

3. 预测模型（Motion Model）

EKF	因子图	说明
状态转移：$\mathbf{x}_k = f(\mathbf{x}_{k-1}, \mathbf{u}_k) + \mathbf{w}_k$，$\mathbf{w}_k \sim \mathcal{N}(0, Q_k)$	运动因子：$\phi_k(x_{k-1}, x_k) \propto \exp\left( -\frac{1}{2} \vert x_k - f(x_{k-1}, u_k) \vert^2_{Q_k^{-1}} \right)$	EKF 的系统模型通过 $f$ 递推，误差协方差为 $Q_k$；因子图中残差即预测误差，协方差控制惩罚强度。

4. 观测模型（Measurement Model）

EKF	因子图	说明
观测方程：$\mathbf{z}_k = h(\mathbf{x}_k) + \mathbf{v}_k$，$\mathbf{v}_k \sim \mathcal{N}(0, R_k)$	观测因子：$\phi_k^{(z)}(x_k) \propto \exp\left( -\frac{1}{2} \vert z_k - h(x_k) \vert^2_{R_k^{-1}} \right)$	EKF 每一步用观测修正状态，因子图中用观测因子连接 $x_k$，惩罚观测误差。

5. 协方差传播与增益（Covariance Propagation & Kalman Gain）

\	EKF 思路	因子图思路
协方差传播	显式传播 $\mathbf{P}$，用于后续加权	隐式表达为残差加权项 $\Sigma^{-1}$，内嵌于优化结构
状态更新	当前时刻预测值加增益修正	所有时刻状态同时优化，统一处理全部信息
信息融合	卡尔曼增益调节观测 vs 预测的比重	残差协方差控制“置信程度”，优化中自适应融合

在 EKF 中，这部分的核心任务是在预测阶段传播不确定性，并在观测到来时利用协方差矩阵自适应地加权观测和预测信息。

🔹EKF 中的做法如下：

协方差预测（时间更新）
当状态从 $\mathbf{x}_{k-1}$ 预测到 $\mathbf{x}_k$ 时，状态协方差 $\mathbf{P}$ 随之传播：

$\mathbf{P}_{k|k-1} = F_k \mathbf{P}_{k-1|k-1} F_k^\top + Q_k$

其中 $F_k$ 是状态转移函数 $f$ 关于 $x$ 的雅可比矩阵，$Q_k$ 是过程噪声协方差。
计算卡尔曼增益（信息融合权重）
利用预测协方差与观测噪声，计算最优融合权重：

$\mathbf{K}_k = \mathbf{P}_{k|k-1} H_k^\top (H_k \mathbf{P}_{k|k-1} H_k^\top + R_k)^{-1}$

其中 $H_k$ 是观测函数 $h$ 关于 $x$ 的雅可比，$R_k$ 是观测噪声协方差。

🔹因子图中的对应思想：

因子图不显式传播协方差矩阵，但它通过构建带协方差权重的残差因子，将预测与观测误差统一编码在优化问题中。

具体来说，因子图构建了如下优化目标：

$\min_{x_{0:k}} \sum \left| \underbrace{r_i(x)}_{\text{残差}} \right|^2_{\Sigma_i^{-1}} = \sum r_i^\top \Sigma_i^{-1} r_i$

其中：

$\Sigma_i^{-1}$ 表示对应误差项的加权矩阵（即协方差的逆）；
对于预测项，其残差 $r^{\text{pred}} = x_k - f(x_{k-1}, u_k)$，权重为 $Q_k^{-1}$；
对于观测项，其残差 $r^{\text{obs}} = z_k - h(x_k)$，权重为 $R_k^{-1}$。

这就相当于在“增益加权”的层面上，将 EKF 的卡尔曼增益逻辑，转化为残差惩罚中的加权策略。在非线性优化中，这种残差的协方差加权正是影响变量更新方向和幅度的关键，相当于自动调节“相信先验”还是“相信观测”。

6. 状态更新（State Update）

🔶EKF 中的状态更新是通过一次卡尔曼增益加权修正完成的：

$\mathbf{x}_{k|k} = \mathbf{x}_{k|k-1} + \mathbf{K}_k \left( \mathbf{z}_k - h(\mathbf{x}_{k|k-1}) \right)$

这个公式的本质是：预测值 + 增益 × 残差，即根据观测与预测之间的差距，对预测状态进行加权修正。

🔶因子图没有显式的“预测 + 更新”过程，而是：

将所有状态变量 $x_0, x_1, …, x_k$ 一起建模；
所有的运动模型、观测模型都转化为误差残差项；
最后通过非线性最小二乘优化（如高斯牛顿或 LM 算法），求解一组最优状态变量，使所有残差总和最小。

即：$\hat{x}_{0:k} = \arg\min \sum_i | r_i(x) |^2_{\Sigma_i^{-1}}$

这相当于一次性求解一个全局最优状态估计，而不是逐步迭代更新。

7. 批量 vs 滑动窗口（时序结构）

EKF	因子图	说明
递推更新（只保留当前状态）	批量建图，优化全序列状态	EKF 无法处理回环等历史依赖场景；因子图可以全局建图（如 SLAM），也可局部滑窗（iSAM）。
仅当前 $\mathbf{x}_k$ 更新	所有 $x_0, x_1, …, x_k$ 可更新	因子图支持调整整个轨迹的估计，保持全局一致性，EKF 则仅当前最优。

8. 概率建模视角

EKF 目标：

$\text{递推估计 } p(x_k | z_{1:k}, u_{1:k}) \approx \mathcal{N}(\hat{x}_k, P_k)$

即对当前状态的后验进行高斯近似。

因子图目标（最大后验估计）：

$\hat{x}_{0:k} = \arg\max_{x_{0:k}} p(x_{0:k} | z_{1:k}, u_{1:k}) = \arg\max_{x_{0:k}} \prod_i \phi_i(x_{\mathcal{S}_i})$

其中每个 $\phi_i$ 是一个因子（残差项），对应不同的信息来源（先验、运动模型、观测模型）。

优化形式为：

$\min_{x_{0:k}} \sum_i \underbrace{| r_i |^2_{\Sigma_i^{-1}}}_{\text{惩罚项（误差加权）}}$

✅ 总结对照表

概念	EKF 表达	因子图表达
状态变量	$\mathbf{x}_k$	隐变量 $x_k$
初始先验	$\mathcal{N}(\mu_0, \Sigma_0)$	先验因子 $\phi(x_0)$
状态转移模型	$x_k = f(x_{k-1}, u_k) + w_k$	运动因子 $\phi(x_{k-1}, x_k)$
观测模型	$z_k = h(x_k) + v_k$	观测因子 $\phi(x_k, z_k)$
过程噪声	$Q_k$	惩罚项 $\vert x_k - f(x_{k-1}) \vert^2_{Q_k^{-1}}$
观测噪声	$R_k$	惩罚项 $\vert z_k - h(x_k) \vert^2_{R_k^{-1}}$
协方差传播	$P_k = F P_{k-1} F^\top + Q$	通过图结构自然融合（Hessian矩阵近似协方差）
状态更新	预测 + 增益修正	全局优化状态节点
卡尔曼增益	$K_k$	优化中残差加权项（协方差逆）
残差惩罚项	$\mathbf{K}_k (\mathbf{z}_k - h(\hat{x}_k))$	$r^\top \Sigma^{-1} r$
滑动窗口结构	单状态存储、不能回溯	支持全图优化、滑窗优化、回环重定位

举个粒子

粒子①

下面通过一个简洁明了的 1 维轨迹估计问题，手把手的演示如何从 EKF 的形式出发，构造出因子图结构，写出残差函数和优化目标函数，以及其物理意义。

小案例：一维轨迹估计 + 高斯观测 + 因子图建模

🎯 问题描述：

一辆车在直线轨道上运动，位置用标量 $x_k$ 表示。假设它以 恒定速度 $v$ 向前运动，但受到过程噪声干扰。

我们有三个时间点的观测：

状态变量：$x_0, x_1, x_2$
控制输入：恒定速度 $v = 1$
时间间隔: $\Delta t = 1$
测量值：$z_0 = 0.2, z_1 = 1.1, z_2 = 2.0$

所有过程噪声 $\mathcal{N}(0, \sigma^2_Q)$，测量噪声 $\mathcal{N}(0, \sigma^2_R)$，其中：

$\sigma_Q = 0.1$（过程不确定度小）
$\sigma_R = 0.5$（测量更不准）

🔶 1. 状态变量与图节点

我们要估计的状态为：

$x_0,\ x_1,\ x_2$

图中，每个变量是一个节点：

x0 —— x1 —— x2
 \     |     |
  \    |     |
   z0  z1    z2  （观测因子）

🔶 2. 构造因子图的因子项（残差项）

（1）先验因子：$x_0$

假设我们对起点有先验: $x_0 \sim \mathcal{N}(0, \sigma_0^2)$，设 $\sigma_0 = 0.2$

残差函数为：$r_0(x_0) = x_0 - 0,\quad \text{代价：} \frac{1}{\sigma_0^2} (x_0)^2$

（2）运动模型因子：$x_{k-1} \rightarrow x_k$

因为速度恒定，状态转移模型为：$x_k = x_{k-1} + v \cdot \Delta t + w_k,\quad w_k \sim \mathcal{N}(0, \sigma_Q^2)$

残差函数为：$r_k^{\text{motion}} = x_k - (x_{k-1} + v)$

代价项为：$\frac{1}{\sigma_Q^2} \left( x_k - x_{k-1} - v \right)^2$

对于 $x_1$：$r_1^{\text{motion}} = x_1 - x_0 - 1$

对于 $x_2$：$r_2^{\text{motion}} = x_2 - x_1 - 1$

（3）观测因子：$z_k$

观测模型为：$z_k = x_k + v_k,\quad v_k \sim \mathcal{N}(0, \sigma_R^2)$

残差为：$r_k^{\text{obs}} = z_k - x_k,\quad \text{代价：} \frac{1}{\sigma_R^2} (z_k - x_k)^2$

即：

$r_0^{\text{obs}} = 0.2 - x_0$
$r_1^{\text{obs}} = 1.1 - x_1$
$r_2^{\text{obs}} = 2.0 - x_2$

🔶 3. 构建总优化目标函数

总目标函数为残差平方和加权：

$J(x_0, x_1, x_2) = \underbrace{\frac{1}{\sigma_0^2} (x_0)^2}_{\text{先验}} + \underbrace{\frac{1}{\sigma_Q^2} (x_1 - x_0 - 1)^2}_{\text{运动}} + \underbrace{\frac{1}{\sigma_Q^2} (x_2 - x_1 - 1)^2}_{\text{运动}} + \underbrace{\frac{1}{\sigma_R^2} (0.2 - x_0)^2}_{\text{观测}} + \underbrace{\frac{1}{\sigma_R^2} (1.1 - x_1)^2}_{\text{观测}} + \underbrace{\frac{1}{\sigma_R^2} (2.0 - x_2)^2}_{\text{观测}}$

🔶 4. 求解（数值最小化）

这就是标准的非线性最小二乘问题，可以用高斯牛顿法或 LM 方法求解。优化变量为：

$\mathbf{x} = [x_0,\ x_1,\ x_2]^T$

也可以用 Python 中的 scipy.optimize.least_squares 或 MATLAB 的 lsqnonlin 直接优化。

步骤	类型	残差函数 $r$	权重 $\Sigma^{-1}$
1	先验因子	$x_0$	$1/\sigma_0^2$
2	运动因子	$x_k - x_{k-1} - v$	$1/\sigma_Q^2$
3	观测因子	$z_k - x_k$	$1/\sigma_R^2$

最终通过联合优化这些残差，就能得到全局一致的状态估计yeah

MATLAB 实现：EKF 与因子图对比

function factor_graph_vs_ekf_1D()
    clc; clear;close all;

    % 参数设置
    v = 1.0; dt = 1.0;          % 速度和时间间隔
    sigma_0 = 0.2;              % 初始状态不确定性(标准差)
    sigma_Q = 0.1;              % 过程噪声 std （运动模型的不确定性）
    sigma_R = 0.5;              % 观测噪声 std （量测误差）

    % 根据真值造量测（加噪）
    true_x = [0; 1; 2];         % 理想真实轨迹 （匀速1m/s前进）
    z = true_x + sigma_R * randn(3,1);  % 量测   模拟量测误差：每个观测点加入高斯噪声 
   

    %======== 因子图优化解 ========%
    x0 = [0; 0.5; 1.8];         % 先验 （不一定准确）
    opts = optimoptions('lsqnonlin', 'Display','off'); % 设置优化器参数
    % lsqnonlin 是 MATLAB 的非线性最小二乘优化器，会最小化所有残差的平方和：
    % 残差由 residual_fg 函数返回
    x_fg = lsqnonlin(@(x) residual_fg(x, z, v, dt, sigma_0, sigma_Q, sigma_R), x0, [], [], opts); 

    %======== EKF 解 ========%
    x_ekf = zeros(3,1);         % 状态估计
    P = zeros(3,1);             % 协方差
    x_ekf(1) = 0;               % 初始值
    P(1) = sigma_0^2;

    for k = 2:3
        % Prediction
        x_pred = x_ekf(k-1) + v * dt;       % 用运动模型预测位置
        P_pred = P(k-1) + sigma_Q^2;   % 协方差传播（加过程噪声）

        % Measurement update
        K = P_pred / (P_pred + sigma_R^2);      % 卡尔曼增益
        x_ekf(k) = x_pred + K * (z(k) - x_pred);   % 状态更新
        P(k) = (1 - K) * P_pred;                            % 协方差更新
    end

    %======== 可视化 ========%
    figure; hold on;
    plot(0:2, true_x, 'k--o', 'DisplayName','True Trajectory');
    plot(0:2, z, 'rx', 'MarkerSize',10, 'DisplayName','Measurements');
    plot(0:2, x_fg, 'b-o', 'LineWidth', 2, 'DisplayName','Factor Graph');
    plot(0:2, x_ekf, 'g--s', 'LineWidth', 2, 'DisplayName','EKF');
    legend;
    grid on;
    xlabel('Time step k'); ylabel('Position');
    title('1D Trajectory Estimation: EKF vs Factor Graph');

    %======== RMSE 计算 ========%
    rmse_ekf = sqrt(mean((x_ekf - true_x).^2));
    rmse_fgo = sqrt(mean((x_fg - true_x).^2));

    %======== RMSE 对比图 ========%
    figure;
    bar([rmse_ekf, rmse_fgo]);
    set(gca, 'XTickLabel', {'EKF', 'Factor Graph'});
    ylabel('RMSE');
    title('RMSE Comparison: EKF vs Factor Graph');
    grid on;

end

% 残差构造（因子图）
function r = residual_fg(x, z, v, dt, sigma_0, sigma_Q, sigma_R)
    % x = [x0; x1; x2]
    r_prior = (x(1) - 0) / sigma_0;                        % 初始先验项（越偏离0惩罚越大）  r_0 = \frac{x_0 - \mu_0}{\sigma_0}
    r_motion1 = (x(2) - x(1) - v*dt) / sigma_Q;   % r_1 = \frac{x_1 - x_0 - v \cdot dt}{\sigma_Q}
    r_motion2 = (x(3) - x(2) - v*dt) / sigma_Q;   % r_2 = \frac{x_2 - x_1 - v \cdot dt}{\sigma_Q}
    r_obs = (z - x) / sigma_R;                              % 所有观测的误差（每一时刻观测 z_k 与估计 x_k 的差）r_z = \frac{z_k - x_k}{\sigma_R}
    r = [r_prior; r_motion1; r_motion2; r_obs];   % 拼接成一维残差向量
end

粒子②

更进一步：

clc; clear; close all;

%% 参数设置
T = 100;              % 时间步数
dt = 1.0;            % 时间间隔
v = 1.0;             % 真实速度（恒定）

Q = 0.5^2;           % 过程噪声方差 (较大噪声)
R = 1.8^2;           % 观测噪声方差 (较大噪声)

rng(42);             % 固定随机种子，方便复现

%% 真值轨迹生成 (匀速运动 + 过程噪声)
% 状态方程: x_k = x_{k-1} + v*dt + w_k  （运动模型）
% w_k ~ N(0, Q) - 过程噪声 （零均值高斯噪声）
% sqrt(Q)*randn 生成标准差为 sqrt(Q) 的高斯噪声
x_true = zeros(T,1);
for k = 2:T
    x_true(k) = x_true(k-1) + v*dt + sqrt(Q)*randn;
end

%% 观测生成 (仅偶数时刻有观测，50%缺失)
z = NaN(T,1);
for k = 1:T
    if mod(k,2)==0
        z(k) = x_true(k) + sqrt(R)*randn;
    end
end

%% ==================== EKF 实现 ====================
x_ekf = zeros(T,1);      % 状态估计
P = 1000;                   % 初始协方差 (较大不确定性)
x_ekf(1) = 0;               % 初始状态设为0 (无初始观测) （设为0，与实际有偏差）

for k = 2:T
    % ---预测---
    % 状态预测: x_pred = f(x_{k-1}) = x_{k-1} + v*dt
    x_pred = x_ekf(k-1) + v*dt; 
    % 协方差预测: P_pred = F·P·F' + Q  % 过程噪声增加不确定性
    % 这里F=1（一维线性系统），所以简化为 P_pred = P + Q
    P_pred = P + Q; 
    
    % --更新--- 
    if ~isnan(z(k)) % 有观测时执行更新
        % 计算Kalman增益: K = P_pred·H'·(H·P_pred·H' + R)^{-1}
        % 这里H=1（观测矩阵），简化为 K = P_pred/(P_pred + R)
        K = P_pred / (P_pred + R);  
        % 状态更新: x = x_pred + K·(z - H·x_pred)
        x_ekf(k) = x_pred + K * (z(k) - x_pred);
        % 协方差更新: P = (I - K·H)·P_pred
        P = (1 - K) * P_pred;
    else % 无观测时仅预测
        x_ekf(k) = x_pred;
        P = P_pred;
    end
end

%% ==================== FGO 实现 ====================
% 初始化信息矩阵(Λ)和信息向量(η)
Lambda = zeros(T, T);  % 信息矩阵 (Hessian)
eta = zeros(T, 1);     % 信息向量

% 添加弱先验因子 (避免矩阵奇异)
Lambda(1,1) = 1/10000; % 弱先验信息 (方差=10000)

% 添加动态因子 (相邻状态间的约束)
for k = 2:T
    % 动态因子表示状态转移约束: x_k = x_{k-1} + v*dt + w_k

    % 信息矩阵更新 (四元素更新)
    % 对应约束: (x_k - x_{k-1} - v*dt)^2 / Q
    Lambda(k-1, k-1) = Lambda(k-1, k-1) + 1/Q;
    Lambda(k-1, k)   = Lambda(k-1, k)   - 1/Q;
    Lambda(k, k-1)   = Lambda(k, k-1)   - 1/Q;
    Lambda(k, k)     = Lambda(k, k)     + 1/Q;
    
    % 信息向量更新
    % 对应约束中的常数项: -2*(v*dt)*(x_k - x_{k-1})/(2Q)
    eta(k-1) = eta(k-1) - (v*dt)/Q;
    eta(k)   = eta(k)   + (v*dt)/Q;
end

% 添加观测因子
for k = 1:T
    if ~isnan(z(k)) % 仅在有观测时添加
        % 观测因子: (x_k - z_k)^2 / R

        % 信息矩阵更新
        Lambda(k, k) = Lambda(k, k) + 1/R;

        % 信息向量更新
        eta(k) = eta(k) + z(k)/R;
    end
end

% ------ 求解线性系统 ------
% 最大后验估计: x̂ = argmin Σ||r_i||^2 = Λ^{-1}η

% 使用Cholesky分解高效求解
R_chol = chol(Lambda); % Cholesky分解: Λ = R'*R
x_fgo = R_chol \ (R_chol' \ eta); % 求解线性系统: R'*R*x = η

%% ==================== 性能评估 ====================
% 计算RMSE
ekf_error = x_true - x_ekf;
fgo_error = x_true - x_fgo;

rmse_ekf = sqrt(mean(ekf_error.^2));
rmse_fgo = sqrt(mean(fgo_error.^2));

% 计算平均误差
mean_ekf = mean(abs(ekf_error));
mean_fgo = mean(abs(fgo_error));

fprintf('============ 性能对比 ============\n');
fprintf('EKF 均方根误差(RMSE): %.4f\n', rmse_ekf);
fprintf('FGO 均方根误差(RMSE): %.4f\n', rmse_fgo);
fprintf('EKF 平均绝对误差: %.4f\n', mean_ekf);
fprintf('FGO 平均绝对误差: %.4f\n', mean_fgo);
fprintf('FGO精度提升: %.2f%%\n', (rmse_ekf - rmse_fgo)/rmse_ekf*100);

%% ==================== 绘图对比 ====================
figure('Position', [100, 100, 1200, 600]);

% 轨迹对比
subplot(2,1,1);
plot(1:T, x_true, '-k', 'LineWidth', 2); hold on;
plot(1:T, x_ekf, '--b', 'LineWidth', 1.5);
plot(1:T, x_fgo, '-.r', 'LineWidth', 1.5);
scatter(find(~isnan(z)), z(~isnan(z)), 70, 'm', 'filled', 'MarkerEdgeColor', 'k');
title('轨迹估计对比');
legend('真实轨迹', 'EKF估计', 'FGO估计', '观测点', 'Location', 'northwest');
xlabel('时间步'); ylabel('位置');
grid on;
set(gca, 'FontSize', 12);

% 误差对比
subplot(2,1,2);
plot(1:T, abs(ekf_error), 'b', 'LineWidth', 1.5); hold on;
plot(1:T, abs(fgo_error), 'r', 'LineWidth', 1.5);
title('估计误差绝对值');
legend('EKF误差', 'FGO误差');
xlabel('时间步'); ylabel('位置误差');
grid on;
set(gca, 'FontSize', 12);

% 标注性能指标
annotation('textbox', [0.15, 0.25, 0.2, 0.1], 'String', ...
    sprintf('EKF RMSE: %.4f\nFGO RMSE: %.4f', rmse_ekf, rmse_fgo), ...
    'FitBoxToText', 'on', 'BackgroundColor', 'white', 'FontSize', 10);