卡尔曼滤波、扩展卡尔曼滤波、无损卡尔曼滤波线性运动模型滤波对比Matlab代码

卡尔曼滤波（Kalman Filter）是一种用于估计系统状态的递归滤波器，它通过融合系统的测量值和预测值来提供最优的状态估计。卡尔曼滤波器假设系统的状态和测量值都是高斯分布，并且系统的动态和测量模型都是线性的。

扩展卡尔曼滤波（Extended Kalman Filter，EKF）是卡尔曼滤波的一种扩展，用于处理非线性系统。EKF通过在每个时间步骤上线性化非线性模型来近似系统的动态和测量模型，然后使用卡尔曼滤波的方法进行状态估计。

无损卡尔曼滤波（Unscented Kalman Filter，UKF）是对EKF的一种改进，它通过使用无损变换（unscented transformation）来近似非线性函数的传播和观测模型。相比于EKF，UKF能够更准确地估计非线性系统的状态。

下面是使用Matlab实现卡尔曼滤波、扩展卡尔曼滤波和无损卡尔曼滤波的简单示例代码：

1. 卡尔曼滤波：

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

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

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

% 测量值
y = [1.2; 2.3; 3.5; 4.7];

% 卡尔曼滤波
x_kalman = zeros(2, length(y));
P_kalman = zeros(2, 2, length(y));
x_kalman(:, 1) = x0;
P_kalman(:, :, 1) = P0;

for k = 2:length(y)
    % 预测步骤
    x_pred = A * x_kalman(:, k-1) + B * u;
    P_pred = A * P_kalman(:, :, k-1) * A' + Q;
    
    % 更新步骤
    K = P_pred * C' / (C * P_pred * C' + R);
    x_kalman(:, k) = x_pred + K * (y(k) - C * x_pred);
    P_kalman(:, :, k) = (eye(2) - K * C) * P_pred;
end

% 输出结果
disp(x_kalman);

2. 扩展卡尔曼滤波：

% 系统动态模型和测量模型（非线性）
f = @(x) [x(1) + x(2); x(2)];
h = @(x) x(1);

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

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

% 测量值
y = [1.2; 2.3; 3.5; 4.7];

% 扩展卡尔曼滤波
x_ekf = zeros(2, length(y));
P_ekf = zeros(2, 2, length(y));
x_ekf(:, 1) = x0;
P_ekf(:, :, 1) = P0;

for k = 2:length(y)
    % 预测步骤
    x_pred = f(x_ekf(:, k-1));
    F = [1 1; 0 1]; % 线性化系统动态模型
    P_pred = F * P_ekf(:, :, k-1) * F' + Q;
    
    % 更新步骤
    H = [1 0]; % 线性化测量模型
    K = P_pred * H' / (H * P_pred * H' + R);
    x_ekf(:, k) = x_pred + K * (y(k) - h(x_pred));
    P_ekf(:, :, k) = (eye(2) - K * H) * P_pred;
end

% 输出结果
disp(x_ekf);

3. 无损卡尔曼滤波：

% 系统动态模型和测量模型（非线性）
f = @(x) [x(1) + x(2); x(2)];
h = @(x) x(1);

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

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

% 测量值
y = [1.2; 2.3; 3.5; 4.7];

% 无损卡尔曼滤波
x_ukf = zeros(2, length(y));
P_ukf = zeros(2, 2, length(y));
x_ukf(:, 1) = x0;
P_ukf(:, :, 1) = P0;

for k = 2:length(y)
    % 预测步骤
    [x_pred, P_pred] = unscented_transform(f, x_ukf(:, k-1), P_ukf(:, :, k-1), Q);
    
    % 更新步骤
    [y_pred, S] = unscented_transform(h, x_pred, P_pred, R);
    C = P_pred * S' / S / S';
    x_ukf(:, k) = x_pred + C * (y(k) - y_pred);
    P_ukf(:, :, k) = P_pred - C * S * C';
end

% 输出结果
disp(x_ukf);

以上是简单的卡尔曼滤波、扩展卡尔曼滤波和无损卡尔曼滤波的Matlab代码示例。请注意，这只是一个简单的演示，实际应用中可能需要根据具体问题进行适当的修改和调整。