序贯滤波的matlab代码

由于序贯滤波的实现方式有多种，这里提供一种基于MATLAB的实现方式的代码示例：

% 初始化
x = [0; 0; 0; 0]; % 状态向量
P = eye(4); % 状态协方差矩阵
Q = diag([0.1, 0.1, 0.1, 0.1]); % 状态噪声协方差矩阵
R = diag([0.5, 0.5]); % 观测噪声协方差矩阵
H = [1, 0, 0, 0; 0, 1, 0, 0]; % 观测矩阵

% 生成模拟数据
t = 0:0.1:10;
x_true = [sin(t); cos(t); -sin(t); -cos(t)];
z = x_true + sqrt(R)*randn(2, length(t));

% 序贯滤波
for i = 1:length(t)
    % 预测
    x = x;
    P = P + Q;
    
    % 更新
    K = P*H'/(H*P*H' + R);
    x = x + K*(z(:,i) - H*x);
    P = (eye(4) - K*H)*P;
    
    % 保存结果
    x_est(:,i) = x;
end

% 绘图
figure;
plot(t, x_true(1,:), 'b', t, x_est(1,:), 'r');
legend('True', 'Estimate');
xlabel('Time');
ylabel('State');

该代码实现了一个简单的二维运动模型的序贯滤波，其中状态向量为位置和速度，观测向量为位置。代码中包括了初始化、模拟数据生成、序贯滤波和绘图四个部分。在序贯滤波部分，首先进行预测，然后根据观测值进行更新，最后保存结果。绘图部分将真实值和估计值进行比较。