GNSS单点定位之卡尔曼滤波
滤波
最小二乘法由于未将不同时刻的状态进行关联约束，导致定位结果往往比较粗糙杂乱。滤波是一种降低，分离信号中所含噪声的技术，能够有效的平滑最小二乘的定位结果。运动物体的状态符合牛顿第一定律(任何物体都将保持静止或沿直线匀速运动，直至作用它上面的力迫使它改变当前的状态为止)。即使受到外力的作用，其运动状态的变化也是逐步的，是需要时间。(这里运动中的惯性，跟能量不能突变似乎有异曲同工之妙！)
分析物体运动状态的变化，也就是对运动物体的状态传播进行合理的假设，建立恰当的运动模型。除运动模型之外，选择合适的滤波算法也相当重要，比如一阶低通滤波，卡尔曼滤波等。
卡尔曼滤波(KF)
卡尔曼滤波作为一种误差最小方差估计算法，能够对状态进行最优的无偏估计(适用于线性系统，且噪声为高斯分布)。对于实时应用的大多数 GNSS 用户设备来说，都采用了滤波的导航计算方法。不像单历元定位算法，滤波的导航定位算法使用了之前测量值的信息。先验的时钟偏差和漂移解用来预测当前时钟偏差和时钟漂移，并且先验的用户位置和速度可用来预测当前的用户位置和速度。当前的伪距和伪距率测量值是用来校正预测的导航解。
对于卡尔曼滤波来说，重要的是建立系统预测模型(得出Φ阵)，量测更新模型(得出H阵)，系统噪声Q阵，量测噪声R阵，误差协方差P阵。
在建立系统模型之前，需要先确定状态向量，这里选取8维的向量(3维位置，3维速度，钟差，钟漂)。有了状态向量，就可以对状态向量进行微分，得出其微分方程，从而求解方程，得出当前状态与上一个状态之间的关系，这个关系称之为状态转移矩阵Φ。由于这里的系统模型是线性的(位置的微分是速度，钟差的微分是钟漂)，所以无需求解偏微分矩阵(雅可比)。
需要注意的是根据微分方程得到的关系是系统矩阵，一般称之为F阵，它是状态向量与状态向量微分量之间的转换关系(位置的微分是速度，钟差的微分是钟漂，对应的系数为1；其余无关的就为0)。求解微分方程(直接套用一阶线性微分方程通解公式，或者使用一阶龙格-库塔等), 就能得出状态转移矩阵Φ。(Φ=I + F*t)
量测更新模型，即通过伪距/伪距率来观测状态量，建立观测模型：伪距等于根据当前位置和卫星位置计算的几何距离加上接收机钟差；伪距率等于根据当前速度和卫星速度计算的伪距率加上接收机钟漂。显然，该模型中的伪距/伪距率是状态向量的非线性函数。所以需要使用扩展卡尔曼滤波(EKF)，对观测函数求其偏微分矩阵(雅可比)，得到的矩阵称之为观测矩阵H。
对于Q/R阵，可以先设置大概的值，然后多次实验整定得出。对于 1Hz 测量更新率的伪距测量噪声协方差阵，典型的取值为(1-5m)^2 ，而对于伪距率，取值为 ( 0. 1 - 1m/s) ^2，在较差的 GNSS 接收条件下，应取更大的值。
确定好初始P，Φ，Q，H，R阵之后，还需要得出残差ΔZk，即量测的伪距/伪距率与根据位置/速度/钟差/钟漂来预测计算的伪距/伪距率之间的差值。这里量测的伪距/伪距率要注意的是，需要对伪距进行电离层/对流层/地球自转等误差的校正。
然后计算卡尔曼增益，即可进行状态的更新，P阵的更新，进而迭代。
仿真
% Constants (sone of these could be changed to inputs at a later date)
c = 299792458; % Speed of light in m/s
omega_ie = 7.292115E-5; % Earth rotation rate in rad/s
% Begins
% SYSTEM PROPAGATION PHASE
% 1. Determine transition matrix using (9.147) and (9.150)
Phi_matrix = eye(8);
Phi_matrix(1,4) = tor_s;
Phi_matrix(2,5) = tor_s;
Phi_matrix(3,6) = tor_s;
Phi_matrix(7,8) = tor_s;
% 2. Determine system noise covariance matrix using (9.152)
Q_matrix = zeros(8);
Q_matrix(1:3,1:3) = eye(3) * GNSS_KF_config.accel_PSD * tor_s^3 / 3;
Q_matrix(1:3,4:6) = eye(3) * GNSS_KF_config.accel_PSD * tor_s^2 / 2;
Q_matrix(4:6,1:3) = eye(3) * GNSS_KF_config.accel_PSD * tor_s^2 / 2;
Q_matrix(4:6,4:6) = eye(3) * GNSS_KF_config.accel_PSD * tor_s;
Q_matrix(7,7) = (GNSS_KF_config.clock_freq_PSD * tor_s^3 / 3) +...
GNSS_KF_config.clock_phase_PSD * tor_s;
Q_matrix(7,8) = GNSS_KF_config.clock_freq_PSD * tor_s^2 / 2;
Q_matrix(8,7) = GNSS_KF_config.clock_freq_PSD * tor_s^2 / 2;
Q_matrix(8,8) = GNSS_KF_config.clock_freq_PSD * tor_s;
% 3. Propagate state estimates using (3.14)
x_est_propagated = Phi_matrix * x_est_old;
% 4. Propagate state estimation error covariance matrix using (3.15)
P_matrix_propagated = Phi_matrix * P_matrix_old * Phi_matrix' + Q_matrix;
% MEASUREMENT UPDATE PHASE
% Skew symmetric matrix of Earth rate
Omega_ie = Skew_symmetric([0,0,omega_ie]);
u_as_e_T = zeros(no_meas,3);
pred_meas = zeros(no_meas,2);
% Loop measurements
for j = 1:no_meas
% Predict approx range
delta_r = GNSS_measurements(j,3:5)' - x_est_propagated(1:3);
approx_range = sqrt(delta_r' * delta_r);
% Calculate frame rotation during signal transit time using (8.36)
C_e_I = [1, omega_ie * approx_range / c, 0;...
-omega_ie * approx_range / c, 1, 0;...
0, 0, 1];
% Predict pseudo-range using (9.165)
delta_r = C_e_I * GNSS_measurements(j,3:5)' - x_est_propagated(1:3);
range = sqrt(delta_r' * delta_r);
pred_meas(j,1) = range + x_est_propagated(7);
% Predict line of sight
u_as_e_T(j,1:3) = delta_r' / range;
% Predict pseudo-range rate using (9.165)
range_rate = u_as_e_T(j,1:3) * (C_e_I * (GNSS_measurements(j,6:8)' +...
Omega_ie * GNSS_measurements(j,3:5)') - (x_est_propagated(4:6) +...
Omega_ie * x_est_propagated(1:3)));
pred_meas(j,2) = range_rate + x_est_propagated(8);
end % for j
% 5. Set-up measurement matrix using (9.163)
H_matrix(1:no_meas,1:3) = -u_as_e_T(1:no_meas,1:3);
H_matrix(1:no_meas,4:6) = zeros(no_meas,3);
H_matrix(1:no_meas,7) = ones(no_meas,1);
H_matrix(1:no_meas,8) = zeros(no_meas,1);
H_matrix((no_meas + 1):(2 * no_meas),1:3) = zeros(no_meas,3);
H_matrix((no_meas + 1):(2 * no_meas),4:6) = -u_as_e_T(1:no_meas,1:3);
H_matrix((no_meas + 1):(2 * no_meas),7) = zeros(no_meas,1);
H_matrix((no_meas + 1):(2 * no_meas),8) = ones(no_meas,1);
% 6. Set-up measurement noise covariance matrix assuming all measurements
% are independent and have equal variance for a given measurement type
R_matrix(1:no_meas,1:no_meas) = eye(no_meas) *...
GNSS_KF_config.pseudo_range_SD^2;
R_matrix(1:no_meas,(no_meas + 1):(2 * no_meas)) =...
zeros(no_meas);
R_matrix((no_meas + 1):(2 * no_meas),1:no_meas) =...
zeros(no_meas);
R_matrix((no_meas + 1):(2 * no_meas),(no_meas + 1):(2 * no_meas)) =...
eye(no_meas) * GNSS_KF_config.range_rate_SD^2;
% 7. Calculate Kalman gain using (3.21)
K_matrix = P_matrix_propagated * H_matrix' * inv(H_matrix *...
P_matrix_propagated * H_matrix' + R_matrix);
% 8. Formulate measurement innovations using (3.88)
delta_z(1:no_meas,1) = GNSS_measurements(1:no_meas,1) -...
pred_meas(1:no_meas,1);
delta_z((no_meas + 1):(2 * no_meas),1) = GNSS_measurements(1:no_meas,2) -...
pred_meas(1:no_meas,2);
% 9. Update state estimates using (3.24)