卡尔曼滤波原理+matlab

卡尔曼滤波是一种用于状态估计的算法，可以用于估计系统状态的变化，同时考虑系统的噪声和不确定性。其基本思想是通过将先验估计和测量结果进行加权平均，来得到一个更准确的估计值，从而实现对系统状态的估计和预测。在实际应用中，卡尔曼滤波广泛应用于信号处理、控制工程、导航系统等领域。

下面是一个简单的用 MATLAB 实现的卡尔曼滤波算法示例：

% 系统模型
A = [1 1; 0 1];
B = [0.5; 1];
C = [1 0];

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

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

% 生成随机信号
N = 100;
w = sqrt(Q) * randn(2, N);
v = sqrt(R) * randn(1, N);

% 初始化变量
x_est = x0;
P_est = P0;
x_pred = zeros(2, N);
P_pred = zeros(2, 2, N);

% 卡尔曼滤波
for k = 1:N
    % 预测
    x_pred(:, k) = A * x_est + B * w(:, k);
    P_pred(:, :, k) = A * P_est * A' + Q;
    
    % 更新
    K = P_pred(:, :, k) * C' / (C * P_pred(:, :, k) * C' + R);
    x_est = x_pred(:, k) + K * (v(k) - C * x_pred(:, k));
    P_est = (eye(2) - K * C) * P_pred(:, :, k);
end

% 绘图
t = 1:N;
figure;
plot(t, x_pred(1, :), '--', t, x_est(1, :), '-', t, v, '-.');
legend('预测', '估计', '测量');
xlabel('时间');
ylabel('状态');

上面的代码演示了一个简单的线性系统状态估计问题。其中，系统模型由状态转移矩阵 A 和输入矩阵 B 定义，测量矩阵 C 为单位矩阵。状态和测量噪声协方差矩阵 Q 和 R 分别为 0.01 和 1。初始状态估计和协方差矩阵分别为 [0; 0] 和单位矩阵。随机信号通过 randn 函数生成，最终结果通过 plot 函数绘制。