利用卡尔曼滤波预测的matlab代码

以下是一个简单的卡尔曼滤波预测的 Matlab 代码示例：

% 定义初始状态向量和协方差矩阵
x = [0; 0]; % 初始状态向量，包含位置和速度
P = [1 0; 0 1]; % 初始协方差矩阵

% 定义系统噪声和测量噪声的方差
Q = [0.1 0; 0 0.1]; % 系统噪声方差
R = 1; % 测量噪声方差

% 定义观测矩阵
H = [1 0]; % 观测矩阵，只能观测到位置

% 定义时间步长和时间序列
dt = 0.1; % 时间步长
t = 0:dt:10; % 时间序列

% 定义真实状态和观测值
x_true = [sin(t); cos(t)]; % 真实状态向量，包含位置和速度
z = x_true(1,:) + randn(size(t)); % 观测值，只能观测到位置并添加了噪声

% 循环执行卡尔曼滤波
for i = 1:length(t)
    % 预测步骤
    x = [1 dt; 0 1] * x; % 预测状态向量，根据运动模型更新位置和速度
    P = [1 dt; 0 1] * P * [1 dt; 0 1]' + Q; % 预测协方差矩阵，根据运动模型更新

    % 更新步骤
    K = P * H' * inv(H * P * H' + R); % 计算卡尔曼增益
    x = x + K * (z(i) - H * x); % 更新状态向量，根据观测值调整位置
    P = (eye(2) - K * H) * P; % 更新协方差矩阵，根据观测值调整

    % 存储预测值
    x_pred(:,i) = x;
end

% 绘图
figure
plot(t, x_true(1,:), 'b', t, z, 'r', t, x_pred(1,:), 'g')
legend('True Position', 'Measured Position', 'Predicted Position')
xlabel('Time (s)')
ylabel('Position (m)')

这段代码演示了如何使用卡尔曼滤波来预测一个运动物体的位置，其中真实状态向量包含位置和速度信息，但观测值只能观测到位置并添加了噪声。代码首先定义了初始状态向量和协方差矩阵，然后定义了系统噪声和测量噪声的方差，以及观测矩阵。接下来，代码循环执行卡尔曼滤波的预测和更新步骤，并存储预测值。最后，代码绘制了真实位置、观测位置和预测位置随时间变化的图像。