matlab扩展卡尔曼滤波代码

以下是一个简单的 MATLAB 扩展卡尔曼滤波器的代码示例：

% 初始化
x = [0; 0]; % 状态向量
P = [1 0; 0 1]; % 状态协方差矩阵
A = [1 1; 0 1]; % 状态转移矩阵
Q = [0.001 0; 0 0.001]; % 状态转移协方差矩阵
H = [1 0]; % 观测矩阵
R = 1; % 观测噪声协方差

% 模拟观测值
t = 0:0.1:10;
y = sin(t);

% 扩展卡尔曼滤波
for k = 1:length(t)
    % 预测
    x = A*x;
    P = A*P*A' + Q;
    
    % 更新
    K = P*H'/(H*P*H' + R);
    x = x + K*(y(k) - H*x);
    P = (eye(2) - K*H)*P;
    
    % 保存结果
    x_pred(k,:) = x';
    P_pred(k,:) = reshape(P,1,[]);
end

% 绘图
figure;
subplot(2,1,1);
plot(t,y,'b-',t,x_pred(:,1),'r--');
legend('观测值','预测值');
ylabel('状态1');
subplot(2,1,2);
plot(t,x_pred(:,2),'r--');
legend('预测值');
ylabel('状态2');
xlabel('时间');

此代码实现了一个简单的扩展卡尔曼滤波器，用于估计一个具有两个状态变量的系统的状态，其动态由线性状态转移方程描述，观测由非线性函数映射到观测值。在此示例中，我们模拟了一个正弦波形，并用扩展卡尔曼滤波器估计其状态。