卡尔曼滤波matlab

卡尔曼滤波是一种常用的状态估计方法，用于估计系统状态。在MATLAB中，可以使用“kalman”函数来实现卡尔曼滤波。下面是一个简单的示例，演示如何使用kalman函数来估计一个匀加速直线运动的小车的位置和速度。

% 定义系统模型
dt = 0.1; % 时间步长
A = [1 dt; 0 1]; % 状态转移矩阵
B = [0.5*dt^2; dt]; % 输入转移矩阵
C = [1 0]; % 观测矩阵
Q = [0.1 0; 0 0.01]; % 状态噪声协方差矩阵
R = 1; % 观测噪声方差

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

% 生成仿真数据
T = 50; % 时间长度
u = 1*ones(T,1); % 输入
w = mvnrnd([0;0],Q,T)'; % 状态噪声
v = mvnrnd(0,R,T)'; % 观测噪声
x_true = zeros(2,T); % 真实状态
x_true(:,1) = x0;
z = zeros(1,T); % 观测值
for k=2:T
    x_true(:,k) = A*x_true(:,k-1) + B*u(k-1) + w(:,k-1);
    z(k) = C*x_true(:,k) + v(k);
end

% 使用kalman函数进行卡尔曼滤波
x_est = zeros(2,T); % 估计状态
x_est(:,1) = x0;
P = P0;
for k=2:T
    [x_est(:,k),P] = kalman(x_est(:,k-1),P,u(k-1),z(k),A,B,C,Q,R);
end

% 绘制结果
subplot(2,1,1);
plot(1:T,x_true(1,:),1:T,x_est(1,:),'--');
legend('真实位置','估计位置');
subplot(2,1,2);
plot(1:T,x_true(2,:),1:T,x_est(2,:),'--');
legend('真实速度','估计速度');

在这个示例中，我们首先定义了系统模型，包括状态转移矩阵A和输入转移矩阵B，以及状态噪声协方差矩阵Q和观测噪声方差R。然后，我们初始化状态和协方差矩阵，并生成了仿真数据。最后，我们使用kalman函数进行卡尔曼滤波，得到估计的状态和协方差矩阵，并将结果绘制出来。

需要注意的是，kalman函数的输入参数包括当前状态估计xk，当前状态协方差矩阵Pk，当前输入uk，当前观测值zk，以及系统模型参数A、B、C、Q和R。输出包括新的状态估计xk+1和新的状态协方差矩阵Pk+1。