卡尔曼滤波算法原理及应用

卡尔曼滤波是⼀种⾼效率的递归滤波器，它能够从⼀系列的不完全及包含噪声的测量中，估计动态系统的状态。卡尔曼滤波在技术领域有许多的应⽤，常见的有飞机及太空船的导引、导航及控制。

卡尔曼算法主要可以分为两个步骤进⾏：预测和更新。基于最⼩均⽅误差为最佳估计准则，利⽤上⼀时刻的估计值和状态转移矩阵进⾏预测，⽤测量值对预测值进⾏修正，得到当前时刻的估计值。卡尔曼算法公式预测：

ˆ(n|n−1)=Asˆ(n−1|n−1)1. s

2. P(n)=Aξ(n−1)AT+Q更新：

3. G(n)=P(n)CT[CP(n)CT+R]−14. ξ(n)=(I−G(n)C)P(n)

ˆ(n|n)=sˆ(n|n−1)+G(n)[x(n)−Csˆ(n|n−1)]5. s

ˆ(n|n)。利⽤上⾯五个式⼦可以递推得到状态的估计值s⽂章的组织如下：

1.基本模型及假设2.卡尔曼算法原理及推导3.卡尔曼滤波算法举例4.Matlab程序1.基本模型与假设

状态⽅程（描述物体运动状态）

s(n)=As(n−1)+w(n)

测量⽅程（利⽤探测器等器件获取物体状态参数）

x(n)=Cs(n)+v(n)

其中w(n)为过程噪声，v(n)为测量噪声。假设：

w(n)，v(n)，为独⽴零均值的⽩噪声过程，即

E[w(n)wT(k)]=

