卡尔曼滤波的matlab代码

卡尔曼滤波是一种用于估计系统状态的优化算法，常用于信号处理和控制系统中。下面是一个简单的卡尔曼滤波的Matlab代码示例：

% 系统模型
A = [1 1; 0 1]; % 状态转移矩阵
B = [0.5; 1];   % 输入矩阵
H = [1 0];      % 观测矩阵

% 过程噪声和测量噪声的协方差矩阵
Q = [0.01 0; 0 0.01];   % 过程噪声协方差
R = 1;                  % 测量噪声方差

% 初始化状态和协方差矩阵
x = [0; 0];             % 初始状态
P = [1 0; 0 1];         % 初始协方差矩阵

% 模拟观测数据
T = 100;                % 时间步数
z = zeros(T, 1);        % 观测数据
for t = 1:T
    % 真实状态更新
    x = A * x + B * randn;
    
    % 生成观测数据
    z(t) = H * x + sqrt(R) * randn;
end

% 卡尔曼滤波
x_est = zeros(2, T);    % 估计的状态
P_est = zeros(2, 2, T); % 估计的协方差矩阵
for t = 1:T
    % 预测步骤
    x_pred = A * x;
    P_pred = A * P * A' + Q;
    
    % 更新步骤
    K = P_pred * H' / (H * P_pred * H' + R);
    x = x_pred + K * (z(t) - H * x_pred);
    P = (eye(2) - K * H) * P_pred;
    
    % 保存估计结果
    x_est(:, t) = x;
    P_est(:, :, t) = P;
end

% 绘制结果
figure;
subplot(2, 1, 1);
plot(1:T, z, 'b', 1:T, H * x_est, 'r');
legend('观测数据', '滤波结果');
xlabel('时间步数');
ylabel('观测值');

subplot(2, 1, 2);
plot(1:T, x_est(1, :), 'b', 1:T, x_est(2, :), 'r');
legend('状态1', '状态2');
xlabel('时间步数');
ylabel('状态值');

这段代码实现了一个简单的一维卡尔曼滤波器，其中系统模型由状态转移矩阵A、输入矩阵B和观测矩阵H定义。过程噪声和测量噪声的协方差矩阵分别由Q和R表示。代码中首先生成了一组模拟观测数据，然后使用卡尔曼滤波算法对观测数据进行滤波估计，并绘制了观测数据和滤波结果的对比图。