基于matlab的扩展卡尔曼滤波（Extended Kalman Filter，EKF），通过卡尔曼滤波算法近似计算系统的状态估计值和方差估计值,对信号进行滤波。 程序已调通，可直接运行。

打开Matlab编辑器，新建一个名为ekf_demo的脚本文件。咱们今天要玩点有意思的——用扩展卡尔曼滤波给非线性系统做状态估计。直接上硬菜，先来看系统模型。假设我们有个带旋转运动的物体，状态方程可以这样定义：

function x_next = stateFunc(x, dt)
    theta = x(3);  % 旋转角度
    omega = 1.5;   % 角速度固定
    x_next = x + [cos(theta)*dt;
                  sin(theta)*dt;
                  omega*dt];  % 非线性状态方程
end

这个状态函数有意思的地方在于第三维角度变化会产生非线性运动轨迹。dt是采样时间，相当于我们每隔多久预测一次物体位置。

观测模型也不走寻常路，故意设计成极坐标形式：

function z = measFunc(x)
    r = norm(x(1:2));       % 距离观测
    phi = atan2(x(2),x(1)); % 方位角观测
    z = [r; phi];  % 非线性观测方程
end

这样设计是为了模拟雷达这类极坐标测量设备，故意增加非线性难度让EKF有发挥空间。

重点来了——雅可比矩阵计算。这是EKF区别于普通卡尔曼滤波的核心：

function [F, H] = jacobian(x, dt)
    % 状态雅可比
    theta = x(3);
    F = [1, 0, -sin(theta)*dt;
         0, 1,  cos(theta)*dt;
         0, 0, 1];
    
    % 观测雅可比
    q = x(1)^2 + x(2)^2;
    H = [x(1)/sqrt(q), x(2)/sqrt(q), 0;
         -x(2)/q,     x(1)/q,      0]; 
end

这里有个坑：当物体接近原点时，H矩阵可能出现除以零的错误。实际应用中得加个保护机制，咱们演示就假设物体不经过原点了。

基于matlab的扩展卡尔曼滤波（Extended Kalman Filter，EKF），通过卡尔曼滤波算法近似计算系统的状态估计值和方差估计值,对信号进行滤波。 程序已调通，可直接运行。

主程序结构很带感：

% 参数初始化
dt = 0.1;
Q = diag([0.1, 0.1, 0.05]);  % 过程噪声
R = diag([0.5, 0.02]);       % 观测噪声
x_est = [0;0;0];             % 初始估计
P_est = eye(3);              % 初始协方差

for k = 1:100
    % 真实状态生成
    x_true = stateFunc(x_true, dt) + sqrt(Q)*randn(3,1);
    
    % 生成带噪声的观测
    z = measFunc(x_true) + sqrt(R)*randn(2,1);
    
    % EKF预测步
    [F, ~] = jacobian(x_est, dt);
    x_pred = stateFunc(x_est, dt);
    P_pred = F*P_est*F' + Q;
    
    % EKF更新步
    [~, H] = jacobian(x_pred, dt);
    K = P_pred*H'/(H*P_pred*H' + R);
    x_est = x_pred + K*(z - measFunc(x_pred));
    P_est = (eye(3) - K*H)*P_pred;
end

跑起来之后会发现个有趣现象：虽然观测噪声在距离方向上很大（R(1)=0.5），但角度测量精度较高（R(2)=0.02），EKF能巧妙融合这两种信息源。试着把R(2)改大到0.1，会发现轨迹估计明显变飘——这说明方位角测量精度对旋转系统状态估计至关重要。

运行结果用这三行代码可视化：

plot(truth_traj(1,:), truth_traj(2,:), 'b--');
hold on;
plot(est_traj(1,:), est_traj(2,:), 'r-','LineWidth',2);

蓝色虚线是真实轨迹，红色实线是EKF估计结果。你会看到虽然初始阶段有些抖动，但大约5次迭代后估计轨迹就紧紧咬住真实轨迹了。这说明咱们设计的Q、R矩阵比例合适，过程噪声和观测噪声达到了良好平衡。

最后留个思考题：如果把状态方程中的固定角速度改成时变参数，代码需要怎么改？提示得把omega纳入状态变量，这样系统维度就变成4维了，雅可比矩阵也要相应调整。试试看，这会是个不错的进阶练习。