EKF的matlab代码

EKF是扩展卡尔曼滤波的缩写，它是一种用于状态估计的算法，常用于机器人、导航、控制等领域。下面是一个简单的EKF的matlab代码示例：

% 定义状态方程和观测方程
f = @(x)[x(1)+0.1*x(2); x(2)+0.1*x(3); x(3)+0.1*x(4); x(4)];
h = @(x)[x(1); x(3)];

% 初始化状态向量和协方差矩阵
x = [0;0;0;0];
P = eye(4);

% 定义系统噪声和观测噪声协方差矩阵
Q = diag([0.1,0.1,0.1,0.1]);
R = diag([1,1]);

% 定义观测向量
z = [1.1;0.9];

% EKF算法
for i = 1:100
    % 预测
    x = f(x);
    F = jacobian(f,x);
    P = F*P*F.'+Q;
    
    % 更新
    h = @(x)[x(1); x(3)];
    H = jacobian(h,x);
    K = P*H.'/(H*P*H.'+R);
    x = x + K*(z-h(x));
    P = (eye(4)-K*H)*P;
end

在这个示例中，首先定义了系统的状态方程和观测方程，然后初始化了状态向量和协方差矩阵，以及系统噪声和观测噪声协方差矩阵。接下来进行EKF算法，包括预测和更新两个步骤。其中预测步骤根据状态方程和协方差矩阵计算出预测值和预测协方差矩阵，更新步骤根据观测值和观测方程计算出卡尔曼增益，然后更新状态向量和协方差矩阵。最后，根据算法进行100次迭代，得到最终的状态估计结果。