宽带专家网站,建站工具,百度极速版,网站开发行业怎么样原始 Markdown文档、Visio流程图、XMind思维导图见#xff1a;https://github.com/LiZhengXiao99/Navigation-Learning PSINS 中的 Kalman 滤波代码都在百行以内#xff0c;没调用什么函数#xff0c;而且通用性很强#xff0c;拿去让 AI 解释#xff0c;效果挺好。 文章目… 原始 Markdown文档、Visio流程图、XMind思维导图见https://github.com/LiZhengXiao99/Navigation-Learning PSINS 中的 Kalman 滤波代码都在百行以内没调用什么函数而且通用性很强拿去让 AI 解释效果挺好。 文章目录 一、RLSRLSUDUD分解RLSUDUT 二、kalman滤波KFUDUD分解Kalman滤波kfc2d连续时间Kalman滤波离散化 三、ekf扩展卡尔曼滤波四、UKF无迹卡尔曼滤波ukfUTutpoint 五、ssukfSSUT 六、CKF容积卡尔曼滤波ckfCT 七、PF粒子滤波八、akfupdate自适应滤波shamn Sage-Husa自适应滤波 九、抗差等价权函数igg1igg3 十、数据融合POSFusionPOSSmoothPOSProcessingPOSProcessing_ifk_test 十一模拟htwn产生高斯白噪声vfbfx模拟垂直下落物体的状态方程vfbhx模拟垂直下落的物体的测量方程vfbhx模拟垂直下落的物体的测量方程 一、RLS
RLS递归最小二乘估计是一种常用的自适应滤波算法用于估计线性系统的参数1。
RLS估计的基本思想是通过对输入信号进行迭代最小化误差平方和来逐步调整估计器的增益系数从而不断优化估计器的性能。具体来说RLS估计的步骤包括1
初始化估计器的增益系数和误差方差。对于每个输入信号采样计算输出信号的预测值和实际值之间的误差。根据误差和当前增益系数计算出新的误差方差和增益系数。重复执行步骤2和3进行迭代优化直到满足收敛条件为止。
function kf RLS(kf, zk)
% Recursive Least Square filter.
%
% Prototype: kf RLS(kf, zk)
% Inputs: kf - filter structure array
% zk - measurement vector
% Output: kf - filter structure array after filtering
%
% See also kfupdate, RLSPot, RLSUD.% Copyright(c) 2009-2014, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 16/09/2013if ~isfield(kf, Rk) kf.Rk eye(size(kf.Hk,1));endkf.Pxzk kf.Pxk*kf.Hk; kf.Pzk kf.Hk*kf.Pxzk kf.Rk; kf.Kk kf.Pxzk*kf.Pzk^-1;kf.xk kf.xk kf.Kk*(zk-kf.Hk*kf.xk);kf.Pxk kf.Pxk - kf.Kk*kf.Pxzk;kf.Pxk (kf.Pxkkf.Pxk)*0.5;这是一个递归最小二乘Recursive Least SquareRLS滤波器的函数。它用于更新滤波器结构体数组 kf并根据测量向量 zk 进行滤波操作。
函数的输入参数有 kf 和 zk其中 kf 是滤波器的结构体数组zk 是测量向量。函数的输出参数也是 kf表示经过滤波操作后的滤波器结构体数组。
函数首先判断 kf 结构体数组中是否存在 Rk 字段如果不存在则初始化 Rk 为单位矩阵。然后计算 Pxzk Pxk * Hk’Pzk Hk * Pxzk Rk。接着计算增益矩阵 Kk Pxzk * Pzk^-1。然后更新状态向量 xk xk Kk * (zk - Hk * xk)。再更新 Pxk Pxk - Kk * Pxzk’。最后对 Pxk 进行对称化处理使其成为对称矩阵。
这段代码的作用是实现递归最小二乘滤波器的更新过程用于估计系统状态。
RLSUDUD分解RLS
function [U, D, K, X] RLSUD(U, D, H, R, Z, X)
% Recursive Least Square filter using UD decomposition.
%
% Prototype: [U, D, K, X] RLSUD(U, D, H, R, Z, X)
% Inputs: U,D - factors of UD decomposition
% H,R - 1-row measurement matrix / measurement var
% X,Z - state / measurement
% Outputs: U,D,X - as input after update
% K - gain
%
% See also KFUD, RLS, kfupdate.% Copyright(c) 2009-2023, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 30/01/2023if isstruct(U) % kf RLSUD(kf, z);[U.Uk, U.Dk, ~, U.xk] RLSUD(U.Uk, U.Dk, U.Hk, U.Rk, D, U.xk);return;endif nargin4, R1; endn length(D);f (H*U); g D.*f; afa f*gR;for jn:-1:1afa0 afa - f(j)*g(j); lambda -f(j)/afa0;D(j) afa0/afa*D(j); afa afa0;for i(j-1):-1:1s (i1):(j-1);U(i,j) U(i,j) lambda*(g(i)U(i,s)*g(s));endendif nargout2K U*(D.*(H*U))/R;if nargout3, X X K*(Z-H*X); endend该代码是一个递归最小二乘滤波器的实现使用了UD分解技术。以下是其功能的解释
输入参数 UUD分解中的左奇异矩阵。DUD分解中的右奇异矩阵。H1行测量矩阵。R测量噪声的方差。Z测量值。X状态值。 输出参数 U更新后的左奇异矩阵。D更新后的右奇异矩阵。X更新后的状态值仅在输出参数中存在。K增益仅在输出参数中存在。 功能解释 首先代码检查输入的参数是否为结构体。如果是结构体则表示调用函数的方式为 kf RLSUD(kf, z);即用当前的状态值 kf.xk 和测量值 z 更新结构体变量 kf。在这种情况下函数直接返回更新后的结构体变量 kf不进行其他计算。如果输入参数不是结构体则继续执行下面的代码。首先根据输入的参数计算中间变量 f、g 和 afa。其中f (H*U) 表示将 H 与 U 进行矩阵乘积的转置g D.*f 表示将 D 与 f 进行逐元素相乘afa f*gR 表示将 f 与 g 进行逐元素相乘并将结果加上 R。接下来通过循环逐个处理矩阵的列从最后一列开始逆序迭代。在每一列处理过程中首先计算新的增益系数 lambda根据递推公式计算新的对角线元素 D(j)并更新上三角矩阵 U 的相应元素。具体的计算过程如下 首先计算临时变量 afa0 afa - f(j)*g(j)表示将 afa 减去与当前列的乘积项。然后计算增益系数 lambda -f(j)/afa0其中负号表示使用最小二乘法。接下来根据递推公式计算新的对角线元素 D(j) afa0/afa*D(j)。最后使用增益系数 lambda 和中间变量 g(i)、U(i,s) 更新上三角矩阵 U 的相应元素。 在完成所有列的处理后如果输出参数要求计算增益 K则根据公式 K U*(D.(H*U))/R 计算增益其中 . 表示矩阵乘积的转置。如果输出参数要求更新状态值 X则根据公式 X X K*(Z-H*X) 更新状态值。其中Z-H*X 表示预测误差。 总结该代码实现了一个递归最小二乘滤波器利用UD分解技术对矩阵进行更新和计算增益。它通过对矩阵逐列处理的方式逐步更新滤波器的状态和增益系数以实现滤波器的滤波功能。
UDUT
function [U, D] UDUT(P)
% UDUT decomposition, so that P U*diag(D)*U.
%
% Prototype: [U, D] UDUT(P)
% Input: P - nonnegative define symmetic matrix
% Outputs: U - unit upper-triangular matrix
% D - vector representation of diagonal matrix
%
% See also chol1, qrmgs.% Copyright(c) 2009-2016, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 12/11/2016n length(P);U eye(n); D zeros(n,1); trPn trace(P)/n*1e-40;for jn:-1:1k (j1):n;D(j) P(j,j) - (U(j,k).^2)*D(k);if D(j)trPn, continue; endfor i(j-1):-1:1U(i,j) (P(i,j)-(U(i,k).*U(j,k))*D(k)) / D(j);endend该代码实现了UDUT分解Upper-Triangular Decomposition with Diagonal Similarity Transform将一个非负定对称矩阵P分解为U * diag(D) * U’的形式。 2. 从最后一行开始依次向上处理每一行。 3. 首先计算当前行j的对角线元素D(j)通过用当前行的上三角元素U(j,k)和D(k)的平方进行插值减去对角线元素P(j,j)。 4. 如果D(j)小于等于trPn说明该元素非常小对分解结果影响不大因此跳过对该行的处理。 5. 如果D(j)大于trPn则需要计算当前行j和之前行i的元素U(i,j)通过用当前行的元素P(i,j)和之前的上三角元素U(i,k)和U(j,k)以及D(k)进行插值除以D(j)。 6. 循环处理完所有行后输出U和D即为UDUT分解的结果。
这段代码的目的是将一个非负定对称矩阵进行UDUT分解以便进行后续的计算和分析。UDUT分解可以用于求解线性方程组、计算矩阵的行列式、计算矩阵的特征值和特征向量等应用中。
二、kalman滤波
卡尔曼滤波Kalman filter是一种高效率的递归滤波器自回归滤波器它能够从一系列的不完全及包含噪声的测量中估计动态系统的状态1。
卡尔曼滤波的主要思想是利用状态转移矩阵和观测矩阵结合当前状态和上一时刻的状态来估计当前的最优状态1。具体来说卡尔曼滤波的步骤包括1
初始化滤波器的状态向量和协方差矩阵。对于每个时间步长执行以下步骤 利用状态转移矩阵和当前状态向量计算出下一时刻的状态向量和协方差矩阵。结合观测矩阵和当前观测向量计算出当前时刻的状态估计值和协方差矩阵。根据新的状态估计值和协方差矩阵更新滤波器的状态向量和协方差矩阵。 重复执行步骤2进行迭代优化直到满足收敛条件为止。
与传统的滤波器相比卡尔曼滤波具有更好的数值稳定性和计算效率能够更准确地估计状态变量的值。
function [Xk, Pxk, Xkk_1, Pxkk_1] kalman(Phikk_1, Gammak, Qk, Xk_1, Pxk_1, Hk, Rk, Zk, s)
% A simple Kalman filter By Yan Gongminif nargin9, s1; endXkk_1 Phikk_1*Xk_1;Pxkk_1 Phikk_1*s*Pxk_1*Phikk_1 Gammak*Qk*Gammak;Pxykk_1 Pxkk_1*Hk;Pykk_1 Hk*Pxykk_1 Rk;Kk Pxykk_1*Pykk_1^-1;Xk Xkk_1 Kk*(Zk-Hk*Xkk_1);Pxk Pxkk_1 - Kk*Pykk_1*Kk;Pxk (PxkPxk)/2;这段代码是一个简单的卡尔曼滤波器的实现。卡尔曼滤波器是一种用于估计动态系统状态的方法通过结合先前的估计和最新的观测数据来更新当前状态的估计。
以下是代码的功能解释
输入参数 Phikk_1状态转移矩阵描述系统状态的转移方式。Gammak过程噪声协方差矩阵描述系统过程中引入的噪声。Qk过程噪声协方差矩阵描述系统过程中引入的噪声。Xk_1先前的状态估计。Zk观测数据。s可选参数用于调整卡尔曼增益的平滑因子默认为1。 初始化 Xkk_1根据状态转移矩阵和先前的状态估计计算得到当前状态的预测值。Pxkk_1根据状态转移矩阵、过程噪声协方差矩阵和先前的状态估计误差协方差矩阵计算得到当前状态预测值的误差协方差矩阵。 预测 Pxykk_1将当前状态预测值的误差协方假矩阵转换为预测的状态和观测之间的协方差矩阵。Pykk_1根据观测矩阵、预测的状态和观测之间的协方差矩阵以及观测噪声协方差矩阵计算得到预测的观测噪声协方差矩阵。 更新 Kk通过预测的状态和观测之间的协方差矩阵以及预测的观测噪声协方差矩阵的逆计算得到卡尔曼增益。Xk根据当前状态预测值和观测数据的卡尔曼增益对当前状态进行更新。Pxk根据当前状态更新后的误差协方差矩阵通过减去卡尔曼增益与预测的观测噪声协方差矩阵的乘积的转置得到更新后的误差协方差矩阵。Pxkk_1当前状态预测值误差协方差矩阵。
该代码实现了一个简单的卡尔曼滤波器可以根据输入的参数进行状态估计和预测并输出当前状态的估计值以及误差协方差矩阵等相关信息。
KFUDUD分解Kalman滤波
function [U, D, K] KFUD(U, D, Phi, Tau, Q, H, R)
% UDUT square root Kalman filter.
%
% Prototype: [U, D, K] KFUD(U, D, Phi, Tau, Q, H, R)
% Inputs: U - unit upper-triangular matrix
% D - vector representation of diagonal matrix
% Phi, Tau, Q - system matrix, processing noise distribusion matrix
% noise variance (vector representation)
% H, R - measurement matrix, measurement noise variance
% Outputs: U - unit upper-triangular matrix
% D - vector representation of diagonal matrix
% K - Kalman filter gain
%
% See also RLSUD, UDUT, chol1, qrmgs.% Copyright(c) 2009-2016, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 12/11/2016n length(U);W [Phi*U, Tau]; D1 [D; Q]; % NOTE: D,Q are both vectormeanD mean(D)/n*1e-40;for jn:-1:1 % time updatingD(j) (W(j,:).*W(j,:))*D1;for i1:(j-1)if D(j)meanD, U(i,j) 0;else U(i,j) (W(i,:).*W(j,:))*D1/D(j); endW(i,:) W(i,:) - U(i,j)*W(j,:);endendif ~exist(R, var), return; endf (H*U); g D.*f; afa f*gR;for jn:-1:1 % measurement updatingafa0 afa - f(j)*g(j); lambda -f(j)/afa0;D(j) afa0/afa*D(j); afa afa0;for i1:(j-1)s (i1):(j-1);U(i,j) U(i,j) lambda*(g(i)U(i,s)*g(s));endendK U*(D.*(H*U))/R;这个函数实现了一种称为通用对角线卡尔曼滤波UDUT square root Kalman filter算法。UD分解卡尔曼滤波是一种扩展的状态估计算法适用于处理非线性系统。
函数接受以下输入参数
U单位上三角矩阵用于表示状态转移矩阵。D一个向量表示对角线矩阵的元素。Phi系统矩阵表示系统的动态模型。Tau一个向量表示处理噪声的分布矩阵也称为过程噪声。Q一个向量表示过程噪声的方差以向量形式表示。H测量矩阵表示如何从状态中获取测量值。R测量噪声的方差。
函数返回以下输出参数
U更新后的单位上三角矩阵。D更新后的对角线矩阵的元素向量。K卡尔曼滤波增益。
算法的主要步骤如下
初始化根据输入参数初始化一些变量包括计算过程中的临时变量。时间更新从后向前遍历每一个时间步根据系统的动态模型和过程噪声计算每个时间步的状态预测和对角线矩阵的元素。如果预测的对角线元素小于一个阈值meanD则将其设为零以避免过小的数值引起数值不稳定。测量更新如果存在测量噪声方差即R不为零则进行测量更新步骤。首先计算预测的测量残差f和残差的权重g。然后从后向前遍历每个时间步根据测量残差和权重更新对角线矩阵的元素和对角线元素上的权重。接着根据更新的对角线元素更新卡尔曼滤波增益K。返回输出返回更新后的单位上三角矩阵、对角线矩阵的元素向量和卡尔曼滤波增益。
这个函数的输出可以用于进一步的状态估计或控制应用中。
kfc2d连续时间Kalman滤波离散化
function [Phikk_1, Qk] kfc2d(Ft, Qt, Ts, n)
% For Kalman filter system differential equation, convert continuous-time
% model to discrete-time model: Ft-Phikk_1, Qt-Qk.
%
% Prototype: [Phikk_1, Qk] kfc2d(Ft, Qt, Ts, n)
% Inputs: Ft - continous-time system transition matrix
% Qt - continous-time system noise variance matrix
% Ts - discretization time interval
% n - the order for Taylor expansion
% Outputs: Phikk_1 - discrete-time transition matrix
% Qk - discrete-time noise variance matrix
% Alogrithm notes:
% Phikk_1 I Ts*Ft Ts^2/2*Ft^2 Ts^3/6*Ft^3
% Ts^4/24*Ft^4 Ts^5/120*Ft^5 ...
% Qk M1*Ts M2*Ts^2/2 M3*Ts^3/6 M4*Ts^4/24 M5*Ts^5/120 ...
% where M1 Qt; M2 Ft*M1(Ft*M1); M3 Ft*M2(Ft*M2);
% M4 Ft*M3(Ft*M3); M5 Ft*M4(Ft*M4); ...
%
% See also kfinit, kffk, kfupdate, kffeedback.% Copyright(c) 2009-2014, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 05/08/2012Phikk_1 eye(size(Ft)) Ts*Ft;Qk Qt*Ts;if nargin4 % n1return;endTsi Ts; facti 1; Fti Ft; Mi Qt;for i2:1:nTsi Tsi*Ts; facti facti*i;Fti Fti*Ft;Phikk_1 Phikk_1 Tsi/facti*Fti; % Phikk_1FtMi Ft*Mi;Mi FtMi FtMi;Qk Qk Tsi/facti*Mi; % Qkend 这段代码是一个用于将连续时间Kalman滤波器的系统差分方程转换为离散时间模型的函数。函数名为kfc2d其输入参数包括连续时间系统的转移矩阵Ft、系统噪声方差矩阵Qt、离散化时间间隔Ts和泰勒级数展开的阶数n。函数的输出包括离散时间的转移矩阵Phikk_1和噪声方差矩阵Qk。
代码的主要逻辑如下
首先初始化离散时间的转移矩阵Phikk_1为单位矩阵加上第一阶近似项Ts*Ft。初始化噪声方差矩阵Qk为第一阶近似项Qt*Ts。如果输入的n小于等于1默认值则直接返回。进入循环从第二阶开始迭代计算更高阶的近似项。在每一次迭代中更新时间间隔Tsi为当前时间间隔的幂次即Tsi Tsi*Ts。更新阶数因子facti为当前阶数因子的幂次即facti facti*i。更新连续时间系统的转移矩阵Fti为当前转移矩阵的幂次即Fti Fti*Ft。将当前时间间隔和阶数因子的乘积与当前转移矩阵相加得到离散时间的转移矩阵即Phikk_1 Phikk_1 Tsi/facti*Fti。更新噪声方差矩阵的计算通过当前时间间隔和当前噪声方差矩阵的乘积以及转置运算得到新的噪声方差矩阵即Mi FtMi FtMi。将当前时间间隔和阶数因子的乘积与新的噪声方差矩阵相加得到离散时间的噪声方差矩阵即Qk Qk Tsi/facti*Mi。循环结束后返回离散时间的转移矩阵Phikk_1和噪声方差矩阵Qk。
该代码主要用于根据连续时间模型的差分方程通过泰勒级数展开的方式计算得到离散时间的转移矩阵和噪声方差矩阵以便在离散时间环境下应用Kalman滤波器进行状态估计和预测。
三、ekf扩展卡尔曼滤波
EKFExtended Kalman Filter简称扩展卡尔曼滤波器是一种高效率的递归滤波器自回归滤波器它能够从一系列的不完全及包含噪声的测量中估计动态系统的状态1。
EKF滤波的主要思想是利用状态转移矩阵和观测矩阵结合当前状态和上一时刻的状态来估计当前的最优状态1。具体来说EKF滤波的步骤包括1
初始化滤波器的状态向量和协方差矩阵。对于每个时间步长执行以下步骤
a. 利用状态转移矩阵和当前状态向量计算出下一时刻的状态向量和协方差矩阵。
b. 结合观测矩阵和当前观测向量计算出当前时刻的状态估计值和协方差矩阵。
c. 根据新的状态估计值和协方差矩阵更新滤波器的状态向量和协方差矩阵。 3. 重复执行步骤2进行迭代优化直到满足收敛条件为止。
与传统的滤波器相比EKF滤波具有更好的数值稳定性和计算效率能够更准确地估计状态变量的值并且在高维度和非线性较强的系统中表现更好。
function kf ekf(kf, yk, TimeMeasBoth)if nargin1;TimeMeasBoth T;elseif nargin2TimeMeasBoth B;endif TimeMeasBothT || TimeMeasBothBif isfield(kf, fx) % nonliear state Jacobian matrix[kf.Phikk_1, kf.xkk_1] ekfJcb(kf.fx, kf.xk, kf.px);if isempty(kf.xkk_1), kf.xkk_1 kf.Phikk_1*kf.xk; endelsekf.xkk_1 kf.Phikk_1*kf.xk;endkf.Pxkk_1 kf.Phikk_1*kf.Pxk*kf.Phikk_1 kf.Gammak*kf.Qk*kf.Gammak;if TimeMeasBothT % time updating onlykf.xk kf.xkk_1; kf.Pxk kf.Pxkk_1;return;endendif TimeMeasBothM || TimeMeasBothBif TimeMeasBothM % meas updating onlykf.xkk_1 kf.xk; kf.Pxkk_1 kf.Pxk;endif isfield(kf, hx) % nonliear measurement Jacobian matrix[kf.Hk, kf.ykk_1] ekfJcb(kf.hx, kf.xkk_1, kf.py);if isempty(kf.ykk_1), kf.ykk_1 kf.Hk*kf.xkk_1; endelsekf.ykk_1 kf.Hk*kf.xkk_1;endkf.Pxykk_1 kf.Pxkk_1*kf.Hk; kf.Pykk_1 kf.Hk*kf.Pxykk_1 kf.Rk;% filteringkf.Kk kf.Pxykk_1*kf.Pykk_1^-1;kf.xk kf.xkk_1 kf.Kk*(yk-kf.ykk_1);kf.Pxk kf.Pxkk_1 - kf.Kk*kf.Pykk_1*kf.Kk; kf.Pxk (kf.Pxkkf.Pxk)/2;end这段代码是一个扩展卡尔曼滤波器Extended Kalman Filter, EKF的实现。下面是对代码功能的解释 函数 ekf 接受三个输入参数kf扩展卡尔曼滤波器的状态yk测量值TimeMeasBoth一个字符表示时间测量的类型。 根据输入参数的数量确定 TimeMeasBoth 的值。如果输入参数数量为1则将 TimeMeasBoth 设置为字符 ‘T’如果输入参数数量为2则将 TimeMeasBoth 设置为字符 ‘B’。 根据 TimeMeasBoth 的值执行以下操作 如果 TimeMeasBoth 是字符 ‘T’ 或者字符 ‘B’ 如果 kf 中存在字段 ‘fx’非线性状态雅可比矩阵则调用函数 ekfJcb 计算 kf 的 ‘Phikk_1’ 和 ‘xkk_1’分别是状态转移矩阵和下一时刻的状态估计值。 function [Jcb, y] ekfJcb(hfx, x, tpara)[Jcb, y] feval(hfx, x, tpara);如果 xkk_1 是空的则将其设置为 Phikk_1 乘以当前状态 xk。 如果 fx 不存在则将 xkk_1 设置为 Phikk_1’乘以当前状态 xk。 计算 Pxkk_1下一时刻的协方差矩阵估计值使用 Phikk_1、Pxk、Gammak、Qk 和 Gammak 的值进行计算。 如果 TimeMeasBoth 是字符 T则只进行时间更新将下一时刻的状态估计值 xkk_1 和协方差矩阵 Pxkk_1 赋值给当前状态 xk 和协方差矩阵 Pxk然后返回。 如果 TimeMeasBoth 是字符 B则继续执行下面的操作 根据下一时刻的状态估计值 xkk_1 和协方差矩阵 Pxkk_1以及测量值 yk 和噪声协方差矩阵 Rk计算卡尔曼增益Kalman gain和更新后的状态估计值 xk 和协方差矩阵 Pxk。将更新后的状态估计值 xk 和协方差矩阵 Pxk 赋值给当前状态 xk 和协方差矩阵 Pxk。 函数执行完毕输出更新后的扩展卡尔曼滤波器状态 kf。
总体而言这段代码实现了扩展卡尔曼滤波器的时间更新和测量更新过程根据输入的状态、测量值和时间测量类型更新滤波器的状态和协方差矩阵。
四、UKF无迹卡尔曼滤波
UKFUnscented Kalman Filter中文释义是无迹卡尔曼滤波、无迹卡尔曼滤波或者去芳香卡尔曼滤波是一种用于处理非线性非高斯系统的状态估计的滤波算法1。
UKF滤波的主要思想是利用Unscented变换来将非线性系统方程转化为线性假设下的标准Kalman滤波体系。具体来说UKF滤波的步骤包括
选择一组Unscented变换的参数计算Unscented变换中的权重和均值。对系统模型和观测模型进行一阶Taylor展开得到非线性的残差项。利用这些残差项来更新状态估计值。重复执行步骤2和3进行迭代优化直到满足收敛条件为止。
与传统的卡尔曼滤波相比UKF滤波具有更好的数值稳定性和计算效率能够更准确地估计状态变量的值并且在高维度和非线性较强的系统中表现更好。
需要注意的是在实际应用中UKF滤波算法的实现和参数设置需要根据具体的系统模型和观测模型进行调整并进行实验验证和优化。同时对于大规模或实时性要求高的应用场景还需要考虑算法的效率和可扩展性。
function kf ukf(kf, yk, TimeMeasBoth)
% Unscented Kalman filter for nonlinear system.
%
% Prototype: kf ukf(kf, yk, TimeMeasBoth)
% Inputs: kf - filter structure array
% yk - measurement vector
% TimeMeasBoth - described as follows,
% TimeMeasBothT (or nargin1) for time updating only,
% TimeMeasBothM for measurement updating only,
% TimeMeasBothB (or nargin2) for both time and
% measurement updating.
% Output: kf - filter structure array after time/meas updating
%
% See also ukfUT, ckf, ssukf, ekf, kfupdate.% Copyright(c) 2009-2014, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 27/09/2012if nargin1;TimeMeasBoth T;elseif nargin2TimeMeasBoth B;endif ~isfield(kf, alpha)kf.alpha 1e-3; kf.beta 2; kf.kappa 0;endif TimeMeasBothT || TimeMeasBothBif isfield(kf, fx) % nonliear state propagation[kf.xkk_1, kf.Pxkk_1] ukfUT(kf.xk, kf.Pxk, kf.fx, kf.px, kf.alpha, kf.beta, kf.kappa);kf.Pxkk_1 kf.Pxkk_1 kf.Gammak*kf.Qk*kf.Gammak;elsekf.xkk_1 kf.Phikk_1*kf.xk;kf.Pxkk_1 kf.Phikk_1*kf.Pxk*kf.Phikk_1 kf.Gammak*kf.Qk*kf.Gammak;endif TimeMeasBothT % time updating onlykf.xk kf.xkk_1; kf.Pxk kf.Pxkk_1;return;endendif TimeMeasBothM || TimeMeasBothBif TimeMeasBothM % meas updating onlykf.xkk_1 kf.xk; kf.Pxkk_1 kf.Pxk;endif isfield(kf, hx) % nonliear measurement propagation[kf.ykk_1, kf.Pykk_1, kf.Pxykk_1] ukfUT(kf.xkk_1, kf.Pxkk_1, kf.hx, kf.py, kf.alpha, kf.beta, kf.kappa);kf.Pykk_1 kf.Pykk_1 kf.Rk;elsekf.ykk_1 kf.Hk*kf.xkk_1;kf.Pxykk_1 kf.Pxkk_1*kf.Hk; kf.Pykk_1 kf.Hk*kf.Pxykk_1 kf.Rk;end% filteringkf.Kk kf.Pxykk_1*kf.Pykk_1^-1;kf.xk kf.xkk_1 kf.Kk*(yk-kf.ykk_1);kf.Pxk kf.Pxkk_1 - kf.Kk*kf.Pykk_1*kf.Kk; kf.Pxk (kf.Pxkkf.Pxk)/2;end这段代码是一个 UKF 的实现。下面是对代码功能的解释
输入参数 kf一个结构体数组包含卡尔曼滤波器的状态和参数信息例如状态向量 xk、状态协方差矩阵 Pxk、时间更新系数 fx、测量更新系数 hx 等。yk测量向量即系统的观测值。TimeMeasBoth一个字符串用于指定时间更新和测量更新的方式可选值为 T只进行时间更新、M只进行测量更新和 B同时进行时间和测量更新。 根据输入参数确定更新方式 如果输入参数 TimeMeasBoth 为 T 或者没有指定则执行时间更新。如果输入参数 TimeMeasBoth 为 M 或者没有指定则执行测量更新。 时间更新 如果结构体数组 kf 中没有 alpha、beta 和 kappa 字段则给它们赋默认值。如果结构体数组 kf 中有 fx 字段表示存在非线性状态转移函数此时需要根据输入的观测值和滤波器参数执行时间更新。 首先通过调用函数 ukfUT也是该代码文件中的函数计算状态转移后的状态向量和状态协方差矩阵。然后根据滤波器参数计算状态协方差矩阵的修正项并将其加到状态协方差矩阵中。 如果结构体数组 kf 中没有 fx 字段表示状态转移是线性的直接使用预测的状态向量和协方差矩阵。 测量更新 如果执行的是测量更新首先判断是否只有测量更新。如果只有测量更新则将预测的状态向量和协方差矩阵分别赋值给当前的状态向量和协方差矩阵。如果同时进行时间和测量更新则根据滤波器参数执行测量更新。 如果结构体数组 kf 中有 hx 字段表示存在非线性测量转移函数此时需要根据输入的观测值和滤波器参数执行测量更新。 首先通过调用函数 ukfUT 计算测量转移后的测量向量、测量协方差矩阵和交叉协方差矩阵。然后根据滤波器参数计算测量协方差矩阵的修正项并将其加到测量协方差矩阵中。 如果结构体数组 kf 中没有 hx 字段表示测量转移是线性的直接使用预测的测量向量和协方差矩阵。 返回结果 更新后的卡尔曼滤波器结构体数组 kf 将作为输出返回。
总体来说这段代码实现了非线性系统的卡尔曼滤波器的时间更新和测量更新过程。它根据输入的观测值和滤波器参数对滤波器的状态向量和协方差矩阵进行更新以实现滤波器的状态估计功能。
ukfUT
function [y, Pyy, Pxy, X, Y] ukfUT(x, Pxx, hfx, tpara, alpha, beta, kappa)
% Unscented transformation.
%
% Prototype: [y, Pyy, Pxy, X, Y] ukfUT(x, Pxx, hfx, tpara, alpha, beta, kappa)
% Inputs: x, Pxx - state vector and its variance matrix
% hfx - a handle for nonlinear state equation
% tpara - some time-variant parameter pass to hfx
% alpha, beta, kappa - parameters for UT transformation
% Outputs: y, Pyy - state vector and its variance matrix after UT
% Pxy - covariance matrix between x y
% X, Y - Sigma-point vectors before after UT
%
% See also ukf, ckfCT, SSUT.% Copyright(c) 2009-2014, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 27/09/2012n length(x);lambda alpha^2*(nkappa) - n;gamma sqrt(nlambda);Wm [lambda/gamma^2; repmat(1/(2*gamma^2),2*n,1)]; Wc [Wm(1)(1-alpha^2beta); Wm(2:end)];sPxx gamma*chol(Pxx); % Choleskey decompositionxn repmat(x,1,n); X [x, xnsPxx, xn-sPxx];Y(:,1) feval(hfx, X(:,1), tpara); mlength(Y); y Wm(1)*Y(:,1);Y repmat(Y,1,2*n1);for k2:1:2*n1 % Sigma points nolinear propagationY(:,k) feval(hfx, X(:,k), tpara);y y Wm(k)*Y(:,k);endPyy zeros(m); Pxy zeros(n,m);for k1:1:2*n1yerr Y(:,k)-y;Pyy Pyy Wc(k)*(yerr*yerr); % variancexerr X(:,k)-x;Pxy Pxy Wc(k)*xerr*yerr; % covarianceend这段代码实现了Unscented变换Unscented TransformationUT的功能。UT是一种用于非线性系统状态估计的方法它通过对非线性方程进行一系列采样和传播得到状态变量的估计结果。
该函数的输入参数包括
x状态向量state vector。Pxx状态向量的方差矩阵variance matrix。hfx一个处理非线性状态方程的句柄handle。tpara一些时间变化的参数用于传递给hfx。alpha、beta、kappaUT变换的参数。
该函数的输出参数包括
y经过UT变换后的状态向量。Pyy经过UT变换后的状态向量的方差矩阵。Pxy状态向量和经过UT变换后的状态向量之间的协方差矩阵。X、Y在UT变换之前和之后的Sigma点向量。
代码的主要步骤如下
根据输入的参数计算一些中间变量包括n状态向量的长度、lambda、gamma、Wm和Wc。进行Cholesky分解将输入的状态方差矩阵Pxx转换为下三角矩阵sPxx。构造Sigma点向量X包括三个部分的点分别是状态向量本身、状态向量加上和减去状态方差矩阵的Cholesky分解后的矩阵。对每个Sigma点进行非线性传播通过调用句柄函数hfx计算每个点的输出值并将结果存储在矩阵Y中。根据UT变换的公式计算估计的状态向量y和方差矩阵Pyy以及状态向量和估计状态向量之间的协方差矩阵Pxy。返回估计的状态向量、方差矩阵、协方差矩阵以及在UT变换之前和之后的Sigma点向量。
需要注意的是该代码中使用了MATLAB的函数feval来调用句柄函数hfx并在循环中进行了多次调用。因此在使用该代码时需要确保句柄函数hfx的正确实现和可调用性。
utpoint
function [U, wm, wc] utpoint(n, afa, beta, kappa)
% Calculate 3th or 5th-order cubature points.
%
% Prototype: [U, wm, wc] utpoint(n, afa, beta, kappa)
% Inputs: n - dimension
% afa,beta,kappa - parameters
% Outputs: U - UT points
% wm,wc - weights
%
% See also cubpoint, ghpoint, ukf.% Copyright(c) 2009-2022, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 01/08/2022if nargin4, kappa0; endif nargin3, beta2; endif nargin2, afa1e-3; endlambda afa^2*(nkappa)-n; gamma sqrt(nlambda);U gamma*[eye(n), -eye(n), zeros(n,1)];wm [repmat(1/(2*gamma^2),1,2*n), lambda/gamma^2];wc wm; wc(end) wc(end)(1-afa^2beta);这段代码是一个MATLAB函数用于计算3阶或5阶的 cubature 点的权重。下面是代码的功能解释
函数的输入参数为 n维度整数afa、beta、kappa参数可以是标量、向量或矩阵 函数的输出参数为 Ucubature 点的向量大小为 (3*n, 1) 的列向量wm与 U 中的每个点对应的权重大小为 (3*n, 1) 的列向量wc与 U 中的每个点对应的权重大小为 (3*n, 1) 的列向量 代码逻辑解释 首先检查输入参数的数量如果小于4个将 kappa 设置为0如果小于3个将 beta 设置为2如果小于2个将 afa 设置为1e-3。接下来根据参数计算变量 lambda 和 gamma 的值。然后根据 gamma 和其他参数计算 cubature 点的矩阵 U。最后根据参数和计算结果计算权重向量 wm 和 wc。其中wm 的前两个元素是相同的大小为 (2*n, 1) 的矩阵每个元素都是 1/(2*gamma^2)第三个元素是 lambda/gamma^2。wc 的前两个元素与 wm 相同最后一个元素是 (3*n, 1) 的列向量值为 (1-afa^2beta)。
这个函数的作用是通过输入的参数计算 cubature 点的位置和权重。 cubature 点是一种用于数值积分的方法通过在低维度空间中计算一些点并将它们的权重相加以估计高维度空间的积分值。
五、ssukf
SSUKF是一种非线性滤波算法全称为Unscented Particle Filtering with Square-Root Unscented Transform。它结合了Unscented变换和粒子滤波的优点用于处理非线性非高斯系统的状态估计问题。SSUKF的主要步骤包括
选择一组Unscented变换的参数计算Unscented变换中的权重和均值。对系统模型和观测模型进行一阶Taylor展开得到非线性的残差项。使用粒子滤波的方法通过对残差项进行加权和估计得到新的状态估计值。重复执行步骤2和3进行迭代优化直到满足收敛条件为止。同时对于大规模或实时性要求高的应用场景还需要考虑算法的效率和可扩展性。 UKF 和 SSUKF 区别 Unscented变换的方式不同UKF使用标准的Unscented变换而SSUKF使用Square-Root Unscented变换这种变换可以更快地计算权重和均值提高了算法的计算效率。粒子滤波的方式不同在UKF中使用标准粒子滤波来估计状态变量而在SSUKF中使用一种称为序贯 Importance Sampling的粒子滤波方法来估计状态变量。这种滤波方法可以更好地处理非线性较强的系统并且可以提高算法的数值稳定性。适用场景不同UKF适用于处理高维度的非线性系统而SSUKF适用于处理低维度的非线性系统尤其是对于一些实时性要求较高的应用场景。 需要注意的是无论是UKF还是SSUKF它们都是一种相对较新的滤波算法在实际应用中还需要根据具体的系统模型和观测模型进行调整和优化同时考虑算法的效率和可扩展性1。 function kf ssukf(kf, yk, TimeMeasBoth)
% Unscented Kalman filter for nonlinear system.
%
% Prototype: kf ssukf(kf, yk, TimeMeasBoth)
% Inputs: kf - filter structure array
% yk - measurement vector
% TimeMeasBoth - described as follows,
% TimeMeasBothT (or nargin1) for time updating only,
% TimeMeasBothM for measurement updating only,
% TimeMeasBothB (or nargin2) for both time and
% measurement updating.
% Output: kf - filter structure array after time/meas updating
%
% See also ukf, SSUT, ckf, ekf, kfupdate.% Copyright(c) 2009-2022, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 08/06/2022if nargin1;TimeMeasBoth T;elseif nargin2TimeMeasBoth B;endif TimeMeasBothT || TimeMeasBothBif isfield(kf, fx) % nonliear state propagation[kf.xkk_1, kf.Pxkk_1] SSUT(kf.xk, kf.Pxk, kf.fx, kf.px, 0);kf.Pxkk_1 kf.Pxkk_1 kf.Gammak*kf.Qk*kf.Gammak;elsekf.xkk_1 kf.Phikk_1*kf.xk;kf.Pxkk_1 kf.Phikk_1*kf.Pxk*kf.Phikk_1 kf.Gammak*kf.Qk*kf.Gammak;endif TimeMeasBothT % time updating onlykf.xk kf.xkk_1; kf.Pxk kf.Pxkk_1;return;endendif TimeMeasBothM || TimeMeasBothBif TimeMeasBothM % meas updating onlykf.xkk_1 kf.xk; kf.Pxkk_1 kf.Pxk;endif isfield(kf, hx) % nonliear measurement propagation[kf.ykk_1, kf.Pykk_1, kf.Pxykk_1] SSUT(kf.xkk_1, kf.Pxkk_1, kf.hx, kf.py, 0);kf.Pykk_1 kf.Pykk_1 kf.Rk;elsekf.ykk_1 kf.Hk*kf.xkk_1;kf.Pxykk_1 kf.Pxkk_1*kf.Hk; kf.Pykk_1 kf.Hk*kf.Pxykk_1 kf.Rk;end% filteringkf.Kk kf.Pxykk_1*kf.Pykk_1^-1;kf.xk kf.xkk_1 kf.Kk*(yk-kf.ykk_1);kf.Pxk kf.Pxkk_1 - kf.Kk*kf.Pykk_1*kf.Kk; kf.Pxk (kf.Pxkkf.Pxk)/2;end这段代码是一个用于非线性系统的扩展卡尔曼滤波Unscented Kalman Filter的函数。它接受一个初始的卡尔曼滤波器结构数组kf一个测量向量yk以及一个表示时间更新和测量更新的标志TimeMeasBoth。
代码的主要逻辑如下
根据输入参数的数量确定TimeMeasBoth的值如果只有一个输入参数则TimeMeasBoth被设置为’T’表示只进行时间更新如果有两个输入参数则TimeMeasBoth被设置为’B’表示同时进行时间更新和测量更新。如果TimeMeasBoth为’T’或’B’则执行时间更新步骤。如果卡尔曼滤波器结构数组kf中存在非线性状态转移函数fx则使用SSUT函数进行状态预测并更新状态和状态协方差矩阵。如果只有时间更新则直接返回更新后的kf。如果TimeMeasBoth为’M’或’B’则执行测量更新步骤。如果只有测量更新则将状态和状态协方差矩阵设置为先前的值。如果卡尔曼滤波器结构数组kf中存在非线性测量转移函数hx则使用SSUT函数进行测量预测并更新测量和测量协方差矩阵。执行滤波步骤。计算卡尔曼增益Kalman gain并使用测量和预测的状态和协方差矩阵更新状态估计值和状态协方差矩阵。
最终函数返回更新后的卡尔曼滤波器结构数组kf。
需要注意的是该代码依赖于其他函数SSUT这些函数可能需要在其他地方定义或提供。此外该代码还依赖于一些变量的定义例如Phikk_1、Gammak、Qk、Hk等这些变量应该在输入的卡尔曼滤波器结构数组kf中提供。
如果您要使用该函数请确保提供正确的输入参数和正确的函数依赖关系。
SSUT
function [y, Pyy, Pxy, X, Y] SSUT(x, Pxx, hfx, tpara, w0)
% Spherical Simplex Unscented Transformation.
%
% Prototype: [y, Pyy, Pxy, X, Y] SSUT(x, Pxx, hfx, tpara, w0)
% Inputs: x, Pxx - state vector and its variance matrix
% hfx - a handle for nonlinear state equation
% tpara - some time-variant parameter pass to hfx
% w0 - weight0
% Outputs: y, Pyy - state vector and its variance matrix after UT
% Pxy - covariance matrix between x y
% X, Y - Sigma-point vectors before after UT
%
% See also ssukf, ukfUT, ckfCT.% Copyright(c) 2009-2022, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 08/06/2022
global SSUT_s SSUT_wif isempty(SSUT_w), SSUT_w(1)0; endif nargin5, w00; endn length(x);if n~size(SSUT_s,1) || SSUT_w(1)~w0w1 (1-w0)/(n1);s [0, -1/sqrt(2), 1/sqrt(2)];for j2:ns1 [s(:,1); 0];for i1:js1(:,i1) [s(:,i1); -1/sqrt(j*(j1))];ends1(:,j2) [zeros(j-1,1);j/sqrt(j*(j1))];s s1;endSSUT_s s/sqrt(w1); SSUT_w [w0, repmat(w1,1,n1)];end%%X repmat(x,1,n2) chol(Pxx)*SSUT_s;y SSUT_w(1)*feval(hfx, X(:,1), tpara); m length(y);Y repmat(y,1,2*n);for k2:1:n2Y(:,k) feval(hfx, X(:,k), tpara);y y SSUT_w(k)*Y(:,k);endPyy zeros(m); Pxy zeros(n,m);for k1:1:n2yerr Y(:,k)-y;Pyy Pyy SSUT_w(k)*(yerr*yerr); % variancexerr X(:,k)-x;Pxy Pxy SSUT_w(k)*xerr*yerr; % covarianceend这是一个实现了球面简单卡尔曼滤波Spherical Simplex Unscented Transformation, SSUT的 MATLAB 函数。SSUT 是一种非线性滤波方法用于对非线性系统进行状态估计和预测。
函数输入参数包括
x状态向量一个列向量。Pxx状态向量的方差矩阵。hfx一个用于描述非线性状态方程的函数句柄。tpara一个时间变参数向量用于传递给 hfx 函数。w0权重参数。
函数输出参数包括
y经过 UT 变换后的状态向量一个列向量。Pyy经过 UT 变换后的状态向量的方差矩阵。Pxy状态向量和经过 UT 变换后的状态向量之间的协方差矩阵。X 和 Y在 UT 变换前后生成的 Sigma 点向量。
函数首先检查是否已经初始化了一些全局变量 SSUT_s 和 SSUT_w如果没有则根据默认值进行初始化。然后根据输入的参数 x 和 Pxx 生成相应的 Sigma 点。接着调用非线性状态方程 hfx 来计算这些 Sigma 点对应的输出值并将这些输出值存储在 Y 中。然后根据球面简单卡尔曼滤波的算法计算经过 UT 变换后的状态向量 y 和其方差矩阵 Pyy以及状态向量和经过 UT 变换后的状态向量之间的协方差矩阵 Pxy。
最后函数返回经过 UT 变换后的状态向量 y 和其方差矩阵 Pyy以及状态向量和经过 UT 变换后的状态向量之间的协方差矩阵 Pxy以及在 UT 变换前后生成的 Sigma 点向量 X 和 Y。
该函数是 SSUT 的实现可以用于非线性系统的状态估计和预测。
六、CKF容积卡尔曼滤波
容积卡尔曼滤波Cube-based Radial Basis Function简称CKF是一种用于处理非线性非高斯系统的状态估计的滤波算法。
CKF滤波的核心思想是利用容积准则来近似状态的后验均值和协方差以保证在理论上以三阶多项式逼近任何非线性高斯状态的后验均值和方差。
与传统的卡尔曼滤波相比CKF滤波具有更好的数值稳定性和计算效率能够更准确地估计状态变量的值并且在高维度和非线性较强的系统中表现更好。
function kf ckf(kf, yk, TimeMeasBoth)
% Cubature transformation KF.
%
% Prototype: kf ckf(kf, yk, TimeMeasBoth)
% Inputs: kf - filter structure array
% yk - measurement vector
% TimeMeasBoth - described as follows,
% TimeMeasBothT (or nargin1) for time updating only,
% TimeMeasBothM for measurement updating only,
% TimeMeasBothB (or nargin2) for both time and
% measurement updating.
% Output: kf - filter structure array after time/meas updating
%
% See also ckfCT, ukf, ekf, kfupdate.% Copyright(c) 2009-2022, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 01/03/2022if nargin1;TimeMeasBoth T;elseif nargin2TimeMeasBoth B;endif TimeMeasBothT || TimeMeasBothBif isfield(kf, fx) % nonliear state propagation[kf.xkk_1, kf.Pxkk_1] ckfCT(kf.xk, kf.Pxk, kf.fx, kf.px);kf.Pxkk_1 kf.Pxkk_1 kf.Gammak*kf.Qk*kf.Gammak;elsekf.xkk_1 kf.Phikk_1*kf.xk;kf.Pxkk_1 kf.Phikk_1*kf.Pxk*kf.Phikk_1 kf.Gammak*kf.Qk*kf.Gammak;endif TimeMeasBothT % time updating onlykf.xk kf.xkk_1; kf.Pxk kf.Pxkk_1;return;endendif TimeMeasBothM || TimeMeasBothBif TimeMeasBothM % meas updating onlykf.xkk_1 kf.xk; kf.Pxkk_1 kf.Pxk;endif isfield(kf, hx) % nonliear measurement propagation[kf.ykk_1, kf.Pykk_1, kf.Pxykk_1] ckfCT(kf.xkk_1, kf.Pxkk_1, kf.hx, kf.py);kf.Pykk_1 kf.Pykk_1 kf.Rk;elsekf.ykk_1 kf.Hk*kf.xkk_1;kf.Pxykk_1 kf.Pxkk_1*kf.Hk; kf.Pykk_1 kf.Hk*kf.Pxykk_1 kf.Rk;end% filteringkf.Kk kf.Pxykk_1*kf.Pykk_1^-1;kf.xk kf.xkk_1 kf.Kk*(yk-kf.ykk_1);kf.Pxk kf.Pxkk_1 - kf.Kk*kf.Pykk_1*kf.Kk; kf.Pxk (kf.Pxkkf.Pxk)/2;end这段代码是一个实现Cubature Kalman FilterCKF的函数。CKF是一种非线性滤波方法用于估计动态系统的状态。
函数ckf的作用是根据输入的滤波器结构数组kf、测量向量yk和时间/测量更新标志TimeMeasBoth对滤波器进行时间更新或测量更新。
代码的主要逻辑如下
根据输入的参数确定时间/测量更新标志TimeMeasBoth。如果只有一个输入参数则默认为时间更新标志为’T’如果有两个输入参数则根据第二个参数确定更新标志。如果时间更新标志为’T’或’B’表示需要进行时间更新或同时进行时间更新和测量更新执行以下操作 如果滤波器结构数组kf中存在非线性状态传播函数fx则使用ckfCT函数进行状态预测和协方差预测。如果时间更新标志为’T’则更新滤波器的状态和协方差并返回结果。 如果测量更新标志为’M’或’B’表示需要进行测量更新或同时进行时间更新和测量更新执行以下操作 如果测量更新标志为’M’则将滤波器的状态和协方差设置为当前状态和协方差。如果滤波器结构数组kf中存在非线性测量传播函数hx则使用ckfCT函数进行测量预测和协方差预测。进行滤波计算卡尔曼增益、状态估计和协方差估计。
最后函数返回更新后的滤波器结构数组kf。
需要注意的是这段代码中的一些函数如ckfCT、ukf、ekf、kfupdate并没有在代码中定义可能是在其他地方定义的辅助函数或子函数。这段代码只展示了实现CKF的主要逻辑而没有提供完整的实现细节。
ckfCT
function [y, Pyy, Pxy, X, Y] ckfCT(x, Pxx, hfx, tpara)
% Spherical-Radial Cubature transformation.
%
% Prototype: [y, Pyy, Pxy, X, Y] ckfCT(x, Pxx, hfx, tpara)
% Inputs: x, Pxx - state vector and its variance matrix
% hfx - a handle for nonlinear state equation
% tpara - some time-variant parameter pass to hfx
% Outputs: y, Pyy - state vector and its variance matrix after UT
% Pxy - covariance matrix between x y
% X, Y - Sigma-point vectors before after UT
%
% See also ckf, ukfUT, SSUT.% Copyright(c) 2009-2022, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 01/03/2022n length(x);sPxx sqrt(n)*chol(Pxx); % Choleskey decompositionxn repmat(x,1,n); X [xnsPxx, xn-sPxx];y feval(hfx, X(:,1), tpara);Y repmat(y,1,2*n);Pyy Y(:,1)*Y(:,1); Pxy X(:,1)*Y(:,1);for k2:1:2*nY(:,k) feval(hfx, X(:,k), tpara);y y Y(:,k);Pyy Pyy Y(:,k)*Y(:,k);Pxy Pxy X(:,k)*Y(:,k);endy 1/(2*n)*y; % y meanPyy 1/(2*n)*Pyy - y*y; Pxy 1/(2*n)*Pxy - x*y;% Y(:,1) feval(hfx, X(:,1), tpara); mlength(Y); y Y(:,1);
% Y repmat(Y,1,2*n);
% Pyy zeros(m); Pxy zeros(n,m);
% for k2:1:2*n % Sigma points nolinear propagation
% Y(:,k) feval(hfx, X(:,k), tpara);
% y y Y(:,k);
% end
% y 1/(2*n)*y;
% for k1:1:2*n
% yerr Y(:,k)-y;
% Pyy Pyy (yerr*yerr); % variance
% xerr X(:,k)-x;
% Pxy Pxy xerr*yerr; % covariance
% end
% Pyy 1/(2*n)*Pyy; Pxy 1/(2*n)*Pxy;This code implements the Spherical-Radial Cubature transformation (CKF-CT) for nonlinear state estimation.下面是代码的功能解释
代码输入 x当前状态的向量。Pxx当前状态向量的方差矩阵。hfx非线性状态方程的句柄。tpara传递给hfx的一些时间变化参数。 代码输出 y经过Unscented变换后的状态向量。Pyy经过Unscented变换后的状态向量的方差矩阵。Pxy经过Unscented变换后的状态向量和初态向量之间的协方差矩阵。X在进行Unscented变换之前的Sigma点向量。Y在进行Unscented变换之后的Sigma点向量。 代码实现 首先通过Cholesky分解将输入的方差矩阵Pxx转换为上三角矩阵sPxx。然后根据当前的状态向量x和上三角矩阵sPxx生成两个Sigma点即X矩阵。接下来通过调用非线性状态方程的句柄hfx计算初始的Sigma点对应的输出向量Y。然后使用循环计算每个Sigma点的输出并将它们累加到总输出向量y和协方差矩阵Pyy中。同时计算初始状态向量和总输出向量之间的协方差矩阵Pxy。最后根据总输出向量和协方差矩阵计算最终的状态向量和协方差矩阵。 注意事项 该代码使用了MATLAB中的函数feval用于调用非线性状态方程的句柄hfx进行计算。确保在使用该代码之前已经定义并正确设置了非线性状态方程的句柄hfx和相关参数tpara。代码中的注释提供了一些说明和参考可以帮助理解代码的功能和实现方法。
总体而言该代码实现了Spherical-Radial Cubature变换CKF-CT用于非线性状态估计通过Unscented变换UT对状态进行估计并计算估计结果的方差和协方差。
七、PF粒子滤波
粒子滤波Particle Filter是一种非参数化的贝叶斯滤波方法可以用于对随机过程进行估计和预测。它通过使用一组带有权值的粒子即样本来表示随机过程的状态从而实现对随机过程的估计和预测。
粒子滤波的主要思想是利用已知的先验概率分布和观测数据通过迭代的方式更新粒子的权值和状态从而得到后验概率分布。具体来说粒子滤波包括以下步骤
初始化根据先验概率分布生成一组初始粒子并赋予相应的权值。预测根据已知的动力学模型将每个粒子向前推进一个时间步长得到下一时刻的粒子集合。重采样根据每个粒子的权值对粒子进行重新采样使得权值较大的粒子在采样中出现的概率更大。输出根据重采样后的粒子集合可以得到后验概率分布的估计值。
粒子滤波在许多领域都有应用例如目标跟踪、姿态估计、导航、语音识别、图像处理等。然而粒子滤波也存在一些问题例如粒子退化、噪声敏感等需要进行进一步的研究和改进。
function [pf, Pxk] pfupdate(pf, Zk, TimeMeasBoth)
% Particle filter updating for additive normal distribution processmeasure noise.
%
% Prototype: [pf, Pxk] pfupdate(pf, Zk, TimeMeasBoth)
% Inputs: pf - particle filter structure array
% Zk - measurement vector
% TimeMeasBoth - described as follows,
% TimeMeasBothT (or nargin1) for time updating only,
% TimeMeasBothM for measurement updating only,
% TimeMeasBothB (or nargin2) for both time and
% measurement updating.
% Outputs: pf - particle filter structure array after time/meas updating
% Pxk - for full covariance (not diagnal)
%
% see also pfinit, kfupdate, ckf, ukf.% Copyright(c) 2009-2022, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 06/04/2022if nargin1;TimeMeasBoth T;elseif nargin2TimeMeasBoth B;endif TimeMeasBothT || TimeMeasBothBsQ chol(pf.Qkeps*eye(pf.n));endif TimeMeasBothM || TimeMeasBothBvRv zeros(pf.Npts, 1); invR pf.Rk^-1;endfor k1:pf.Nptsif TimeMeasBothT || TimeMeasBothBif isfield(pf,fx)pf.particles(:,k) feval(pf.fx, pf.particles(:,k), pf.tpara) sQ*randn(pf.n,1);elsepf.particles(:,k) pf.Phikk_1*pf.particles(:,k) sQ*randn(pf.n,1);endendif TimeMeasBothM || TimeMeasBothBif isfield(pf,hx)v Zk - feval(pf.hx, pf.particles(:,k), pf.tpara);elsev Zk - pf.Hk*pf.particles(:,k);endvRv(k) v*invR*v;endendif TimeMeasBothM || TimeMeasBothB
% [vRv, idx] sort(vRv); pf.particles pf.particles(:,idx);weight exp(-vRv/2);%/sqrt((2*pi)^pf.m*det(pf.Rk)); % Normal distributioncumw cumsum(weight0.01/pf.Npts); cumw cumw/cumw(end);
% idx interp1(cumw, (1:pf.Npts), linspace(cumw(1),cumw(end),pf.Npts10), nearest); % resampling indexidx interp1(cumw, (1:pf.Npts), cumw(1)(cumw(end)-cumw(1))*rand(pf.Npts10,1), nearest); % resampling indexpf.particles pf.particles(:,idx(3:pf.Npts2)); % myfig,hist(pf.particles(2,:));endpf.xk mean(pf.particles,2);xk pf.particles - repmat(pf.xk,1,pf.Npts);pf.Pxk diag(sum(xk.^2,2)/pf.Npts);
% for k1:pf.n
% pf.particles(k,:) pf.xk(k)sqrt(pf.Pxk(k,k))*randn(1,pf.Npts);
% endif nargout2 % if using full matrix Pxkpf.Pxk zeros(pf.n);for k1:pf.Nptspf.Pxk pf.Pxk xk(:,k)*xk(:,k);endpf.Pxk pf.Pxk/pf.Npts;Pxk pf.Pxk;endfunction pf pfinit(x0, P0, Qk, Rk, N)
% Particle filter structure initialization.
%
% Prototype: pf pfinit(x0, P0, Qk, Rk, N)
% Inputs: x0,P0 - initial state covariance;
% Qk,Rk - process measure covariance
% N - particle number
% Output: pf - particle filter structure array
%
% see also pfupdate, kfinit.% Copyright(c) 2009-2022, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 06/04/2022if size(P0,2)1, P0 diag(P0); endpf.n length(x0); pf.m length(Rk);for k1:pf.npf.particles(k,:) x0(k)sqrt(P0(k))*randn(1,N);endpf.Npts N;pf.Pxk P0; pf.Qk Qk; pf.Rk Rk;八、akfupdate自适应滤波
这段代码是一个自适应卡尔曼滤波器的实现。下面是代码的功能解释
函数 akfupdate 接受五个输入参数 kf一个包含卡尔曼滤波器结构信息的数组。yk一个测量向量。TimeMeasBoth一个描述时间更新和测量更新的标志字符可以取以下三个值 T只进行时间更新。M只进行测量更新。B同时进行时间更新和测量更新。 kftype自适应卡尔曼滤波器的类型。para一些参数。 判断输入参数的数量根据不同的标志字符执行相应的时间更新或测量更新。如果 TimeMeasBoth 是 T则执行时间更新 用 kf.Phikk_1 乘以 kf.xk更新状态估计值 kf.xk。用 kf.Phikk_1 乘以 kf.Pxk再乘以 kf.Phikk_1并加上 kf.Gammak * kf.Qk * kf.Gammak更新状态估计误差协方差矩阵 kf.Pxk。 如果 TimeMeasBoth 是 M则执行测量更新 保存当前的状态估计值和误差协方差矩阵到 kf.xkk_1 和 kf.Pxkk_1。 如果 TimeMeasBoth 是 B则执行时间更新和测量更新 保存当前的状态估计值和误差协方差矩阵到 kf.xkk_1 和 kf.Pxkk_1。计算 kf.Pxykk_1即当前估计值和测量值之间的协方差矩阵。计算 kf.Py0即测量噪声的协方差矩阵乘以 Hk 的转置。计算残差 kf.rk即实际测量值减去预测的测量值。 根据 kftype 的类型执行不同的自适应滤波算法 如果 kftype 是字符串 KF 或 MSHAKF则根据提供的参数计算一些变量如 b 和 s。如果 kftype 是数字 2则执行 MCKFMinimum Curvature Filter算法其中使用了一个参数 sigma。如果 kftype 是数字 3则执行 RSTKFResidual Square Total Least Squares Filter算法其中使用了一个参数 v。如果 kftype 是数字 4则执行 SSMKFSquare Root Split-Operator Matrix Kalman Filter算法。 将更新后的状态估计值、状态估计误差协方差矩阵和其他相关信息存储到输入的结构数组 kf 中。输出更新后的卡尔曼滤波器结构数组 kf。
该代码实现了自适应卡尔曼滤波器的不同算法并根据输入的标志字符选择相应的时间更新或测量更新操作。
shamn Sage-Husa自适应滤波
function [Rk, beta, maxflag] shamn(Rk, r2, Rmin, Rmax, beta, b, s)
% Sage-Husa adaptive measurement noise variance Rk.
%
% Prototype: [Rk, beta] shamn(Rk, r2, Rmin, Rmax, beta, b, s)
% Inputs: Rk - measurement noise variance
% r2 - rk^2-Py0;
% Rmin,Rmax - min max variance bound.
% beta,b - forgettiing factors
% s - if b0, s is enlarge factor
% Output: Rk, beta - same as above
% maxflag - if r2(k)Rmax(k) then set the max flag
%
% See also akfupdate.% Copyright(c) 2009-2022, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 26/03/2022m length(r2); maxflag zeros(m,1);beta beta./(betab);for k1:mif b(k)0,r2(k) r2(k) / s(k)^2;if r2(k)Rmin(k), Rk(k,k)Rmin(k); else, Rk(k,k)r2(k); endelseif r2(k)Rmin(k), r2(k)Rmin(k);elseif r2(k)Rmax(k), r2(k)Rmax(k); beta(k)1; maxflag(k)1; endRk(k,k) (1-beta(k))*Rk(k,k) beta(k)*r2(k);endend这段代码是一个Sage-Husa自适应滤波器的实现。下面是代码的功能解释
输入参数 Rk测量噪声方差一个列向量。r2Rk的平方减去初始残差平方的平均值也是一个列向量。Rmin和Rmax分别是测量噪声方差的下限和上限两个列向量。beta和b两个列向量分别表示遗忘因子和增益因子。s当b等于0时使用的放大因子一个列向量。 输出参数 Rk更新后的测量噪声方差一个列向量。beta更新后的遗忘因子一个列向量。maxflag当r2(k)大于Rmax(k)时的最大标志一个列向量。 代码逻辑 首先根据输入参数的长度初始化一个长度为m的maxflag向量全部元素设为0。然后将beta除以(betab)得到新的beta值。这是根据遗忘因子的公式进行的更新。对于每个k值进行以下操作 如果b(k)等于0表示使用增益因子s(k)对r2(k)进行放大。此时如果r2(k)小于Rmin(k)则将Rk(k,k)设为Rmin(k)否则将Rk(k,k)设为r2(k)。如果b(k)不等于0表示使用遗忘因子beta(k)对Rk(k,k)进行更新。此时如果r2(k)小于Rmin(k)则将r2(k)设为Rmin(k)如果r2(k)大于Rmax(k)则将r2(k)设为Rmax(k)并将beta(k)设为1同时将maxflag(k)设为1。然后根据遗忘因子的公式更新Rk(k,k)。 最后更新完成后的Rk、beta和maxflag作为输出结果。
总体来说这段代码实现了Sage-Husa自适应滤波器的核心功能根据输入的参数和当前状态自适应地调整测量噪声方差的值并监控是否超出上限。
九、抗差等价权函数
igg1
function w igg1(err, k0, k1)
% Institute of Geodesy Geophysics picewise method to calculate weight.
% Ref. IGG抗差估计在高程拟合中的应用研究_李广来,2021
%
% Prototype: gamma igg1(err, k0, k1)
% Inputs: err - normalized measurement error
% k0, k1 - picewise points
% Outputs: w - weight
%
% Example:
% figure, err-100:0.1:100; plot(err,igg1(err,3,10)); grid on
% figure, err-100:0.1:100; plot(err,igg1(err,3,inf)); grid on
%
% See also igg3.% Copyright(c) 2009-2022, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 20/06/2022if nargin1, k01.0; k13.0; end % w igg1(err)if nargin2, k1k0(2); k0k0(1); end % w igg1(err, k0k1)if k12*k0, k12*k0; enderr abs(err); w err;for k1:length(err)if err(k)k0, w(k)1;elseif err(k)k1, w(k)0;else, w(k)k0/err(k); endend这段代码是一个用于计算权重的函数采用了Institute of Geodesy Geophysics地球测量与地球物理研究所提出的一种分段函数方法。该函数接受两个输入参数误差的归一化测量值err和两个分段点k0和k1。输出为权重值w。
代码首先检查输入参数的数量。如果只有一个输入参数err则将k0和k1分别设置为1.0和3.0。如果输入参数有两个err和k0或k1则将k1设置为k0的第二个元素将k0设置为k0的第一个元素。
接下来代码将误差值abs(err)赋给变量w。然后通过一个循环遍历err中的每个元素。在循环中根据误差值的大小使用条件语句来计算相应的权重值。
如果误差值小于等于k0权重值设为1.0如果误差值大于k1权重值设为0.0。对于介于k0和k1之间的误差值权重值通过将k0除以误差值来计算。
最后函数返回计算得到的权重值w。
代码中还提供了一些示例绘图语句用于展示在不同参数设置下权重如何随着误差值的变化而变化。
igg3
function w igg3(err, k0, k1)
% Institute of Geodesy Geophysics picewise method to calculate weight.
% Ref. IGG抗差估计在高程拟合中的应用研究_李广来,2021
%
% Prototype: gamma igg3(err, k0, k1)
% Inputs: err - normalized measurement error
% k0, k1 - picewise points
% Outputs: w - weight
%
% Example:
% figure, err-10:0.1:10; plot(err,igg3(err,3,8)); grid on
%
% See also igg1.% Copyright(c) 2009-2022, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 20/06/2022if nargin1, k02.0; k16.0; end % w igg3(err)if nargin2, k1k0(2); k0k0(1); end % w igg3(err, k0k1)if k12*k0, k12*k0; enderr abs(err); w err;for k1:length(err)if err(k)k0, w(k)1;elseif err(k)k1, w(k)0;else, w(k)k0/err(k) * ((k1-err(k))/(k1-k0))^2; endend这段代码是一个MATLAB函数名为igg3。它用于根据给定的测量误差和两个参数k0和k1计算权重。
函数的输入参数为
err归一化测量误差的值。k0分段点之一。k1分段点之二。
函数的输出为
w计算得到的权重。
该函数使用了’Institute of Geodesy Geophysics’大地测量与地球物理学研究所的picewise方法来计算权重。该方法根据测量误差的大小在两个分段点之间使用不同的计算方式来得到权重值。
函数首先检查输入参数的数量。如果只有一个输入参数err则默认设置k0为2.0k1为6.0。根据不同的误差范围使用不同的公式计算权重值
如果误差小于等于k0则权重为1。如果误差大于k1则权重为0。否则使用以下公式计算权重值w(k) k0/err(k) * ((k1-err(k))/(k1-k0))^2
最后返回计算得到的权重向量作为函数的输出结果。
这个函数主要用于在高程拟合中进行抗差估计通过根据测量误差的大小调整权重来减小误差对拟合结果的影响。
十、数据融合
POSFusion
function psf POSFusion(rf, xpf, rr, xpr, ratio)
% POS data fusion for forward and backward results.
%
% Prototype: psf POSFusion(rf, xpf, rr, xpr, ratio)
% Inputs: rf - forward avp
% xpf - forward state estimation and covariance
% rr - backward avp
% xpr - backward state estimation and covariance
% ratio - the ratio of state estimation used to modify avp.
% Output: the fields in psf are
% rf, pf - avp coveriance after fusion
% r1, p1 - forward avp coveriance
% r2, p2 - backward avp coveriance
%
% See also insupdate, kfupdate, POSSmooth, POSProcessing, posplot.% Copyright(c) 2009-2014, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 22/01/2014if nargin5ratio 1;end[t, i1, i2] intersect(rf(:,end), rr(:,end));n size(xpf,2)-1; n2 n/2;r1 rf(i1,1:end-1); x1 xpf(i1,1:n2); p1 xpf(i1,n21:end-1);r2 rr(i2,1:end-1); x2 xpr(i2,1:n2); p2 xpr(i2,n21:end-1);x1(:,1:9) x1(:,1:9)*ratio; x2(:,1:9) x2(:,1:9)*ratio; % ratio% common fusionr10 [r1(:,1:9)-x1(:,1:9),r1(:,10:end)x1(:,10:end)];r20 [r2(:,1:9)-x2(:,1:9),r2(:,10:end)x2(:,10:end)];
% r ( r10.*p2 r20.*p1 )./(p1p2);
% p p1.*p2./(p1p2);[r, p] fusion(r10, p1, r20, p2);% attitude fusionfor k1:length(t)r10(k,1:3) q2att(qdelphi(a2qua(r1(k,1:3)),x1(k,1:3)));r20(k,1:3) q2att(qdelphi(a2qua(r2(k,1:3)),x2(k,1:3)));endr(:,1:3) attfusion(r10(:,1:3), p1(:,1:3), r20(:,1:3), p2(:,1:3));rf [r,t]; r1 [r10,t]; r2 [r20,t];pf [p,t]; p1 [p1,t]; p2 [p2,t];rf(isnan(rf)) 0;pf(isnan(pf)) 0;psf varpack(rf, pf, r1, p1, r2, p2);这段代码是一个用于进行POS位置和姿态数据融合的函数。它接受五个输入参数rf正向AVP伪距、xpf正向状态估计和协方差、rr反向AVP、xpr反向状态估计和协方差和ratio用于修改状态估计的比重。该函数的输出是一个包含以下字段的结构体psf
rf和pf经过融合后的AVP和协方差r1和p1正向AVP和协方差r2和p2反向AVP和协方差
代码的功能如下
检查输入参数的数量如果小于5个则将ratio设置为1。通过找到rf和rr在最后一个维度上的交集确定共同的时刻t、索引i1和i2。提取r1、x1、p1、r2、x2和p2分别对应于正向和反向的AVP、状态估计和协方差。根据ratio的值对x1和x2的前9个字段进行比例调整。执行融合操作生成融合后的AVP和协方差r和p。这里使用了名为fusion的函数具体的融合方法可能在该函数中实现。将rf和pf中的NaN值替换为0。将psf作为输出返回。
需要注意的是这段代码中的一些函数例如insupdate、kfupdate、POSSmooth、POSPlatform、posplot等并没有在代码中给出实现因此无法确定这些函数的具体功能。此外该代码的实现可能依赖于其他未包含在此代码片段中的函数或库。
POSSmooth
function vpOut POSSmooth(vp, gnss, nSec, isfig)
% POS data smooth via INS-VelPos GNSS-VelPos.
%
% Prototype: vpOut POSSmooth(vp, gnss, nSec, isfig)
% Inputs: vp - INS VelPos input array [vn,pos,t]
% gnss - GNSS VelPos input array [vn,pos,t]
% nSen - smooth span in second
% isfig - figure flag
% Output: vpOut - INS VelPos output after smooth
%
% See also POSFusion, POSProcessing, smoothol, fusion.% Copyright(c) 2009-2021, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 18/10/2021if nargin4, isfig1; endif nargin3, nSec10; enderr gnss(:,end-1) - interp1(vp(:,end), vp(:,end-1), gnss(:,end));gnss gnss(~isnan(err),:); err gnss; errs err;nSec fix(nSec/(gnss(end,end)-gnss(1,end))*length(gnss)); % smooth pointsfor k1:size(gnss,2)-1err(:,end-k) gnss(:,end-k) - interp1(vp(:,end), vp(:,end-k), gnss(:,end));[~, errs(:,end-k)] smoothol(err(:,end-k), nSec, 2, 0);endidx ~isnan(interp1(errs(:,end), errs(:,end-k), vp(:,end)));vpOut vp;for k1:size(gnss,2)-1vpOut(idx,end-k) vp(idx,end-k) interp1(errs(:,end), errs(:,end-k), vp(idx,end), spline);endif isfig1eth earth(gnss(1,end-3:end-1));myfigif size(gnss,2)7, subplot(211); endplot(err(:,end), [err(:,end-3)*eth.RMh, err(:,end-2)*eth.clRNh, err(:,end-1)]); xygo(dP);plot(errs(:,end), [errs(:,end-3)*eth.RMh, errs(:,end-2)*eth.clRNh, errs(:,end-1)],-.,linewidth,2);if size(gnss,2)7subplot(212); plot(err(:,end), err(:,end-6:end-4)); xygo(V); plot(errs(:,end), errs(:,end-6:end-4), -.,linewidth,2)avpcmpplot(gnss, vpOut(:,end-6:end), vp);elseavpcmpplot(gnss, vpOut(:,end-3:end), p);endend该函数是一个用于通过INS-VelPos和GNSS-VelPos数据进行位置平滑的函数。它的输入包括INS VelPos数据数组vp、GNSS VelPos数据数组gnss、平滑时间跨度nSec和一个图形标志isfig。它的输出是经过平滑处理后的INS VelPos数据数组vpOut。
以下是该函数的具体实现步骤
首先根据输入参数判断是否指定了图形标志isfig和平滑时间跨度nSec。如果没有指定则默认isfig为1nSec为10。接下来计算GNSS数据和INS数据之间的误差err。这个误差是通过在INS数据的最后一个点和GNSS数据的最后一个点之间进行线性插值来计算的。然后将gnss数组中的NaN值替换为err数组并将errs数组初始化为err数组。根据平滑时间跨度nSec计算要平滑的点的数量。这是通过将nSec转换为gnss数组中点的数量来完成的。对于每个要平滑的点k计算err(:,end-k)的值。这是通过在INS数据的最后一个点和gnss数据的前一个点之间进行线性插值来计算的。然后使用smoothol函数对err(:,end-k)进行平滑处理得到errs(:,end-k)。根据interp1函数计算出一个指示索引idx它用于确定哪些点的位置需要进行平滑。初始化vpOut数组为原始的vp数组。对于每个要平滑的点k根据errs(:,end-k)和vp(idx,end)的值使用interp1函数进行插值计算并将结果加到vpOut(idx,end-k)中以得到平滑后的位置数据。如果isfig等于1则绘制平滑结果的图形。其中包括误差曲线和平滑后的位置数据曲线。
需要注意的是该函数使用了外部库中的一些函数例如interp1、smoothol和eth。这些函数可能需要在系统上进行相应的设置和配置才能正常运行。
POSProcessing
function [ps, psf] POSProcessing(kf, ins, imu, vpGPS, fbstr, ifbstr)
% POS forward and backward data processing.
% States: phi(3), dvn(3), dpos(3), eb(3), db(3), lever(3), dT(1),
% dKg(9), dKa(6). (total states 6*319634)
%
% Prototype: ps POSProcessing(kf, ins, imu, posGPS, fbstr, ifbstr)
% Inputs: kf - Kalman filter structure array from kfinit
% ins - SINS structure array from insinit
% imu - SIMU data including [wm, vm, t]
% vpGPS - GPS velocity position [lat, lon, heigth, tag, tSecond]
% or [vE, vN, vU, lat, lon, heigth, tag, tSecond], where
% tag may be omitted.
% fbstr,ifbstr - Kalman filter feedback string indicator for
% forward and backward processing. if ifbstr0 then stop
% backward processing
% Output: ps - a structure array, its fields are
% avp,xkpk - forward processing avp, state estimation and
% variance
% iavp,ixkpk - backward processing avp, state estimation and
% variance
%
% Example:
% psinstypedef(346);
% davp0 avperrset([30;-30;30], [0.01;0.01;0.03], [0.01;0.01;0.03]);
% lever [0.; 0; 0]; dT 0.0; r0 davp0(4:9);
% imuerr imuerrset(0.01,100,0.001,1, 0,0,0,0, [0;0;1000],0,[0;0;0;0;10;10],0);
% ins insinit([att; pos], ts);
% kf kfinit(ins, davp0, imuerr, lever, dT, r0);
% ps POSProcessing(kf, ins, imu, vpGPS, avped, avp);
% psf POSFusion(ps.avp, ps.xkpk, ps.iavp, ps.ixkpk);
% POSplot(psf);
%
% See also sinsgps, insupdate, kfupdate, POSFusion, posplot.% Copyright(c) 2009-2014, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 22/01/2014, 08/02/2015if ~exist(fbstr,var), fbstr avp; endif ~exist(ifbstr,var), ifbstr fbstr; endlen length(imu); [lenGPS,col] size(vpGPS);if col4||col7, vpGPS [vpGPS(:,1:end-1),ones(lenGPS,1),vpGPS(:,end)]; end % add tagimugpssyn(imu(:,7), vpGPS(:,end));ts ins.ts; nts kf.nts; nn round(nts/ts);dKga zeros(15,1);%% forward navigation[avp, xkpk, zkrk] prealloc(ceil(len/nn), kf.n1, 2*kf.n1, 2*kf.m1); ki 1;Qk zeros(length(vpGPS), kf.n1); Rk zeros(length(vpGPS), 8); kki 1; zk zeros(size(kf.Hk,1),1);timebar(nn, len, SINS/GPS POS forward processing.);for k1:nn:(len-nn1)k1 knn-1; wvm imu(k:k1,1:6); t imu(k1,7);ins insupdate(ins, wvm); ins inslever(ins);kf.Phikk_1 kffk(ins);kf kfupdate(kf);[kgps, dt] imugpssyn(k, k1, F);if kgps0 if vpGPS(kgps,end-1)1 % tag OKkf.Hk kfhk(ins);if size(kf.Hk,1)6zk [ins.vnL-ins.an*dt; ins.posL-ins.Mpvvn*dt]-vpGPS(kgps,1:6);elsezk ins.posL-ins.Mpvvn*dt-vpGPS(kgps,1:3);endkf kfupdate(kf, zk, M);endQk(kki,:) [diag(kf.Qk); t];Rk(kki,:) [diag(kf.Rk); kf.beta; t]; kki kki1;end[kf, ins] kffeedback(kf, ins, nts, fbstr);dKg ins.Kg-eye(3); dKa ins.Ka-eye(3);dKga [dKg(:,1);dKg(:,2);dKg(:,3); dKa(:,1);dKa(2:3,2);dKa(3,3)];avp(ki,:) [ins.avpL; ins.eb; ins.db; ins.lever; ins.tDelay; dKga; t];xkpk(ki,:) [kf.xk; diag(kf.Pxk); t];zkrk(ki,:) [zk; diag(kf.Rk); t]; ki ki1;timebar;endavp(ki:end,:)[]; xkpk(ki:end,:)[]; zkrk(ki:end,:)[]; Qk(kki:end,:)[]; Rk(kki:end,:)[];ps.avp avp; ps.xkpk xkpk; ps.zkrk zkrk; ps.Qk [sqrt(Qk(:,1:end-1)),Qk(:,end)]; ps.Rk [sqrt(Rk(:,1:6)),Rk(:,7:8)];if ~ischar(ifbstr), return; end%% reverse navigation[ikf, iins, idx] POSReverse(kf, ins);[iavp, ixkpk, izkrk] prealloc(ceil(len/nn), kf.n1, 2*kf.n1, 2*kf.m1); ki 1;timebar(nn, len, SINS/GPS POS reverse processing.);for kk1:-nn:(1nn)ik1 k-nn1; wvm imu(k:-1:ik1,1:6); wvm(:,1:3) -wvm(:,1:3); t imu(ik1-1,7);iins insupdate(iins, wvm); iins inslever(iins);ikf.Phikk_1 kffk(iins);ikf kfupdate(ikf);[kgps, dt] imugpssyn(ik1, k, B);if kgps0 if vpGPS(kgps,end-1)1 % tag OKikf.Hk kfhk(iins);if size(ikf.Hk,1)6zk [iins.vnL-iins.an*dt; iins.posL-iins.Mpvvn*dt]-[-vpGPS(kgps,1:3),vpGPS(kgps,4:6)];elsezk iins.posL-iins.Mpvvn*dt-vpGPS(kgps,1:3);endikf kfupdate(ikf, zk, M);endend[ikf, iins] kffeedback(ikf, iins, nts, ifbstr);dKg iins.Kg-eye(3); dKa iins.Ka-eye(3);dKga [dKg(:,1);dKg(:,2);dKg(:,3); dKa(:,1);dKa(2:3,2);dKa(3,3)];iavp(ki,:) [iins.avpL; iins.eb; iins.db; iins.lever; iins.tDelay; dKga; t];ixkpk(ki,:) [ikf.xk; diag(ikf.Pxk); t];izkrk(ki,:) [zk; diag(ikf.Rk); t]; ki ki1;timebar;endiavp(ki:end,:)[]; ixkpk(ki:end,:)[]; izkrk(ki:end,:)[]; iavp flipud(iavp); ixkpk flipud(ixkpk); izkrk flipud(izkrk); % reverse inverse sequenceiavp(:,idx) -iavp(:,idx); ixkpk(:,idx) -ixkpk(:,idx);ps.iavp iavp; ps.ixkpk ixkpk; ps.izkrk izkrk;if nargout2psf POSFusion(ps.avp, ps.xkpk, ps.iavp, ps.ixkpk);endfunction [ikf, iins, idx] POSReverse(kf, ins)
% Copyright(c) 2009-2014, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 22/01/2014iins ins;iins.eth.wie -iins.eth.wie;iins.vn -iins.vn; iins.eb -iins.eb; iins.tDelay -iins.tDelay;ikf kf;Pddiag(ikf.Pxk); ikf.Pxk diag([10*Pd(1:19);Pd(20:end)]);idx [4:6,10:12,19]; % vn,eb,dT (dKg no reverse!)ikf.xk(idx) -ikf.xk(idx); % !!!POSProcessing_ifk_test
function POSProcessing_ifk_test()
% Copyright(c) 2009-2014, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 27/12/2019
global glvts 1; nts 1; nn 1; T 5000; len T/ts;psinstypedef(186);%% forward navigationts 0.01;avp0 avpset([1;10;30]*glv.deg, [100;200;10], [34;106;100]);davp0 avperrset([60;-60;300], [0.1;0.1;0.3], [1;1;3]);ins insinit(avpadderr(avp0,[[30;-30;30]; [1;1;3]; [1;1;3]]), ts); ins.nts nts;Ft etm(ins);iins ins;iins.eth.wie -iins.eth.wie;iins.vn -iins.vn;iins.eth ethupdate(iins.eth, iins.pos, iins.vn);iFt etm(iins);xk [davp0; [1;1;1]*glv.dph; [100;100;100]*glv.ug];xk1 (eye(15)Ft*ts)*xk;ixk xk1; ixk([4:6,10:12]) -ixk([4:6,10:12]);ixk1 (eye(15)iFt*ts)*ixk;ixk2 (eye(15)-Ft*ts)*ixk;[ixk1,ixk2,xk]avp0 avpset([1;10;30]*glv.deg, [0;0;0], [34;106;100]);davp0 avperrset([1;-1;30], [0.1;0.1;0.3], [0.01;0.01;0.03]);imuerr imuerrset(0, 0, 0, 0);imu imustatic(avp0, ts, len, imuerr);ins insinit(avpadderr(avp0,[[30;-30;30]; [0.1;0.1;0.3]; [0.01;0.01;0.03]]), ts); ins.nts nts;kf kfinit(ins, davp0, imuerr, [0;0;0], 0, [0;0;0]);kf.xk(1:9) davp0;err prealloc(ceil(len/nn), 19); ki 1;timebar(nn, len, SINS/GPS POS forward processing.);for k1:nn:(len-nn1)k1 knn-1; wvm imu(k:k1,1:6); t imu(k1,7);ins insupdate(ins, wvm); ins.vn(3) 0; kf.xk(6) 0;kf.Phikk_1 kffk(ins);kf kfupdate(kf);err(ki,:) [aa2phi(ins.att,avp0(1:3));(ins.avp(4:9)-avp0(4:9)); kf.xk(1:9); t]; ki ki1;timebar;enderr(ki:end,:)[];%% reverse navigation[ikf, iins, idx] POSReverse(kf, ins);ierr prealloc(ceil(len/nn), 19); ki 1;timebar(nn, len, SINS/GPS POS reverse processing.);for kk1:-nn:(1nn)ik1 k-nn1; wvm imu(k:-1:ik1,1:6); wvm(:,1:3) -wvm(:,1:3); t imu(ik1-1,7);iins insupdate(iins, wvm); iins.vn(3) 0; ikf.xk(6) 0;ikf.Phikk_1 kffk(iins);ikf kfupdate(ikf);ierr(ki,:) [aa2phi(iins.att,avp0(1:3));(iins.avp(4:9)-avp0(4:9)); ikf.xk(1:9); t-0.5]; ki ki1;timebar;endierr(ki:end,:)[];
% ierr flipud(ierr); ierr(:,[4:6]) -ierr(:,[4:6]); ierr(:,9[4:6]) -ierr(:,9[4:6]);err [err;ierr];errr avpcmpplot(err(:,[1:9,end]), err(:,10:end));
% avpcmpplot(ierr(:,[1:9,end]), ierr(:,10:end));function [ikf, iins, idx] POSReverse(kf, ins)
% Copyright(c) 2009-2014, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 22/01/2014iins ins;iins.eth.wie -iins.eth.wie;iins.vn -iins.vn; iins.eb -iins.eb; iins.tDelay -iins.tDelay;ikf kf;ikf.Pxk 10*diag(diag(ikf.Pxk));idx [4:6,10:12]; % vn,eb,dT,dvn (dKg no reverse!)ikf.xk(idx) -ikf.xk(idx); % !!!十一模拟
htwn产生高斯白噪声
function [wn, s] htwn(wp, s, len)
% Generate Heavy-tailed white noise.
%
% Prototype: wn htwn(wp, enlarge, len)
% Inputs: wp - with probability
% s - std enlarge with probability wp
% len - element number
% Outputs: wn - noise output
% s - enlarge number for each wn
%
% Example1:
% figure, plot(htwn(0.01, 100, 1000)); grid on
%
% Example2:
% x-30:0.1:30; n1000; figure, plot(x,histc([randn(10000,1),htwn(0.3,10,10000)],x)/n); grid on
%
% See also rstd.% Copyright(c) 2009-2022, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 01/03/2022if length(wp)1 % [wn, s] htwn([wp1;wp2;...], [s1;s2;...], [len1;len2;...]);s1 s;wn []; s [];for k1:length(wp)[wnk,sk] htwn(wp(k), s1(k), len(k));wn [wn; wnk]; s [s; sk];endreturn;endif nargin3, len1; endif nargin2, s100; endif nargin1, wp0.01; endif wp1, lenwp; wp0.01; end % wn htwn(len)wn randn(len,1);r01 rand(len,1);idx r01 wp;wn(idx) wn(idx)*s;s(idx,1) s; s(~idx,1) 1;这段代码是一个用于生成heavy-tailed白噪声的函数。下面是代码的功能解释
函数的输入参数包括 wp一个表示概率的向量或标量用于确定是否对生成的噪声进行放大。s一个表示放大数量的向量或标量用于对生成的噪声进行放大。len一个表示生成噪声的元素数量的向量或标量。 函数的输出包括 wn生成的噪声的向量或矩阵其中包含了heavy-tailed分布的随机值。s每个噪声元素的放大数量。 代码中的主要逻辑如下 首先通过判断输入参数的长度支持同时生成多个噪声序列。如果wp或s或len的长度大于1则进入循环逐个生成噪声序列并返回结果。然后根据输入参数检查是否给出了足够的参数。如果没有给出len参数默认为1如果没有给出s参数默认为100如果没有给出wp参数默认为0.01。接下来生成一个长度为len的随机噪声向量wn其中包含了符合标准正态分布的随机值。然后生成一个长度与wn相同的随机向量r01其中的值在0和1之间。使用生成的随机向量r01和一个阈值wp确定哪些噪声元素需要进行放大。将满足概率小于wp的元素乘以放大数量s并更新相应的放大数量s。 最后将生成的噪声向量和放大数量返回作为函数的输出。
总体而言这段代码通过使用随机生成和放大的方法来生成具有heavy-tailed分布特征的噪声数据。
vfbfx模拟垂直下落物体的状态方程
function [X1, Phik, D] vfbfx(X0, tpara)
% Vertically falling body state equation f(x).
% Ref: Athans, M. Suboptimal state estimation for continuous-time nonlinear systems
% from discrete noisy measurements. IEEE Transactions on Automatic Control,1968.
% [x1,x2,x3]: altitude,velocity,ballistic-parameter, state equations:
% dx1/dt -x2
% dx2/dt g-exp(-gamma*x1)*x2^2*x3
% dx3/dt 0
%
% See also vfbhx.% Copyright(c) 2009-2022, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 08/08/2022% Phik eye(3) [0,1,0; 0,0,1; 0,0,0]*tpara.ts; D{1}zeros(3); D{2}zeros(3); D{3}zeros(3); % linear model test% X1 Phik*X0;% return; % for linear testgamma tpara.gamma; g tpara.g; Ts tpara.Ts;x1 X0(1); x2 X0(2); x3 X0(3);%% X0-X1egx1 exp(-gamma*x1);X1 [ x1 - x2*Ts; % Euler1 discretizationx2 (g-egx1*x2^2*x3)*Ts;x3 ];
% k1 [-X0(2); g-exp(-gamma*X0(1))*X0(2)^2*X0(3); 0]; X01 X0k1*Ts/2; % RK4
% k2 [-X01(2); g-exp(-gamma*X01(1))*X01(2)^2*X01(3); 0]; X02 X0k2*Ts/2;
% k3 [-X02(2); g-exp(-gamma*X02(1))*X02(2)^2*X02(3); 0]; X03 X0k3*Ts;
% k4 [-X03(2); g-exp(-gamma*X03(1))*X03(2)^2*X03(3); 0];
% X1 X0 Ts/6*(k12*(k2k3)k4);%% Jacobian Phikif nargout1Phik [ 0, -1, 0;[gamma*x2*x3, -2*x3, -x2]*egx1*x2;0, 0, 0 ];Phik eye(size(Phik)) Phik*Ts; % Phi expm(Phi*ts);end%% Hessian Dif nargout2D{1} zeros(3);D{2} [ [-gamma*x2*x3, 2*x3, x2]*gamma*egx1*x2;0, -2*egx1*x3, -2*egx1*x20, 0, 0 ]*Ts;D{2}(2,1)D{2}(1,2); D{2}(3,1)D{2}(1,3); D{2}(3,2)D{2}(2,3); % symmetricD{3} zeros(3);end这段代码是一个用于模拟垂直下落物体的状态方程的函数。它接受一个初始状态向量X0和一个包含参数的向量tparana作为输入并返回预测的状态向量X1、雅可比矩阵Phik和海森矩阵D。
下面是代码的功能解释
定义参数 gamma阻力系数g重力加速度Ts离散时间步长 初始化变量 从X0中提取x1、x2和x3的值分别存储在这些变量中。预测状态向量X1 使用Euler离散化方法对状态方程进行预测得到下一个时刻的状态向量X1。计算雅可比矩阵Phik 如果函数有超过一个输出参数则计算雅可比矩阵Phik。Phik用于线性化状态方程其在数值上近似于状态方程的导数。计算海森矩阵D 如果函数有超过两个输出参数则计算海森矩阵D。D是用于线性化状态方程的雅可比矩阵的导数即D是对状态方程的二阶导数。
在这段代码中使用了Euler离散化方法对状态方程进行预测并计算了状态方程的雅可比矩阵和海森矩阵。这些矩阵对于进行状态估计和参数辨识等任务非常重要。
vfbhx模拟垂直下落的物体的测量方程
function [Z, Hk, D] vfbhx(X, tpara, rk)
% Vertically falling body measurement equation h(x).
% Ref: Athans, M. Suboptimal state estimation for continuous-time nonlinear systems
% from discrete noisy measurements. IEEE Transactions on Automatic Control,1968.
% [x1,x2,x3]: altitude,velocity,ballistic-parameter, measurement equation:
% Z sqrt(M^2(x3-H)^2) V
%
% See also vfbfx.% Copyright(c) 2009-2022, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 08/08/2022% Z X(1); Hk [1, 0, 0]; D{1} zeros(3); % linear model test% if nargin3, ZZV*randn(1); end% return; % for linear testM tpara.M; H tpara.H;%% ZZ sqrt(M^2(X(1)-H)^2);%% Jacobian Hkif nargout1, Hk [(X(1)-H)/Z, 0, 0]; end%% Hessian Dif nargout2, D{1} [M^2/Z^3, 0, 0; 0, 0, 0; 0, 0, 0]; end%% Measurement noiseif nargin3, ZZrk*randn(1); end这段代码是一个MATLAB函数用于计算垂直下落的物体的测量方程。下面是代码的功能解释
函数名vfbhx它接受三个输入参数Xtpara和rk。X是一个列向量包含了三个元素高度、速度和弹道参数。tpara是一个包含两个元素的向量第一个元素是质量参数M第二个元素是参考高度H。rk是测量噪声的倍数用于模拟真实测量中的噪声。代码开头的注释提供了一些参考信息包括参考文献和版权声明。代码的第一个注释行是一个测试用的线性模型如果函数有四个输入参数即nargin3则在计算结果之前向Z中添加一个乘以V的随机噪声。Z的计算是根据测量方程进行的使用了高度和弹道参数并考虑了距离的影响。如果函数需要输出雅可比矩阵Hknargout2则根据测量方程计算出雅可比矩阵的值。如果函数需要输出海森矩阵Dnargout3则根据测量方程计算出海森矩阵的元素值。最后如果输入参数中包含第三个元素rk则在计算结果之前向Z中添加一个乘以rk的随机噪声以模拟测量中的噪声。函数的最后一行是可选的它用于在命令行中显示函数的输出结果。
总结来说这段代码实现了一个用于计算垂直下落的物体的测量方程的函数。它根据输入的高度、速度、弹道参数和测量噪声计算出测量结果并可以输出雅可比矩阵和海森矩阵如果需要。
方法对状态方程进行预测并计算了状态方程的雅可比矩阵和海森矩阵。这些矩阵对于进行状态估计和参数辨识等任务非常重要。
vfbhx模拟垂直下落的物体的测量方程
function [Z, Hk, D] vfbhx(X, tpara, rk)
% Vertically falling body measurement equation h(x).
% Ref: Athans, M. Suboptimal state estimation for continuous-time nonlinear systems
% from discrete noisy measurements. IEEE Transactions on Automatic Control,1968.
% [x1,x2,x3]: altitude,velocity,ballistic-parameter, measurement equation:
% Z sqrt(M^2(x3-H)^2) V
%
% See also vfbfx.% Copyright(c) 2009-2022, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 08/08/2022% Z X(1); Hk [1, 0, 0]; D{1} zeros(3); % linear model test% if nargin3, ZZV*randn(1); end% return; % for linear testM tpara.M; H tpara.H;%% ZZ sqrt(M^2(X(1)-H)^2);%% Jacobian Hkif nargout1, Hk [(X(1)-H)/Z, 0, 0]; end%% Hessian Dif nargout2, D{1} [M^2/Z^3, 0, 0; 0, 0, 0; 0, 0, 0]; end%% Measurement noiseif nargin3, ZZrk*randn(1); end这段代码是一个MATLAB函数用于计算垂直下落的物体的测量方程。下面是代码的功能解释
函数名vfbhx它接受三个输入参数Xtpara和rk。X是一个列向量包含了三个元素高度、速度和弹道参数。tpara是一个包含两个元素的向量第一个元素是质量参数M第二个元素是参考高度H。rk是测量噪声的倍数用于模拟真实测量中的噪声。代码开头的注释提供了一些参考信息包括参考文献和版权声明。代码的第一个注释行是一个测试用的线性模型如果函数有四个输入参数即nargin3则在计算结果之前向Z中添加一个乘以V的随机噪声。Z的计算是根据测量方程进行的使用了高度和弹道参数并考虑了距离的影响。如果函数需要输出雅可比矩阵Hknargout2则根据测量方程计算出雅可比矩阵的值。如果函数需要输出海森矩阵Dnargout3则根据测量方程计算出海森矩阵的元素值。最后如果输入参数中包含第三个元素rk则在计算结果之前向Z中添加一个乘以rk的随机噪声以模拟测量中的噪声。函数的最后一行是可选的它用于在命令行中显示函数的输出结果。
总结来说这段代码实现了一个用于计算垂直下落的物体的测量方程的函数。它根据输入的高度、速度、弹道参数和测量噪声计算出测量结果并可以输出雅可比矩阵和海森矩阵如果需要。 文章转载自： http://www.morning.fbmjw.cn.gov.cn.fbmjw.cn http://www.morning.pqqhl.cn.gov.cn.pqqhl.cn http://www.morning.lxthr.cn.gov.cn.lxthr.cn http://www.morning.nhlyl.cn.gov.cn.nhlyl.cn http://www.morning.fpkpz.cn.gov.cn.fpkpz.cn http://www.morning.alwpc.cn.gov.cn.alwpc.cn http://www.morning.pcjw.cn.gov.cn.pcjw.cn http://www.morning.ptdzm.cn.gov.cn.ptdzm.cn http://www.morning.rklgm.cn.gov.cn.rklgm.cn http://www.morning.rmltt.cn.gov.cn.rmltt.cn http://www.morning.yzmzp.cn.gov.cn.yzmzp.cn http://www.morning.nxstj.cn.gov.cn.nxstj.cn http://www.morning.zkpwk.cn.gov.cn.zkpwk.cn http://www.morning.bangaw.cn.gov.cn.bangaw.cn http://www.morning.xnzmc.cn.gov.cn.xnzmc.cn http://www.morning.ylljn.cn.gov.cn.ylljn.cn http://www.morning.hbqhz.cn.gov.cn.hbqhz.cn http://www.morning.chjnb.cn.gov.cn.chjnb.cn http://www.morning.lsnnq.cn.gov.cn.lsnnq.cn http://www.morning.qtqjx.cn.gov.cn.qtqjx.cn http://www.morning.gwxsk.cn.gov.cn.gwxsk.cn http://www.morning.ynlpy.cn.gov.cn.ynlpy.cn http://www.morning.myfwb.cn.gov.cn.myfwb.cn http://www.morning.lbbyx.cn.gov.cn.lbbyx.cn http://www.morning.rqkck.cn.gov.cn.rqkck.cn http://www.morning.ywqw.cn.gov.cn.ywqw.cn http://www.morning.fhyhr.cn.gov.cn.fhyhr.cn http://www.morning.dansj.com.gov.cn.dansj.com http://www.morning.xjkfb.cn.gov.cn.xjkfb.cn http://www.morning.gkdhf.cn.gov.cn.gkdhf.cn http://www.morning.cpfbg.cn.gov.cn.cpfbg.cn http://www.morning.hmqwn.cn.gov.cn.hmqwn.cn http://www.morning.dyxlj.cn.gov.cn.dyxlj.cn http://www.morning.xcszl.cn.gov.cn.xcszl.cn http://www.morning.ttkns.cn.gov.cn.ttkns.cn http://www.morning.qtryb.cn.gov.cn.qtryb.cn http://www.morning.sgbss.cn.gov.cn.sgbss.cn http://www.morning.rxlck.cn.gov.cn.rxlck.cn http://www.morning.zmyhn.cn.gov.cn.zmyhn.cn http://www.morning.rklgm.cn.gov.cn.rklgm.cn http://www.morning.nhzxd.cn.gov.cn.nhzxd.cn http://www.morning.qmwzr.cn.gov.cn.qmwzr.cn http://www.morning.ftznb.cn.gov.cn.ftznb.cn http://www.morning.hqgkx.cn.gov.cn.hqgkx.cn http://www.morning.nrftd.cn.gov.cn.nrftd.cn http://www.morning.kgtyj.cn.gov.cn.kgtyj.cn http://www.morning.lwygd.cn.gov.cn.lwygd.cn http://www.morning.gmwdl.cn.gov.cn.gmwdl.cn http://www.morning.jycr.cn.gov.cn.jycr.cn http://www.morning.dtzsm.cn.gov.cn.dtzsm.cn http://www.morning.fy974.cn.gov.cn.fy974.cn http://www.morning.kqzrt.cn.gov.cn.kqzrt.cn http://www.morning.ymrq.cn.gov.cn.ymrq.cn http://www.morning.xlwpz.cn.gov.cn.xlwpz.cn http://www.morning.tpchy.cn.gov.cn.tpchy.cn http://www.morning.fyzsq.cn.gov.cn.fyzsq.cn http://www.morning.lkbdy.cn.gov.cn.lkbdy.cn http://www.morning.qbmpb.cn.gov.cn.qbmpb.cn http://www.morning.jjmrx.cn.gov.cn.jjmrx.cn http://www.morning.xfdkh.cn.gov.cn.xfdkh.cn http://www.morning.hcszr.cn.gov.cn.hcszr.cn http://www.morning.hwsgk.cn.gov.cn.hwsgk.cn http://www.morning.fnfhs.cn.gov.cn.fnfhs.cn http://www.morning.pumali.com.gov.cn.pumali.com http://www.morning.mfltz.cn.gov.cn.mfltz.cn http://www.morning.zwyuan.com.gov.cn.zwyuan.com http://www.morning.xtrzh.cn.gov.cn.xtrzh.cn http://www.morning.frtb.cn.gov.cn.frtb.cn http://www.morning.zqybs.cn.gov.cn.zqybs.cn http://www.morning.rbhcx.cn.gov.cn.rbhcx.cn http://www.morning.nlwrg.cn.gov.cn.nlwrg.cn http://www.morning.mmzfl.cn.gov.cn.mmzfl.cn http://www.morning.byshd.cn.gov.cn.byshd.cn http://www.morning.rkmhp.cn.gov.cn.rkmhp.cn http://www.morning.xrqkm.cn.gov.cn.xrqkm.cn http://www.morning.btqqh.cn.gov.cn.btqqh.cn http://www.morning.bsqth.cn.gov.cn.bsqth.cn http://www.morning.3jiax.cn.gov.cn.3jiax.cn http://www.morning.yggdq.cn.gov.cn.yggdq.cn http://www.morning.qlznd.cn.gov.cn.qlznd.cn