{Q(n),

0,n=kn≠kn=kn≠k

E[v(n)vT(k)]=

v(n)和s(n)、w(n)不相关，即

{R(n),0,

E[v(n)s(n)]=0E[v(n)w(n)]=0

2.卡尔曼算法原理及推导

基于最⼩均⽅误差准则，通过观测值x(n)求真实信号s(n)的线性⽆偏最优估计。ˆ(n−1|n−1)已知上⼀时刻的估计值s

利⽤状态⽅程对s(n)进⾏预测，最佳预测为

ˆ(n|n−1)=Asˆ(n−1|n−1)s

利⽤测量⽅程对x(n)进⾏预测，最佳预测为

ˆ(n|n−1)=Csˆ(n|n−1)=CAsˆ(n−1|n−1)x

噪声不参与预测。

当n时刻到来时，已知x(n)，可以得到预测误差（新息）

ˆ(n|n−1)=x(n)−CAsˆ(n−1|n−1)α(n)=x(n)−x

ˆ(n|n−1)的修正值。修正后得到对信号的最佳估计为选择合适的系数G(n)对预测误差进⾏加权，作为对预测值s

ˆ(n|n)=sˆ(n|n−1)+G(n)α(n)=Asˆ(n−1|n−1)+G(n)[x(n)−CAsˆ(n−1|n−1)]s

其中G(n)是未知的，因此要求出G(n)的关系式。最佳估计使得均⽅误差最⼩，即

ˆ(n|n))(s(n)−sˆ(n|n))T]=minξ(n)=E[e(n)eT(n)]=E[(s(n)−s

求ξ(n)对G(n)的偏导数并令其等于零，有

∂ξ(n)

∂G(n)

对公式中的两项分别进⾏变换，有

ˆ(n|n)e(n)=s(n)−s

ˆ(n|n−1)−G(n)[x(n)−Csˆ(n|n−1)]=s(n)−s

ˆ(n|n−1)−G(n)[Cs(n)+v(n)−Csˆ(n|n−1)]=s(n)−s

ˆ(n|n−1))−G(n)v(n)=(I−G(n)C)(s(n)−s

ˆ(n|n−1)=Cs(n)+v(n)−Csˆ(n|n−1)=C(s(n)−sˆ(n|n−1))+v(n)x(n)−Cs

令⼀步预测误差

ˆ(n|n−1)e1(n)=s(n)−s

预测误差功率

P(n)=E[e1(n)eT1(n)]

有

ˆ(n|n−1)=Ce(n)+v(n)e(n)=(I−G(n)C)e1(n)−G(n)v(n)=x(n)−Cs1

则有

ˆ(n|n−1))]T}E{e(n)[x(n)−Cs

=E{[(I−G(n)C)e1(n)−G(n)v(n)][Ce1(n)+v(n)]T}

TT=E{[(I−G(n)C)e1(n)−G(n)v(n)][eT1(n)C+v(n)]}TT=E{(I−G(n)C)e1(n)eT1(n)C−G(n)v(n)v(n)}

=2E[e(n)

∂eT(n)

∂G(n)

ˆ(n|n−1)]T}=0]=−2E{e(n)[x(n)−Cs

=(I−G(n)C)P(n)CT−G(n)R

=0

由上式可解得

G(n)=P(n)CT[CP(n)CT+R]−1

ˆ(n|n−1)]=0，有E[e(n)xT(n)]=0，E[e(n)s

E[e(n)xT(n)]=E[e(n)(Cs(n)+v(n))T]=E[e(n)sT(n)]CT+E[e(n)vT(n)]=0

有

E[e(n)sT(n)]=−E[e(n)vT(n)]CT

−1

均⽅误差

ˆ(n|n)]T}=E[e(n)sT(n)]ξ(n)=E[e(n)eT(n)]=E{e(n)[s(n)−s

将其展开并且v(n)和e1(n)不相关，有

ξ(n)=G(n)RCT

−1

=(I−G(n)C)P(n)

可以看出，通过对信号进⾏修正，最⼩均⽅误差⽐预测误差功率⼩G(n)CP(n)其中

ˆ(n−1|n−1))(s(n)−Asˆ(n−1|n−1))T]P(n)=E[(s(n)−As

ˆ(n−1|n−1))(As(n−1)+w(n)−Asˆ(n−1|n−1))T]=E[(As(n−1)+w(n)−As

ˆ(n−1|n−1))+w(n))(A(s(n−1)−sˆ(n−1|n−1))+w(n))T]=E[(A(s(n−1)−s

=E[(Ae(n−1)+w(n))(Ae(n−1)+w(n))T]=Aξ(n−1)AT+Q

要求E[e(n)wT(n)]=0即所有公式都得到。预测：

ˆ(n|n−1)=Asˆ(n−1|n−1)1. s

2. P(n)=Aξ(n−1)AT+Q更新：

3. G(n)=P(n)CT[CP(n)CT+R]−14. ξ(n)=(I−G(n)C)P(n)

ˆ(n|n)=sˆ(n|n−1)+G(n)[x(n)−Csˆ(n|n−1)]5. s

ˆ(n−1|n−1)、ξ(n−1)，（可以设为0）设定初始值s

A，C，Q，R通过测量⽅程、状态⽅程以及噪声⽅差得到。3.卡尔曼滤波算法应⽤

假设有⼀匀速转动模型，⾓度量为α，⾓速度为ω由匀速运动的运动学⽅程可以得到

{假定状态量为s(n)=

α(n)=α(n−1)+ω(n)Δtω(n)=ω(n−1)

[]α(n)ω(n)

s(n)=As(n−1)=

由运动学⽅程可以得到状态⽅程

[][10

Δt1

α(n−1)ω(n−1)

]0

假设过程噪声⽅差为σ1=0.1，则Q=0

[]0σ1

x(n)=Cs(n)+v(n)=[1

0]测量值可以看作状态值加上⼀个测量噪声得到测量⽅程

[]α(n)ω(n)

+v(n)

假设测量噪声⽅差为\\sigma_a=1，则R=\\sigma_a设定初始⾓度为10，⾓速度为2。

初始预测值设为0，即\\hat s(0|0)=0，\\xi(0)=\\begin{bmatrix}0&0\\\\ 0&0\\end{bmatrix}匀速运动

其中蓝⾊直线表⽰的是真实轨迹，可以看到测量值相对于真实轨迹有很⼤的噪声，Kalman算法收敛后可以⽐较好的贴近真实值。匀速运动误差4.Matlab代码

clc;clear all;

T = 0.01; %采样间隔sigma_1 = 0.1;sigma_a = 1;N = 200;

A = [1 T; 0 1];C = [1 0];

Q = [0 0; 0 sigma_1];R = sigma_a;

%v = sqrt(sigma_a)*randn(1,N);%save('v01','v')load('v01');

s = zeros(2,N);

s_estimate = zeros(2,N);x = zeros(1,N);xi = zeros(2);

%给定初始值s(:,1) = [10 2]';

s_estimate(:,1) = [0 0];

%% Kalman 算法for n=2:N

s(:,n) = A*s(:,n-1); %状态⽅程

x(:,n) = C*s(:,n)+v(n); %测量⽅程 %kalman滤波算法 P = A*xi*A'+Q;

G = P*C'*(C*P*C'+R)^(-1);

s_estimate(:,n) = A*s_estimate(:,n-1)+G*(x(:,n)-C*A*s_estimate(:,n-1)); xi = P-G*C*P;end

t = 1:200;figure;

plot(t,s(1,:),t,x,t,s_estimate(1,:));

legend('真实轨迹','测量值','kalman估计值');xlabel('取样点数/ ×0.01s');ylabel('幅值');

error1 = s(1,:)-x;

error2 = s(1,:)-s_estimate(1,:);figure;

plot(t,error1,t,error2);

xlabel('取样点数/ ×0.01s');ylabel('幅值');

legend('测量值-真实值','估计值-真实值');title(['过程噪声⽅差为',num2str(sigma_1)]);

这是我学习Kalman算法时整理的内容，也算是⼀个总结，供⼤家参考。如有不当之处，还请多多见谅。

Processing math: 94%