捷联惯导基础知识解析之三（噪声和kalman滤波）

一、噪声

1、一阶马尔可夫过程

β为反相关时间常数

连续时间一阶马尔可夫过程表示为：

离散化后为：

一般在组合导航中，采用allan方差中的Bias Instibility 参数和τ 当作随机噪声和反相关时间常数；

2、随机游走

连续时间随机游走表示为：

其中w(t)为激励高斯白噪声，并且：

离散化为，如下：

在仿真IMU数据时，一般我们都会添加噪声，采样过程是两相邻时刻的增量，添加的噪声也应该是两相邻时刻的增量；

因此，添加随机游走时，

添加零偏时，

添加一阶马尔可夫时，

3、随机常值：

连续时间随机常值为：

上式离散化为：

综上，一阶马尔可夫、随机游走、随机常值的离散形式表达式可以看出：

当β=0时，随机游走和一阶马尔可夫一样；当随机游走中噪声强度（方差）为0时，随机常值和随机游走一样！

二、离散化（针对INS/GNSS组合导航系统的Kalman）

Kalman滤波全套全套算法的五个公式：

其中，滤波增益计算公式K，两种表达方式：

滤波流程框架图：

带确定性输入（wb、fb）的状态空间模型可表示为：

1、状态方程离散化：

其中，各个矩阵的计算如下：

其中，Qk在kalman滤波5个方程中的第二个会用到；如下，Qk=q(tk-1)*T，

即，严格按照上面的单位：实现代码如下：

其中kf.Q为：

kf.Q  = diag([imu.arw, imu.vrw, imu.gb_psd, imu.ab_psd].^2);

% Discretization of continous-time system
kf.A =  expm(kf.F * dt);          				% Exact solution for linerar systems
% S.A = I + (S.F * dt);         				% Approximated solution by Euler method 
kf.Qd = (kf.G * kf.Q * kf.G') .* dt;            % Digitalized covariance matrix

% Step 1, predict the a priori state vector xi
kf.xi = kf.A * kf.xp;

% Step 2, update the a priori covariance matrix Pi
kf.Pi = (kf.A * kf.Pp * kf.A') + kf.Qd;
kf.Pi =  0.5 .* (kf.Pi + kf.Pi');                  % Force Pi to be symmetric matrix

2、量测方程离散化：

 

 

 

 

 

 

参考链接：

Random Walks