卡尔曼滤波matlab

卡尔曼滤波（Kalman Filter）是一种利用状态方程和观测方程对系统状态进行估计的算法。在Matlab中，可以使用`kalman`函数进行卡尔曼滤波。

假设我们有一个线性系统，其状态方程和观测方程如下：

状态方程：x(k+1) = A*x(k) + w(k)
观测方程：y(k) = H*x(k) + v(k)

其中，x(k)表示系统在时刻k的状态，w(k)和v(k)分别表示系统噪声和观测噪声，A和H为系统矩阵。

我们需要对系统状态进行估计，假设我们已经知道了系统的初值x(0)，以及系统矩阵A和H，我们可以使用`kalman`函数进行卡尔曼滤波，代码如下：

% 系统矩阵
A = [1 1; 0 1];
H = [1 0];

% 系统噪声和观测噪声协方差矩阵
Q = eye(2);
R = 1;

% 初始状态和协方差矩阵
x0 = [0; 0];
P0 = eye(2);

% 生成系统状态和观测数据
T = 100;
x = zeros(2, T);
y = zeros(1, T);
for k = 1:T
    % 系统状态方程
    x(:, k+1) = A*x(:, k) + mvnrnd([0; 0], Q)';
    % 观测方程
    y(:, k) = H*x(:, k) + sqrt(R)*randn();
end

% 卡尔曼滤波
x_kf = zeros(2, T);
x_kf(:, 1) = x0;
P_kf = P0;
for k = 1:T
    % 预测
    x_kf(:, k+1) = A*x_kf(:, k);
    P_kf = A*P_kf*A' + Q;
    % 更新
    K = P_kf*H'/(H*P_kf*H' + R);
    x_kf(:, k+1) = x_kf(:, k+1) + K*(y(:, k) - H*x_kf(:, k+1));
    P_kf = (eye(2) - K*H)*P_kf;
end

% 绘图
figure;
plot(x(1,:), 'b');
hold on;
plot(x_kf(1,:), 'r');
legend('真实状态', '卡尔曼滤波估计');

上述代码中，我们首先定义了系统矩阵A和观测矩阵H，以及系统噪声和观测噪声的协方差矩阵Q和R。然后我们生成了一个长度为T的系统状态和观测数据，接着使用`kalman`函数进行卡尔曼滤波，最后绘制真实状态和卡尔曼滤波估计的状态曲线。

需要注意的是，在实际应用中，我们需要根据具体问题选择合适的系统矩阵和观测矩阵，以及噪声协方差矩阵。同时，对于非线性系统，我们需要使用扩展卡尔曼滤波（Extended Kalman Filter）或无迹卡尔曼滤波（Unscented Kalman Filter）等算法进行估计。