最近需要做一個空中飛鼠（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