kalman滤波实现状态估计的代码
时间: 2023-10-26 22:07:42 浏览: 34
Kalman滤波是一种常用的状态估计方法,在嵌入式系统、机器人、自动控制等领域得到广泛应用。下面是一个基于Matlab实现的Kalman滤波状态估计代码示例。
```
% 系统参数
Ts = 0.1; % 采样时间
A = [1 Ts; 0 1]; % 系统矩阵
B = [Ts^2/2; Ts]; % 输入矩阵
C = [1 0]; % 观测矩阵
Q = [0.1 0; 0 Ts^2/3]; % 系统噪声协方差
R = 1; % 观测噪声协方差
% 状态初始化
x0 = [0; 0]; % 初始状态
P0 = [1 0; 0 1]; % 初始协方差
% 生成随机信号
t = 0:Ts:10;
u = 0.1*randn(1,length(t)); % 输入信号
z = C*x0 + sqrt(R)*randn(1); % 初始观测信号
x_true = zeros(2,length(t)); % 真实状态
x_est = zeros(2,length(t)); % 估计状态
x_true(:,1) = x0;
x_est(:,1) = x0;
% Kalman滤波
for k = 2:length(t)
% 真实状态
x_true(:,k) = A*x_true(:,k-1) + B*u(k-1) + sqrt(Q)*randn(2,1);
% 观测信号
z(k) = C*x_true(:,k) + sqrt(R)*randn(1);
% 状态估计
x_pred = A*x_est(:,k-1) + B*u(k-1);
P_pred = A*P0*A' + Q;
K = P_pred*C'/(C*P_pred*C' + R);
x_est(:,k) = x_pred + K*(z(k) - C*x_pred);
P0 = (eye(2) - K*C)*P_pred;
end
% 绘图
figure;
subplot(211);
plot(t,x_true(1,:),'r',t,x_est(1,:),'b');
legend('真实状态','估计状态');
subplot(212);
plot(t,x_true(2,:),'r',t,x_est(2,:),'b');
legend('真实状态','估计状态');
```
其中,`Ts`为采样时间,`A`、`B`、`C`分别为系统矩阵、输入矩阵和观测矩阵,`Q`为系统噪声协方差,`R`为观测噪声协方差,`x0`为初始状态,`P0`为初始协方差,`u`为输入信号,`z`为观测信号,`x_true`为真实状态,`x_est`为估计状态。在循环中,先计算真实状态和观测信号,然后进行状态估计,更新状态和协方差。最后绘制真实状态和估计状态的图像。