卡尔曼滤波估计一个二维匀速直线运动matlab代码实现

Sure, here's an example of implementing a Kalman filter for estimating a two-dimensional constant velocity linear motion in MATLAB:

% Define the state transition matrix A
A = [1 0 1 0; 0 1 0 1; 0 0 1 0; 0 0 0 1];

% Define the measurement matrix H
H = [1 0 0 0; 0 1 0 0];

% Define the process noise covariance matrix Q
Q = [0.01 0 0 0; 0 0.01 0 0; 0 0 0.01 0; 0 0 0 0.01];

% Define the measurement noise covariance matrix R
R = [0.1 0; 0 0.1];

% Initialize the state vector x (position and velocity)
x = [0; 0; 0; 0];

% Initialize the error covariance matrix P
P = eye(4);

% Initialize the estimated state vector x_hat and error covariance matrix P_hat
x_hat = x;
P_hat = P;

% Generate some noisy measurements
t = linspace(0, 10, 100);
true_state = [sin(t); cos(t); cos(t); -sin(t)]; % True state trajectory
measurements = true_state(1:2, :) + sqrt(R(1,1))*randn(2, length(t));

% Perform the Kalman filter estimation
filtered_state = zeros(4, length(t));
for i = 1:length(t)
    % Predict step
    x_hat_minus = A * x_hat;
    P_minus = A * P_hat * A' + Q;
    
    % Update step
    K = P_minus * H' / (H * P_minus * H' + R);
    x_hat = x_hat_minus + K * (measurements(:, i) - H * x_hat_minus);
    P_hat = (eye(4) - K * H) * P_minus;
    
    % Save the filtered state
    filtered_state(:, i) = x_hat;
end

% Plot the results
figure;
subplot(2, 1, 1);
plot(t, true_state(1,:), 'b-', t, measurements(1,:), 'r.', t, filtered_state(1,:), 'g--');
legend('True state', 'Measurements', 'Filtered state');
xlabel('Time');
ylabel('Position (x)');
title('X-coordinate estimation');

subplot(2, 1, 2);
plot(t, true_state(2,:), 'b-', t, measurements(2,:), 'r.', t, filtered_state(2,:), 'g--');
legend('True state', 'Measurements', 'Filtered state');
xlabel('Time');
ylabel('Position (y)');
title('Y-coordinate estimation');

In this example, the state vector `x` represents the position and velocity in the x and y directions. The state transition matrix `A` describes the dynamics of the system. The measurement matrix `H` maps the state to the measurements. The process noise covariance matrix `Q` represents the uncertainty in the system dynamics, and the measurement noise covariance matrix `R` represents the uncertainty in the measurements.

The code generates some noisy measurements of the true state trajectory and performs the Kalman filter estimation to estimate the true state. The results are then plotted to visualize the estimation performance.

Note that this is a basic implementation and can be further optimized or extended depending on specific requirements and considerations.