卡尔玛二维滤波matlab代码

以下是一个简单的二维卡尔曼滤波的 MATLAB 代码实现。假设我们要估计一个平面上运动物体的位置和速度，观测数据包括物体的位置（x,y）。

% 定义系统状态转移矩阵 A 和观测矩阵 H
dt = 0.1; % 时间间隔
A = [1 0 dt 0; 0 1 0 dt; 0 0 1 0; 0 0 0 1]; % 状态转移矩阵，假设匀加速直线运动
H = [1 0 0 0; 0 1 0 0]; % 观测矩阵，只能观测到位置

% 定义系统噪声和观测噪声的协方差矩阵
Q = [0.01 0 0 0; 0 0.01 0 0; 0 0 0.01 0; 0 0 0 0.01]; % 系统噪声协方差矩阵，假设位置和速度的噪声方差相等
R = [0.1 0; 0 0.1]; % 观测噪声协方差矩阵，假设位置的噪声方差相等

% 初始化状态向量和协方差矩阵
x = [0; 0; 0; 0]; % 初始状态，假设位置和速度都为零
P = [1 0 0 0; 0 1 0 0; 0 0 1 0; 0 0 0 1]; % 初始协方差矩阵

% 定义观测数据
t = 0:dt:10; % 时间
x_true = [sin(t); cos(t); cos(t); -sin(t)]; % 真实状态，假设运动轨迹为 sin 和 cos 函数
y = x_true(1:2,:) + 0.1*randn(2,length(t)); % 观测数据，假设位置有噪声

% 卡尔曼滤波
x_filtered = zeros(4,length(t)); % 滤波后的状态向量，包含位置和速度
for i = 1:length(t)
    % 预测步骤
    x_pred = A * x; % 预测状态
    P_pred = A * P * A.' + Q; % 预测协方差矩阵

    % 更新步骤
    K = P_pred * H.' * inv(H * P_pred * H.' + R); % 计算卡尔曼增益
    x = x_pred + K * (y(:,i) - H * x_pred); % 更新状态
    P = (eye(4) - K * H) * P_pred; % 更新协方差矩阵

    % 保存滤波后的状态向量
    x_filtered(:,i) = x;
end

% 绘制结果
figure;
plot(x_true(1,:),x_true(2,:),'r',y(1,:),y(2,:),'k.',x_filtered(1,:),x_filtered(2,:),'b');
legend('真实轨迹','观测值','滤波轨迹');
xlabel('x');
ylabel('y');

该代码中，首先定义了系统状态转移矩阵 `A` 和观测矩阵 `H`，以及系统噪声和观测噪声的协方差矩阵 `Q` 和 `R`。然后初始化状态向量和协方差矩阵，定义观测数据。接着，使用 for 循环对每个时间步骤进行卡尔曼滤波，包括预测步骤和更新步骤，并保存滤波后的状态向量。最后，绘制真实轨迹、观测值和滤波轨迹的对比图。