卡尔曼滤波 matlab代码.pdf

卡尔曼滤波matlab代码

卡尔曼滤波Matlab代码

卡尔曼滤波是一种递归的状态估计算法，用于估计随时间变化的系统状态，

它通过将过去的观测值与预测模型相结合，得出对当前状态的最优估计。

在Matlab中，我们可以利用内置函数或自己编写的函数来实现卡尔曼滤

波算法。

首先，我们需要定义一个状态空间模型。状态空间模型由状态方程和观测

方程组成。状态方程描述了系统状态如何从先前的状态和控制输入中演化

到当前状态，观测方程描述了如何从系统状态中得出观测值。

在Matlab中，我们可以使用以下代码定义状态方程和观测方程。

matlab

状态方程

A=[11;01];状态转移矩阵

B=[0.5;1];控制输入矩阵

C=[10];观测矩阵

Q=[0.10;00.1];状态噪声协方差矩阵

R=1;观测噪声方差

观测方程

sys=ss(A,B,C,0);

[K,P,E]=lqr(sys,Q,R);最优控制器增益矩阵

上述代码中，`A`是状态转移矩阵，表示系统状态如何从t-1时刻转移到t

时刻。`B`是控制输入矩阵，表示控制输入如何影响系统状态的演化。`C`

是观测矩阵，用于将系统状态映射到观测值。`Q`是状态噪声协方差矩阵，

用于描述系统状态的不确定性。`R`是观测噪声方差，用于描述观测值的不

确定性。

接下来，我们可以利用卡尔曼滤波算法来估计系统状态。在Matlab中，

可以使用`kalman`函数来实现卡尔曼滤波。

matlab

卡尔曼滤波

x0=[0;0];初始状态估计

P0=eye(2);初始估计误差协方差矩阵

观测值

t=0:0.1:10;

u=sin(t);

w=sqrt(Q)*randn(size(t));

v=sqrt(R)*randn(size(t));

x=zeros(2,length(t));

y=zeros(1,length(t));

fork=1:length(t)

系统模型

x(:,k+1)=A*x(:,k)+B*u(k)+w(:,k);

y(:,k)=C*x(:,k)+v(:,k);

end

卡尔曼滤波

x_hat=zeros(size(x));

P=zeros(size(P0));

fork=1:length(t)

预测

x_hat(:,k+1)=A*x_hat(:,k)+B*u(k);

P=A*P*A+Q;

更新

K=P*C/(C*P*C+R);

x_hat(:,k+1)=x_hat(:,k+1)+K*(y(:,k)-C*x_hat(:,k+1));

P=(eye(2)-K*C)*P;

end

在上述代码中，`x0`和`P0`分别是初始状态估计和初始估计误差协方差矩

阵。`x`是真实系统状态，`y`是观测值。在一个循环中，我们首先使用系统

模型生成真实系统状态和观测值。然后，我们使用卡尔曼滤波算法对系统

状态进行估计，包括预测和更新步骤。

最后，我们可以使用Matlab的绘图函数来显示真实系统状态和卡尔曼滤

波估计的状态。

matlab

绘图

figure;

subplot(2,1,1);

plot(t,x(1,1:end-1),r-,t,x_hat(1,1:end-1),b);

xlabel(时间);

ylabel(状态1);

legend(真实状态,估计状态);

subplot(2,1,2);

plot(t,x(2,1:end-1),r-,t,x_hat(2,1:end-1),b);

xlabel(时间);

ylabel(状态2);

legend(真实状态,估计状态);

上述代