卡尔曼滤波 matlab 代码

卡尔曼滤波是一种用于估计系统状态的算法，它可以通过对系统的测量值和模型进行融合，得到更加准确的状态估计。以下是一个简单的卡尔曼滤波 Matlab 代码样例：

%初始化
x = [0; 0]; %状态向量
P = [1 0; 0 1]; %状态协方差矩阵
F = [1 1; 0 1]; %状态转移矩阵
Q = [0.1 0; 0 0.1]; %过程噪声协方差矩阵
H = [1 0]; %观测矩阵
R = 1; %观测噪声协方差

%生成观测数据
z = sin(1:0.1:10) + randn(1, 91);

%卡尔曼滤波
for k = 1:length(z)
    %预测
    x = F * x;
    P = F * P * F' + Q;
    
    %更新
    K = P * H' / (H * P * H' + R);
    x = x + K * (z(k) - H * x);
    P = (eye(2) - K * H) * P;
    
    %保存结果
    x_est(:, k) = x;
end

%绘图
plot(1:length(z), z, 'b', 1:length(z), x_est(1, :), 'r');
legend('观测值', '估计值');

该代码实现了一个简单的一维卡尔曼滤波器，用于估计一个正弦信号的状态。其中，x 表示状态向量，P 表示状态协方差矩阵，F 表示状态转移矩阵，Q 表示过程噪声协方差矩阵，H 表示观测矩阵，R 表示观测噪声协方差。在每个时间步骤中，先进行预测，然后根据观测值进行更新，最后保存估计结果并绘制图像。