Learning Kalman filter
by luoshi006
参考:A practical approach to Kalman filter and how to implement it、 Kalman filter wikiPedia
参考 PX4 中 EKF 的matlab实现,并将代码注释于第6节。
1、 introduction
http://mathworld.wolfram.com/KalmanFilter.html
2、 预备知识
3、 定义
卡尔曼滤波器,是一种高效的递归滤波器,其输入是一组随时间变化观测到的数值序列(如:加速度计、陀螺仪),这些观测值 包含噪声。卡尔曼滤波器基于当前和前一时刻的状态,估计出更为精确的系统状态。
这种滤波方法以它的发明者鲁道夫.E.卡尔曼(Rudolph E. Kalman)命名。
4、 噪声来源
加速度计:
当机器人往复运动时,测量的重力加速度会有很多噪声。
陀螺仪:
它随着时间漂移。(就像旋转的陀螺失速时,转轴指向无法维持)【这个栗子(+﹏+)~】
简单说来,就是短时间内,陀螺仪的数据可信度高;长时间以后,加速度计的数据可信度高。
互补滤波
这种情况下,互补滤波器是种比较简单的处理方法。基本原理就是:对加速度计的数据低通滤波,对陀螺仪的数据高通滤波。它没有卡尔曼滤波器准确,但经过调节,也能够简单使用。
互补滤波器和卡尔曼滤波器的对比:
参考: blog
图中:
红色:加速度计;绿色:陀螺仪;蓝色:卡尔曼滤波;黑色:互补滤波;黄色:二阶互补滤波;
可以看到,由于振动的影响,加速度计已被振傻,而陀螺仪的数据越来越飘~
图中:
绿色:卡尔曼;黑色:互补滤波;黄色:二阶互补滤波;
卡尔曼滤波比互补滤波多了延时,但更适应振动。二阶互补滤波曲线不理想,可能是参数的问题。
5、 符号定义
卡尔曼滤波是对观测量的最优估计。首先,它需要知道观测器的测量噪声(measurement noise)和系统的过程噪声(process noise)。这些噪声必须是高斯分布,且均值为零。幸运的是,大部分随机噪声都有这些特性。
更多卡尔曼滤波信息:
5.1 系统状态 xk x k
文中使用的符号沿用wikiPedia Kalman filter。
其中,常数矩阵(与时间无关),不需要标下标
k
k
。即,
Fk
F
k
表示为
F
F
。
另外,还有一些记号表示的解释:
- 上一时刻状态(previous state)
表示: k−1 k − 1 时刻,状态的最优估计
- 预测状态(先验状态)(priori state)
x^k∣k−1 x ^ k ∣ k − 1
表示: k k 时刻,状态的最优预测;由之前的状态和估计得到当前 时刻的状态矩阵估计值。
- 校正状态(后验状态)(posteriori state)
x^k∣k x ^ k ∣ k
表示: k k 时刻,状态的最优估计;由 时刻的观测值校正后的最优估计。
提出问题:系统状态不能直接得到,只能通过观测值 zk z k 得到。称为:隐马儿可夫模型Hidden Markov model
这就意味着,系统状态只能由当前时刻观测值和之前所有的观测状态得到。同时,在卡尔曼滤波稳定之前,得到的状态估计是不可信的。x^ x ^ 表示状态的估计值;
x x 表示状态的真实值(希望得到的值);故:
时刻状态表示为:
xk x kk k 时刻的系统状态由该式给出:
xk x k 是状态矩阵:
xk=[θθ˙b]k x k = [ θ θ ˙ b ] k所以,滤波器的输出是角度 θ θ 和偏差 θ˙b θ ˙ b 。偏差为陀螺仪的偏移量。所以,有陀螺仪的测量值减去该偏差,就得到真是的速度值。
F F 矩阵,状态转换模型,用于上一时刻状态:
uk u k 是系统输入。本文中,它表示陀螺仪在 k k 时刻的读数(单位:°/s),也被称为角速度 。
- 系统状态可重写为:
xk=Fxk−1+Bθ˙k+wk x k = F x k − 1 + B θ ˙ k + w k其中,矩阵 B B 为控制/输入矩阵,定义为:
矩阵 B B 中的 乘以角速度 θ˙ θ ˙ 可以得到角度 θ θ 。但是,偏差(bias)无法直接由角速度计算得到,所以矩阵 B B 底部元素设为0.
- 是过程噪声,均值为0的高斯分布,协方差
Q
Q
与时间 相关:
wk∼N(0,Qk) w k ∼ N ( 0 , Q k )
其中, Qk Q k 是过程噪声的协方差矩阵。在本文中,表示为加速度计和偏差的状态估计的协方差矩阵。本文认为偏差和加速度计的估计是独立的,所以,协方差等于加速度计和偏差的方差的乘积。
最终得到:Qk=[Qθ00Qθ˙]Δt Q k = [ Q θ 0 0 Q θ ˙ ] Δ t可以看出, Qk Q k 协方差矩阵随时间变化,所以,加速度计方差 Qθ Q θ 和偏差的方差 Qθ˙b Q θ ˙ b 需要乘以时间变化量 Δt Δ t 。
所以,距离上一次状态更新的时间越长,过程噪声会越大。例如:陀螺仪的漂移。【??? 存疑,待修正】。必须知道卡尔曼滤波运行所需要的常数(值越大,状态估计的噪声越大):
例如,如果估计的角度开始漂移,就必须增大 Qθ˙b Q θ ˙ b 的值。如果状态变化较慢,角度的估计更加可信,应该减小 Qθ Q θ ,增大加速度计可信度。5.2 观测值 zk z k
现在开始考虑真实状态 xk x k 的观测值 zk z k 。
观测方程由下式给出:zk=Hxk+vk z k = H x k + v k式中,观测值 zk z k 由当前状态 xk x k 乘以矩阵 H H ,再加上测量噪声 。
矩阵 H H 是观测模型,用于将真实的状态映射到观测空间。因为,真实状态是不可直接获取的,测量值是加速度计的读数(加速度角度),测量噪声 vk v k 认为是高斯分布(正态分布),期望为0,协方差为 R R :
以下部分待验证
相同变量的协方差等于其方差,当 R R 不是矩阵时,就等于测量值的方差。详情参见 协方差矩阵的符号分歧。定义 :
R=E[vkvTk]=var(vk) R = E [ v k v k T ] = v a r ( v k )假设测量噪声不随时间变化:
var(vk)=var(v) v a r ( v k ) = v a r ( v )需要注意的是,如果将测量噪声值 var(v) v a r ( v ) 设的太高,滤波器响应时间会变慢,因为新的测量值可信度较低;如果设的太低,可能会造成超调,引入大量噪声,因为此时加速度计可信度较高。
所以,找到过程噪声方差 Qθ Q θ 、 Qθ˙b Q θ ˙ b 和测量噪声 var(v) v a r ( v ) 十分重要。方法较多,另行开盘。。。
5.3
AttitudeEKF.m
以 px4 中的 EKF 代码为例。(该部分代码有matlab生成,在新版中已删除)
function [xa_apo,Pa_apo,Rot_matrix,eulerAngles,debugOutput]... = AttitudeEKF(approx_prediction,use_inertia_matrix,zFlag,dt,z,q_rotSpeed,q_rotAcc,q_acc,q_mag,r_gyro,r_accel,r_mag,J) %LQG Position Estimator and Controller % Observer: % x[n|n] = x[n|n-1] + M(y[n] - Cx[n|n-1] - Du[n]) % x[n+1|n] = Ax[n|n] + Bu[n] % % $Author: Tobias Naegeli $ $Date: 2014 $ $Revision: 3 $ % % % Arguments: % approx_prediction: if 1 then the exponential map is approximated with a % first order taylor approximation. has at the moment not a big influence % (just 1st or 2nd order approximation) we should change it to rodriquez % approximation. % 预测近似方式,1 -- 一阶泰勒展开, % use_inertia_matrix: set to true if you have the inertia matrix J for your % quadrotor % 是否使用转动惯量矩阵; % xa_apo_k: old state vectotr % 之前状态向量 % zFlag: if sensor measurement is available [gyro, acc, mag] % 传感器信号可用(测量值) % dt: dt in s 秒 % z: measurements [gyro, acc, mag] % 测量值 % q_rotSpeed: process noise gyro % 陀螺仪角速度 过程噪声 Q % q_rotAcc: process noise gyro acceleration % 陀螺仪角加速度 过程噪声 Q % q_acc: process noise acceleration % 加速度 过程噪声 Q % q_mag: process noise magnetometer % 磁力计 过程噪声 Q % r_gyro: measurement noise gyro % 陀螺仪 测量噪声 R % r_accel: measurement noise accel % 加速度计 测量噪声 R % r_mag: measurement noise mag % 磁力计 测量噪声 R % J: moment of inertia matrix % 惯性矩,惯性积? % Output: % xa_apo: updated state vectotr % 状态向量 % Pa_apo: updated state covariance matrix % 状态协方差 % Rot_matrix: rotation matrix % 旋转矩阵 % eulerAngles: euler angles % 欧拉角 % debugOutput: not used 调试用; %% model specific parameters
初始化变量:
% compute once the inverse of the Inertia % persistent 申明静态变量; persistent Ji; if isempty(Ji) Ji=single(inv(J)); end %% init % 起飞时的初始值; persistent x_apo if(isempty(x_apo)) gyro_init=single([0;0;0]); gyro_acc_init=single([0;0;0]); acc_init=single([0;0;-9.81]); mag_init=single([1;0;0]); % NED 中指北; x_apo=single([gyro_init;gyro_acc_init;acc_init;mag_init]); % single 转换为单精度; end persistent P_apo if(isempty(P_apo)) % P_apo = single(eye(NSTATES) * 1000); P_apo = single(200*ones(12)); % 200 表示 P 协方差很大,数据可信度低; end debugOutput = single(zeros(4,1)); %% copy the states wx= x_apo(1); % x body angular rate wy= x_apo(2); % y body angular rate wz= x_apo(3); % z body angular rate wax= x_apo(4); % x body angular acceleration way= x_apo(5); % y body angular acceleration waz= x_apo(6); % z body angular acceleration zex= x_apo(7); % x component gravity vector zey= x_apo(8); % y component gravity vector zez= x_apo(9); % z component gravity vector mux= x_apo(10); % x component magnetic field vector muy= x_apo(11); % y component magnetic field vector muz= x_apo(12); % z component magnetic field vector
6、 卡尔曼滤波器
卡尔曼滤波器,主要用于估计当前时刻 k k 的系统状态。主要分为预测、校正两步。
6.1 预测
状态预测
滤波器通过先前状态和陀螺仪的测量值,预测当前的状态。得到预测状态(先验状态priori state)
x^k∣k−1=Fx^k−1∣k−1+Bθ˙k x ^ k ∣ k − 1 = F x ^ k − 1 ∣ k − 1 + B θ ˙ k%% prediction section % compute the apriori state estimate from the previous aposteriori estimate %body angular accelerations if (use_inertia_matrix==1) wak =[wax;way;waz]+Ji*(-cross([wax;way;waz],J*[wax;way;waz]))*dt; else wak =[wax;way;waz]; end %body angular rates % 更新 角速度; wk =[wx; wy; wz] + dt*wak; %derivative of the prediction rotation matrix % 更新后的旋转矩阵 求导; O=[0,-wz,wy;wz,0,-wx;-wy,wx,0]'; %prediction of the earth z vector % 更新 加速度计 向量 % 任何旋转向量都可以表示为斜对称矩阵 O 的指数 % 这里的指数是以泰勒级数定义的矩阵乘法。O 矩阵叫做旋转的“生成元” % 此部分概念请参考 【李代数】。 if (approx_prediction==1) %e^(Odt)=I+dt*O+dt^2/2!O^2 % so we do a first order approximation of the exponential map zek =(O*dt+single(eye(3)))*[zex;zey;zez]; else zek =(single(eye(3))+O*dt+dt^2/2*O^2)*[zex;zey;zez]; %zek =expm2(O*dt)*[zex;zey;zez]; not working because use double %precision end %prediction of the magnetic vector if (approx_prediction==1) %e^(Odt)=I+dt*O+dt^2/2!O^2 % so we do a first order approximation of the exponential map muk =(O*dt+single(eye(3)))*[mux;muy;muz]; else muk =(single(eye(3))+O*dt+dt^2/2*O^2)*[mux;muy;muz]; %muk =expm2(O*dt)*[mux;muy;muz]; not working because use double %precision end % 预测状态: x_apr=[wk;wak;zek;muk];
误差协方差预测
根据先前的误差协方差矩阵 Pk−1∣k−1 P k − 1 ∣ k − 1 ,预测当前的先验误差协方差矩阵 Pk∣k−1 P k ∣ k − 1 :
Pk∣k−1=FPk−1∣k−1FT+Qk P k ∣ k − 1 = F P k − 1 ∣ k − 1 F T + Q k该矩阵用于估计当前状态估计的可信度。值越小,当前状态估计的可信度越高。
由上式可知,如果不进行校正(更新),误差协方差会一直累积。% compute the apriori error covariance estimate from the previous %aposteriori estimate EZ=[0,zez,-zey; -zez,0,zex; zey,-zex,0]'; MA=[0,muz,-muy; -muz,0,mux; muy,-mux,0]'; E=single(eye(3)); Z=single(zeros(3)); % 根据 状态向量 求出; A_lin=[ Z, E, Z, Z Z, Z, Z, Z EZ, Z, O, Z MA, Z, Z, O]; A_lin=eye(12)+A_lin*dt; %process covariance matrix persistent Q if (isempty(Q)) Q=diag([ q_rotSpeed,q_rotSpeed,q_rotSpeed,... q_rotAcc,q_rotAcc,q_rotAcc,... q_acc,q_acc,q_acc,... q_mag,q_mag,q_mag]); end P_apr=A_lin*P_apo*A_lin'+Q;
6.2 校正
新息
首先,由当前的观测值 zk z k 和预测状态 xk∣k−1 x k ∣ k − 1 计算出新息:y˜k=zk−Hx^k∣k−1 y ~ k = z k − H x ^ k ∣ k − 1观测阵 H H 用于将预测状态 映射到观测空间(加速度计观测);
新息是向量形式【?】:
y^k=[y^]k y ^ k = [ y ^ ] k%% update if zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==1 % R=[r_gyro,0,0,0,0,0,0,0,0; % 0,r_gyro,0,0,0,0,0,0,0; % 0,0,r_gyro,0,0,0,0,0,0; % 0,0,0,r_accel,0,0,0,0,0; % 0,0,0,0,r_accel,0,0,0,0; % 0,0,0,0,0,r_accel,0,0,0; % 0,0,0,0,0,0,r_mag,0,0; % 0,0,0,0,0,0,0,r_mag,0; % 0,0,0,0,0,0,0,0,r_mag]; R_v=[r_gyro,r_gyro,r_gyro,r_accel,r_accel,r_accel,r_mag,r_mag,r_mag]; %observation matrix %[zw;ze;zmk]; H_k=[ E, Z, Z, Z; Z, Z, E, Z; Z, Z, Z, E]; y_k=z(1:9)-H_k*x_apr;
新息协方差:
Sk=HPk∣k−1HT+R S k = H P k ∣ k − 1 H T + R式中:观测阵 H H 用于将误差协方差预测矩阵 映射到观测空间;
该式根据误差协方差预测矩阵 Pk∣k−1 P k ∣ k − 1 和测量协方差矩阵 R R 估计出观测值的可信度。测量噪声越大, 的值越大。即,当前测量值可信度较低。本文中, S S 是向量:
卡尔曼增益
卡尔曼增益用于表征新息的可信度。Kk=Pk∣k−aHTS−1k K k = P k ∣ k − a H T S k − 1可以得出,若新息可信度低,新息协方差 S S 较大;状态估计可信度高,误差协方差矩阵 较小,此时,卡尔曼增益 Kk K k 较小。反之,同理。
式中,观测阵 H H 的转置用于将误差协方差矩阵 映射到观测空间。
启动时不知道状态,可以将误差协方差矩阵设为:P=[L00L],(L:largenumber) P = [ L 0 0 L ] , ( L : l a r g e n u m b e r )若启动状态完全已知,且陀螺仪已校准,可将误差协方差矩阵初始化为零阵。
本文中,卡尔曼增益为2*1的矩阵:
K=[K0K1] K = [ K 0 K 1 ]%S_k=H_k*P_apr*H_k'+R; S_k=H_k*P_apr*H_k'; S_k(1:9+1:end) = S_k(1:9+1:end) + R_v; K_k=(P_apr*H_k'/(S_k));
校正
x^k∣k=x^k∣k−1+Kky˜k x ^ k ∣ k = x ^ k ∣ k − 1 + K k y ~ k式中,新息 y˜k y ~ k 为观测值 zk z k 和预测状态 x^k∣k−1 x ^ k ∣ k − 1 的差,所以,正负均有可能。
Pk|k=(I−KkH)Pk|k−1 P k | k = ( I − K k H ) P k | k − 1式中: I I 为单位矩阵。
滤波器根据当前状态估计的可信度,修正误差协方差矩阵。使得可以由误差协方差预测矩阵 Pk|k−1 P k | k − 1 和新息协方差 Sk S k 得到系统状态。
x_apo=x_apr+K_k*y_k; P_apo=(eye(12)-K_k*H_k)*P_apr;
7、 参考文献中 Implement
A practical approach to Kalman filter and how to implement it
该部分的 C 代码,需根据实际系统修改。
step 1:
x^k|k−1=Fx^k−1|k−1+Bθ˙k x ^ k | k − 1 = F x ^ k − 1 | k − 1 + B θ ˙ k[θθ˙b]k|k−1=[10−Δt1][θθ˙b]k−1|k−1+[Δt0]θ˙k [ θ θ ˙ b ] k | k − 1 = [ 1 − Δ t 0 1 ] [ θ θ ˙ b ] k − 1 | k − 1 + [ Δ t 0 ] θ ˙ k=[θ−θ˙bΔtθ˙b]k−1|k−1+[Δt0]θ˙k = [ θ − θ ˙ b Δ t θ ˙ b ] k − 1 | k − 1 + [ Δ t 0 ] θ ˙ k=[θ−θ˙bΔt+θ˙Δtθ˙b] = [ θ − θ ˙ b Δ t + θ ˙ Δ t θ ˙ b ]=[θ+Δt(θ˙−θ˙b)θ˙b] = [ θ + Δ t ( θ ˙ − θ ˙ b ) θ ˙ b ]rate = newRate - bias; angle += dt * rate;
step 2:
Pk|k−1=FPk−1|k−1FT+Qk P k | k − 1 = F P k − 1 | k − 1 F T + Q k[P00P10P01P11]k|k−1=[10−Δt1][P00P10P01P11]k−1|k−1[1−Δt01]+[Qθ00Qθ˙b]Δt [ P 00 P 01 P 10 P 11 ] k | k − 1 = [ 1 − Δ t 0 1 ] [ P 00 P 01 P 10 P 11 ] k − 1 | k − 1 [ 1 0 − Δ t 1 ] + [ Q θ 0 0 Q θ ˙ b ] Δ t=[P00−ΔtP10P10P01−ΔtP11P11]k−1|k−1[1−Δt01]+[Qθ00Qθ˙b]Δt = [ P 00 − Δ t P 10 P 01 − Δ t P 11 P 10 P 11 ] k − 1 | k − 1 [ 1 0 − Δ t 1 ] + [ Q θ 0 0 Q θ ˙ b ] Δ t=[P00−ΔtP10−Δt(P01−ΔtP11)P10−ΔtP11P01−ΔtP11P11]k−1|k−1+[Qθ00Qθ˙b]Δt = [ P 00 − Δ t P 10 − Δ t ( P 01 − Δ t P 11 ) P 01 − Δ t P 11 P 10 − Δ t P 11 P 11 ] k − 1 | k − 1 + [ Q θ 0 0 Q θ ˙ b ] Δ t=[P00−ΔtP10−Δt(P01−ΔtP11)+QθΔtP10−ΔtP11P01−ΔtP11P11+Qθ˙bΔt] = [ P 00 − Δ t P 10 − Δ t ( P 01 − Δ t P 11 ) + Q θ Δ t P 01 − Δ t P 11 P 10 − Δ t P 11 P 11 + Q θ ˙ b Δ t ]=[P00+Δt(ΔtP11−P01−P10+Qθ)P10−ΔtP11P01−ΔtP11P11+Qθ˙bΔt] = [ P 00 + Δ t ( Δ t P 11 − P 01 − P 10 + Q θ ) P 01 − Δ t P 11 P 10 − Δ t P 11 P 11 + Q θ ˙ b Δ t ]P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle); P[0][1] -= dt * P[1][1]; P[1][0] -= dt * P[1][1]; P[1][1] += Q_gyroBias * dt;
step 3:
y~k=zk−Hx^k|k−1 y ~ k = z k − H x ^ k | k − 1=zk−[10][θθ˙b]k|k−1 = z k − [ 1 0 ] [ θ θ ˙ b ] k | k − 1=zk−θk|k−1 = z k − θ k | k − 1y = newAngle - angle;
step 4:
Sk=HPk|k−1HT+R S k = H P k | k − 1 H T + R=[10][P00P10P01P11]k|k−1[10]+R = [ 1 0 ] [ P 00 P 01 P 10 P 11 ] k | k − 1 [ 1 0 ] + R=P00k|k−1+R = P 00 k | k − 1 + R=P00k|k−1+var(v) = P 00 k | k − 1 + v a r ( v )S = P[0][0] + R_measure;
step 5:
Kk=Pk|k−1HTS−1k K k = P k | k − 1 H T S k − 1[K0K1]k=[P00P10P01P11]k|k−1[10]S−1k [ K 0 K 1 ] k = [ P 00 P 01 P 10 P 11 ] k | k − 1 [ 1 0 ] S k − 1=[P00P10]k|k−1S−1k = [ P 00 P 10 ] k | k − 1 S k − 1=[P00P10]k|k−1Sk = [ P 00 P 10 ] k | k − 1 S k注意:此处 S S 是标量,但 可以是矩阵,此时,需要计算矩阵的逆。
K[0] = P[0][0] / S; K[1] = P[1][0] / S;
step 6:
x^k|k=x^k|k−1+Kky~k x ^ k | k = x ^ k | k − 1 + K k y ~ k[θθ˙b]k|k=[θθ˙b]k|k−1+[K0K1]ky~k [ θ θ ˙ b ] k | k = [ θ θ ˙ b ] k | k − 1 + [ K 0 K 1 ] k y ~ k=[θθ˙b]k|k−1+[K0y~K1y~]k = [ θ θ ˙ b ] k | k − 1 + [ K 0 y ~ K 1 y ~ ] kangle += K[0] * y; bias += K[1] * y;
step 7:
Pk|k=(I−KkH)Pk|k−1 P k | k = ( I − K k H ) P k | k − 1[P00P10P01P11]k|k=([1001]−[K0K1]k[10])[P00P10P01P11]k|k−1 [ P 00 P 01 P 10 P 11 ] k | k = ( [ 1 0 0 1 ] − [ K 0 K 1 ] k [ 1 0 ] ) [ P 00 P 01 P 10 P 11 ] k | k − 1=([1001]−[K0K100]k)[P00P10P01P11]k|k−1 = ( [ 1 0 0 1 ] − [ K 0 0 K 1 0 ] k ) [ P 00 P 01 P 10 P 11 ] k | k − 1=[P00P10P01P11]k|k−1−[K0P00K1P00K0P01K1P01] = [ P 00 P 01 P 10 P 11 ] k | k − 1 − [ K 0 P 00 K 0 P 01 K 1 P 00 K 1 P 01 ]float P00_temp = P[0][0]; float P01_temp = P[0][1]; P[0][0] -= K[0] * P00_temp; P[0][1] -= K[0] * P01_temp; P[1][0] -= K[1] * P00_temp; P[1][1] -= K[1] * P01_temp;
一些建议参数:
float Q_angle = 0.001; float Q_gyroBias = 0.003; float R_measure = 0.03;
注意事项:
- 如果要求滤波器启动时输出有意义,需要给定初试状态;
未完待续。。。。
- 预测状态(先验状态)(priori state)