wxwyb

卡尔曼滤波（KF）和扩展卡尔曼滤波（EKF）

最近需要做⼀个空中飞⿏（AirMou）项⽬，其中对六轴陀螺仪回传的数据处理算法中需要⽤到⼀套滤波算法。来滤除⽆⽤的噪声和

⾓度误差。

经过处理效果和可实现性⽐对，有三种滤波算法脱颖⽽出：卡尔曼滤波（KF）、扩展卡尔曼滤波（EKF）以及互补滤波。其中我着重

学习了卡尔曼滤波和扩展卡尔曼滤波，并对扩展卡尔曼滤波进⾏实现。

1.卡尔曼滤波学习

卡尔曼滤波算法是卡尔曼（Kalman）等⼈在20世纪60年代提出的⼀种递推滤波算法。它的实质是以最⼩均⽅误差为估计的最佳准

则，来寻求⼀套递推估计的算法。其基本思想是：采⽤信号与噪声的状态空间模型，利⽤前⼀时刻的估计值和现时刻的观测值来更新对状态

变量的估计，求得现时刻的估计值。它的⼴泛应⽤已经超过30年，包括机器⼈导航，控制，传感器数据融合甚⾄军事⽅⾯的雷达系统以及

导弹追踪等等。

⾸先以⼀维时变随机信号的数学模型为例。对每⼀确定的取样时刻k，x(k)是⼀个随机变量。当取样时刻k在范围内变化时，可得到⼀

个离散的随机序列{x(k)}。

假设待估随机信号的数学模型是⼀个由⽩噪声序列w(k)驱动的⼀阶⾃递归过程，其状态⽅程为：

信号测量过程的数学模型为：

所以可以得到⼀维时变随机信号及其测量过程的数学模型。如下所⽰

⼀维随机信号的递归型估计其的⼀般表达式：

代⼊递归型估计器的⼀般表达式，得：

在实际应⽤中，常⽤到向量卡尔曼滤波算法，因此需要⼀些转化。该转化主要为标量运算和矩阵运算的差异：

修正差异后，可以直接将标量KF推⼴到向量KF。

2.扩展卡尔曼滤波学习

由于卡尔曼滤波估计是⼀个线性随机系统的状态。然⽽实际中，很多系统是⾮线性的，处理这些系统时，⽤扩展卡尔曼滤波（EKF），它是

将期望和⽅差线性化的卡尔曼滤波器。

3.扩展卡尔曼滤波的matlab实现

下⾯这段代码应⽤于空中飞⿏的数据处理中。

q为四元数，wb为⾓度偏置，z，h，y均为中间向量，a为加速度计原数据，w为陀螺仪原数据，dt为积分时间

function[q,wb,z,h,y]=ekf2(a,w,dt)

persistentxP;

%Q

Q=zeros(6,6);

Qwb=.01;

Q(5,5)=Qwb;

Q(6,6)=Qwb;

%R,DMPtake1sconverge,

R=eye(3)*1e4;

%P

ifimpty(P)

P=eye(length(Q))*1e4;

x=[1,0,0,0,0,0]';

end

%%%%%%%%%inputs%%%%%%%%%

%savex_k-1

q0=x(1);q1=x(2);q2=x(3);q3=x(4);

wxb=x(5);%bias

wyb=x(6);

%input

wx=w(1);

wy=w(2);

wz=w(3);

z=a';

%%%%%%%%%%%%%%%%%%

%PopulateFjacobian=d(f)/d(x_k-1/k-1)

F=[1,-(dt/2)*(wx-wxb),-(dt/2)*(wy-wyb),-(dt/2)*(wz),(dt/2)*q1,(dt/2)*q2;

(dt/2)*(wx-wxb),1,(dt/2)*(wz),-(dt/2)*(wy-wyb),-(dt/2)*q0,(dt/2)*q3;

(dt/2)*(wy-wyb),-(dt/2)*(wz),1,(dt/2)*(wx-wxb),-(dt/2)*q3,-(dt/2)*q0;

(dt/2)*(wz),(dt/2)*(wy-wyb),-(dt/2)*(wx-wxb),1,(dt/2)*q2,-(dt/2)*q1;

0,0,0,0,1,0;

0,0,0,0,0,1;];

%%%%%%%%%PREDICT%%%%%%%%%

%Predictedstateestimate

%x_k/k-1=f(x_k-1/k-1,u_k-1)

%refer"Appling",p17,x_t+1=x_t+g(x_t)*ts

x=[q0+(dt/2)*(-q1*(wx-wxb)-q2*(wy-wyb)-q3*(wz));

q1+(dt/2)*(q0*(wx-wxb)-q3*(wy-wyb)+q2*(wz));

q2+(dt/2)*(q3*(wx-wxb)+q0*(wy-wyb)-q1*(wz));

q3+(dt/2)*(-q2*(wx-wxb)+q1*(wy-wyb)+q0*(wz));

wxb;

wyb;];

%

%NormalizeQuaternion

x(1:4)=x(1:4)/norm(x(1:4));

q0=x(1);q1=x(2);q2=x(3);q3=x(4);

%Predictedcovarianceestimate,P_k/k-1=F_k-1*P_k-1/k-1*F_k-1'+Q_k-1

P=F*P*F'+Q;

%%%%%%%%%%UPDATE%%%%%%%%%%%

%%%h(x_k/k-1)

h=[2*q1*q3-2*q0*q2;%referAN4676,page26

2*q0*q1+2*q2*q3;

q0^2-q1^2-q2^2+q3^2;];

%Measurementresidual

%y_k=z_k-h(x_k/k-1),whereh(x)isthematrixthatmapsthestateontothemeasurement

%z_k:Acc,h=[00g/g]*[quaternion->rotation_matrix]

%y(3x1)=z(3x1)-h(3x1)

y=z-h;

%%%%%%%%%%%%%%%%%%

%TheHmatrixmapsthemeasurementtothestates

%H=d(h)/d(x_k/k-1)

H=[-2*q2,2*q3,-2*q0,2*q1,0,0;

2*q1,2*q0,2*q3,2*q2,0,0;

2*q0,-2*q1,-2*q2,2*q3,0,0;];

%%%%%%%%%%%%%%%%%%

%Measurementcovarianceupdate,S_k=H_k*P_k/k-1*H_k'+R_k

S=H*P*H'+R;

%%%%%%%%%%%%%%%%%%

%CalculateKalmangain,K_k=P_k/k-1*H_k'*S_k^-1

%K(7x3)=P(7x7)*H'(7x3)/S(3x3);

K=P*H'/S;

%%%%%%%%%%%%%%%%%%

%Correctedmodelprediction,x_k/k=x_k/k-1+K_k*y_k

%x(7x1)=x(7x1)+K(7x3)*y(3x1)

x=x+K*y;%Outputstatevector

%

%%%%%%%%%%%%%%%%%%

%Updatestatecovariancewithnewknowledge,P_k/k=(I-K_k*H_k)*P_k/k-1

I=eye(length(P));

P=(I-K*H)*P;%Outputstatecovariance

%%%%%%%%%%%%%%%%%%

q=[x(1),x(2),x(3),x(4)];

wb=[x(5),x(6)];

end