一、噪声
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、量测方程离散化:
参考链接: