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

        最近需要做一个空中飞鼠（AirMouse）项目，其中对六轴陀螺仪回传的数据处理算法中需要用到一套滤波算法。来滤除无用的噪声和角度误差。

        经过处理效果和可实现性比对，有三种滤波算法脱颖而出：卡尔曼滤波（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 )


persistent x P;


% Q
Q=zeros(6,6);
Qwb=.01;
Q(5,5)=Qwb; 
Q(6,6)=Qwb;    


% R, DMP take 1s converge, 
R=eye(3)*1e4;


% P
if isempty(P)    
   P = eye(length(Q))*1e4;
   x = [1, 0, 0, 0, 0, 0]';
end


%%%%%%%%% inputs %%%%%%%%%
%save x_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';


%%%%%%%%%%%%%%%%%%
% Populate F jacobian= 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 %%%%%%%%%
%Predicted state estimate
% 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;];
%
% Normalize Quaternion
x(1:4)=x(1:4)/norm(x(1:4));
q0 = x(1);q1 = x(2);q2 = x(3);q3 = x(4);


% Predicted covariance estimate, 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; % refer AN4676, page 26
     2*q0*q1 + 2*q2*q3;
     q0^2 - q1^2 - q2^2 + q3^2;];


%Measurement residual
% y_k = z_k - h(x_k/k-1), where h(x) is the matrix that maps the state onto the measurement
% z_k: Acc, h=[0 0 g/g]*[quaternion-> rotation_matrix ]
% y(3x1)=z(3x1)-h(3x1)
y = z - h;


%%%%%%%%%%%%%%%%%%
% The H matrix maps the measurement to the states
% 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;];


%%%%%%%%%%%%%%%%%%
% Measurement covariance update,S_k=H_k*P_k/k-1*H_k'+R_k
S = H*P*H' + R;


%%%%%%%%%%%%%%%%%%
% Calculate Kalman gain,K_k=P_k/k-1*H_k'*S_k^-1
% K(7x3) = P(7x7)*H'(7x3)/S(3x3);
K = P*H'/S;
%%%%%%%%%%%%%%%%%%
% Corrected model prediction, x_k/k=x_k/k-1+K_k*y_k
% x(7x1) = x(7x1) + K(7x3)*y(3x1)
x = x + K*y;      % Output state vector
%
%%%%%%%%%%%%%%%%%%
% Update state covariance with new knowledge,P_k/k=(I-K_k*H_k)*P_k/k-1
I = eye(length(P));
P = (I - K*H)*P;  % Output state covariance


%%%%%%%%%%%%%%%%%%
q = [x(1), x(2), x(3), x(4)];
wb = [x(5), x(6)];


end