卡尔曼滤波算法代码matlab

Sure! Here is a basic implementation of the Kalman filter algorithm in MATLAB:

% Kalman Filter Algorithm

% Initialization
dt = 0.1;                      % Time step
t = 0:dt:10;                   % Time vector
n = length(t);                 % Number of measurements

% Define system dynamics (continuous)
A = [1 dt; 0 1];               % State transition matrix
B = [0.5*dt^2; dt];            % Control input matrix
C = [1 0];                     % Measurement matrix

Q = [0.1^2 0; 0 1];            % Process noise covariance
R = 1;                         % Measurement noise covariance

% Generate true state trajectory
x_true = zeros(2,n);           % True state vector
x_true(:,1) = [0; 0];          % Initial state

for i = 2:n
    x_true(:,i) = A * x_true(:,i-1) + B * sin(t(i));
end

% Generate measurements with noise
y_meas = C * x_true + sqrt(R) * randn(size(t));

% Kalman Filter Initialization
x_est = zeros(2,n);            % Estimated state vector
P_est = zeros(2,2,n);          % Estimated error covariance matrix

x_est(:,1) = [0; 0];           % Initial estimate
P_est(:,:,1) = [100 0; 0 100]; % Initial error covariance

% Kalman Filter Algorithm
for i = 2:n
    % Prediction step
    x_pred = A * x_est(:,i-1);
    P_pred = A * P_est(:,:,i-1) * A' + Q;
    
    % Update step
    K = P_pred * C' / (C * P_pred * C' + R);
    x_est(:,i) = x_pred + K * (y_meas(i) - C * x_pred);
    P_est(:,:,i) = (eye(2) - K * C) * P_pred;
end

% Plot results
figure;
subplot(2,1,1);
plot(t, x_true(1,:), 'b', t, x_est(1,:), 'r--');
xlabel('Time');
ylabel('Position');
legend('True position', 'Estimated position');
title('Kalman Filter - Position Estimation');

subplot(2,1,2);
plot(t, x_true(2,:), 'b', t, x_est(2,:), 'r--');
xlabel('Time');
ylabel('Velocity');
legend('True velocity', 'Estimated velocity');
title('Kalman Filter - Velocity Estimation');

This code implements a simple 1D Kalman filter for estimating the position and velocity of a moving object. The true state trajectory is generated based on a given system dynamics, and noisy measurements are generated using the measurement matrix. The Kalman filter algorithm consists of a prediction step and an update step, where the estimated state and error covariance are updated based on the previous estimates and the current measurements.

Hope this helps! Let me know if you have any other questions.