编写卡尔曼滤波器的matlab程序

以下是一个简单的Kalman滤波器在MATLAB中的实现示例：

% Kalman filter example
%
% This example demonstrates a simple Kalman filter. It estimates the
% position and velocity of a moving object based on noisy measurements.
%
% Author: Sebastian Castro
%         https://sebastiancastro.ca

clear all;

% Define the system
dt = 1;             % Time step
A = [1 dt; 0 1];    % State transition matrix
B = [dt^2/2; dt];   % Control matrix
C = [1 0];          % Measurement matrix
u = 0;              % Control input
Q = [0 0; 0 1];     % Process noise covariance
R = 2;              % Measurement noise covariance

% Generate some noisy measurements
T = 50;             % Number of time steps
x = zeros(2, T);    % State vector
y = zeros(1, T);    % Measurement vector
for t = 1:T
    % Update the state
    x(:,t) = A*x(:,t-1) + B*u + mvnrnd([0;0], Q)';
    
    % Take a measurement
    y(t) = C*x(:,t) + normrnd(0, sqrt(R));
end

% Run the Kalman filter
x_hat = zeros(2, T);    % Estimated state vector
P = eye(2);             % Initial error covariance matrix
for t = 1:T
    % Predict the state
    x_hat(:,t) = A*x_hat(:,t-1) + B*u;
    P = A*P*A' + Q;
    
    % Update the estimate
    K = P*C'/(C*P*C' + R);
    x_hat(:,t) = x_hat(:,t) + K*(y(t) - C*x_hat(:,t));
    P = (eye(2) - K*C)*P;
end

% Plot the results
t = 1:T;
figure;
subplot(2,1,1); plot(t, x(1,:), 'b', t, x_hat(1,:), 'r');
title('Position'); legend('true', 'estimated');
subplot(2,1,2); plot(t, x(2,:), 'b', t, x_hat(2,:), 'r');
title('Velocity'); legend('true', 'estimated');