kalman滤波matlab代码
时间: 2023-09-20 22:13:11 浏览: 45
Sure! Here's a basic implementation of the Kalman filter in MATLAB:
```matlab
% Define the system dynamics
A = [1 1; 0 1]; % State transition matrix
B = [0.5; 1]; % Control input matrix
C = [1 0]; % Measurement matrix
% Define the process and measurement noise covariance matrices
Q = [0.01 0; 0 0.01]; % Process noise covariance matrix
R = 1; % Measurement noise covariance
% Initialize the state and covariance matrices
x_hat = [0; 0]; % Initial state estimate
P = eye(2); % Initial covariance estimate
% Simulate the system
T = 100; % Number of time steps
X_true = zeros(2, T); % True state values
Y_meas = zeros(1, T); % Measured output values
for t = 1:T
% Generate random process and measurement noise
w = mvnrnd([0 0], Q)';
v = mvnrnd(0, R)';
% Update the true state and measured output
X_true(:,t) = A*X_true(:,t) + B*u + w;
Y_meas(t) = C*X_true(:,t) + v;
% Kalman filter prediction step
x_hat_minus = A*x_hat + B*u;
P_minus = A*P*A' + Q;
% Kalman filter update step
K = P_minus*C'/(C*P_minus*C' + R);
x_hat = x_hat_minus + K*(Y_meas(t) - C*x_hat_minus);
P = (eye(2) - K*C)*P_minus;
end
% Plot the true state and estimated state
figure;
subplot(2,1,1);
plot(1:T, X_true(1,:), 'b-', 1:T, x_hat(1,:), 'r--');
ylabel('State 1');
legend('True state', 'Estimated state');
subplot(2,1,2);
plot(1:T, X_true(2,:), 'b-', 1:T, x_hat(2,:), 'r--');
xlabel('Time step');
ylabel('State 2');
legend('True state', 'Estimated state');
```
This code implements a simple Kalman filter for a two-dimensional system. It assumes a linear state transition and measurement model with Gaussian process and measurement noise. The code generates random noise at each time step to simulate the system dynamics and measurement process. The estimated state is updated based on the measurements using the Kalman filter algorithm.
Please note that this is just a basic implementation and may need customization based on your specific application.