Kalman滤波中状态方程初始化参数设置_卡尔曼滤波初始值设定

arcdeg = pi/180;   
%初始化参数
att = [0;0;90]*arcdeg; vn0 = [0;0;0]; pos0 = [[34;108]*arcdeg;100];        % att -- 欧拉角；arcdeg -- 度转换为弧度；vn -- 速度；pos -- 位置
qnb0 = a2qua(att);  
qnb = qnb0;  vn = vn0;  pos = pos0;                                        % 姿态、速度和位置初始化
%添加初始误差
phi = [0.1; 0.2; 3]*arcmin;  qnb = qaddphi(qnb, phi);                      % 失准角
eb = [0.01;0.015;0.02]*dph;  web = [0.001;0.001;0.001]*dpsh;               % 陀螺常值零偏，角度随机游走                                                                         
db = [80;90;100]*ug;         wdb = [1;1;1]*ugpsHz;                         % 加速度计常值偏值，速度随机游走
                                                                           % ug=ge*1e-6; g0 = ge; ge -- 赤道重力； ugpsHz= ug/sqrt(1);

%初始化kalman参数                                                                          
Qk = diag([web; wdb; zeros(9,1)])^2*nts;                                   % Qk系统噪声
rk = [[0.1;0.1;0.1];[[10;10]/Re;10]];  Rk = diag(rk)^2;                    % Rk量测噪声
P0 = diag([[0.1;0.1;10]*arcdeg; [1;1;1]; [[10;10]/Re;10];                  % Re赤道长半轴
           [0.1;0.1;0.1]*dph; [80;90;100]*ug])^2;                          % 协方差矩阵，x = [phi, delta_vn, delta_p, eb, db]
Hk = [zeros(6,3),eye(6),zeros(6)];
kf = kfinit(Qk, Rk, P0, zeros(15), Hk); 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

$x_k^{¯}$：表示k时刻先验状态估计值，这是算法根据前次迭代结果（就是上一次循环的后验估计值）做出的不可靠估计。
$x_k$、$x_{k-1}$：分别表示k时刻、k-1时刻后验状态估计值，也就是要输出的该时刻最优估计值，这个值是卡尔曼滤波的结果。
$A$ ：表示状态转移矩阵，是n×n阶方阵，它是算法对状态变量进行预测的依据，状态转移矩阵如果不符合目标模型有可能导致滤波发散。$A$一般都是由n*n的单位阵加上SINS误差转移矩阵构成。

$B$ ：表示可选的控制输入u∈R的增益，在大多数实际情况下并没有控制增益。

$u_{k-1}$：表示k-1时刻的控制增益，一般没有这个变量，可以设为0。

$P_k^{¯}$ ：表示k时刻的先验估计协方差矩阵，对角矩阵，这个协方差矩阵只要确定了一开始的$P_0$，后面都可以递推出来，而且初始协方差矩阵 $P_0$只要不是为0，它的取值对滤波效果影响很小，都能很快收敛。一般将滤波状态初值设置为真值附近的某值，有时甚至直接设置为零向量。

$P_k$、$P_{k-1}$：分别表示k时刻、k-1时刻的后验估计协方差，是滤波结果之一。

$Q$：表示过程激励噪声的协方差，它是状态转移矩阵与实际过程之间的误差。这个矩阵是卡尔曼滤波中比较难确定的一个量，一般有两种思路：一是在某些稳定的过程可以假定它是固定的矩阵，通过寻找最优的Q值使滤波器获得更好的性能，这是调整滤波器参数的主要手段，Q一般是对角阵，且对角线上的值很小，便于快速收敛；二是在自适应卡尔曼滤波（AKF）中Q矩阵是随时间变化的。

$K_k$ ：表示卡尔曼增益，是滤波的中间结果。

$z_k$ ：表示测量值，是m阶向量。

$H$：表示量测矩阵，是m×n阶矩阵，它把m维测量值转换到n维与状态变量相对应。

$R$：表示测量噪声协方差，它是一个数值，这是和仪器相关的一个特性，作为已知条件输入滤波器。需要注意的是这个值过大过小都会使滤波效果变差，且R取值越小收敛越快，所以可以通过实验手段寻找合适的R值再利用它进行真实的滤波。

欲启动Kalman滤波器，必须预设初始值$P_0$和$x_0^{¯}$。对于可观测性较强的状态分量，对应的状态初值和均方误差阵设置偏差容许适当大些，它们随着滤波更新将会快速收敛，如果均方误差阵设置太小，则会使收敛速度变慢。