编写卡尔曼滤波器matlab代码

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

% 假设我们有一些初始状态和测量值
x = [0; 0]; % 初始状态向量
P = [1 0; 0 1]; % 初始协方差矩阵
z = [1.2; 0.9]; % 测量向量

% 定义系统动态模型
A = [1 1; 0 1]; % 状态转移矩阵
B = [0; 1]; % 输入转移矩阵
u = 0; % 输入向量
Q = [0.01 0; 0 0.01]; % 系统噪声协方差矩阵
R = [0.1 0; 0 0.1]; % 测量噪声协方差矩阵

% 卡尔曼滤波器主体循环
for k = 1:100
    % 预测
    x = A * x + B * u;
    P = A * P * A' + Q;
    
    % 更新
    K = P * inv(P + R);
    x = x + K * (z - x);
    P = (eye(2) - K) * P;
    
    % 生成一些随机噪声作为测量值
    z = [1.2; 0.9] + randn(2,1) * sqrt(R(1,1));
end

% 输出结果
disp(x);

这个代码演示了一个简单的卡尔曼滤波器，其中我们假设有一个二维状态向量和一个二维测量向量。我们使用矩阵来描述状态转移、输入转移、系统噪声和测量噪声。在主体循环中，我们首先进行预测步骤，然后根据测量值进行更新步骤。最后，我们输出最终的状态向量。当然，这只是一个简单的例子，实际上卡尔曼滤波器的实现会更加复杂。