卡尔曼滤波matlab代码

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

% 状态转移矩阵
A = [1 1; 0 1]; 

% 系统噪声协方差矩阵
Q = [0.01 0; 0 0.01]; 

% 观测矩阵
H = [1 0]; 

% 观测噪声协方差矩阵
R = 1; 

% 初始状态向量
x0 = [0; 0]; 

% 初始状态协方差矩阵
P0 = [1 0; 0 1]; 

% 生成模拟数据
N = 100; 
x = zeros(2, N); 
y = zeros(1, N); 
for k = 1:N
    x(:, k) = A * x0 + chol(Q) * randn(2, 1); 
    y(k) = H * x(:, k) + sqrt(R) * randn(1); 
    x0 = x(:, k); 
end

% 卡尔曼滤波
xkf = zeros(2, N); 
Pkf = zeros(2, 2, N); 
xkf(:, 1) = x0; 
Pkf(:, :, 1) = P0; 
for k = 2:N
    % 预测
    xkf(:, k) = A * xkf(:, k-1); 
    Pkf(:, :, k) = A * Pkf(:, :, k-1) * A' + Q; 
    
    % 更新
    K = Pkf(:, :, k) * H' / (H * Pkf(:, :, k) * H' + R); 
    xkf(:, k) = xkf(:, k) + K * (y(k) - H * xkf(:, k)); 
    Pkf(:, :, k) = (eye(2) - K * H) * Pkf(:, :, k); 
end

% 画图
figure; 
plot(1:N, x(1, :), 'b-', 1:N, xkf(1, :), 'r--'); 
legend('实际值', '卡尔曼滤波值'); 
xlabel('时间'); 
ylabel('状态值'); 

上述代码实现了一个简单的一维卡尔曼滤波，用于估计一个具有随机游走模型的状态序列。在代码中，我们首先设置了状态转移矩阵 `A`、系统噪声协方差矩阵 `Q`、观测矩阵 `H`、观测噪声协方差矩阵 `R`、初始状态向量 `x0` 和初始状态协方差矩阵 `P0`。

接下来，我们生成了一个模拟数据集，其中包括状态序列 `x` 和观测序列 `y`。在卡尔曼滤波循环中，我们首先进行状态预测，并计算预测状态协方差矩阵。然后，我们根据观测值进行状态更新，并计算新的状态协方差矩阵。最后，我们将卡尔曼滤波估计的状态序列 `xkf` 与实际状态序列 `x` 进行比较，并进行画图展示。

请注意，这只是一个简单的卡尔曼滤波代码示例，实际中可能需要更复杂的模型和更多的参数调整。