kalman 滤波器 matlab 代码

Kalman滤波器是一种用于估计系统状态的算法。它通过结合系统模型和观测数据来获得更准确的状态估计值。下面是一个用MATLAB编写的Kalman滤波器的示例代码：

% 定义状态转移矩阵
A = [1 1; 0 1];
% 定义测量矩阵
C = [1 0];
% 定义控制输入矩阵
B = [0; 1];
% 定义过程噪声协方差矩阵
Q = [0.1 0; 0 0.1];
% 定义测量噪声协方差矩阵
R = 1;
% 定义初始状态估计值
x_hat = [0; 0];
% 定义初始状态协方差矩阵
P = [1 0; 0 1];
% 定义输入向量
u = 1;

% 定义观测数据
y = [5.1 6 6.9 9.2 10.9];

% Kalman滤波
for i = 1:length(y)
    % 预测步骤
    x_hat_minus = A * x_hat + B * u;
    P_minus = A * P * A' + Q;
    
    % 更新步骤
    K = P_minus * C' / (C * P_minus * C' + R);
    x_hat = x_hat_minus + K * (y(i) - C * x_hat_minus);
    P = (eye(2) - K * C) * P_minus;
    
    disp(['第', num2str(i), '次观测的状态估计值为：']);
    disp(x_hat);
end

在上述代码中，首先定义了状态转移矩阵A、测量矩阵C、控制输入矩阵B、过程噪声协方差矩阵Q和测量噪声协方差矩阵R等参数。然后，定义了初始状态估计值x_hat和初始状态协方差矩阵P。接下来，给定了观测数据y和输入向量u。在循环中，根据Kalman滤波的预测和更新步骤，通过计算估计出每次观测的状态估计值x_hat，并更新状态协方差矩阵P。最后，显示每次观测的状态估计值。