PSINS工具箱学习（三）让 AI 解释 PSINS 中的各种卡尔曼滤波代码：RLS、UDRLS、KF、UDKF、EKF、CKF、UKF、SSUKF、PF、AKF、IGG1、IGG3

PSINS 中的 Kalman 滤波代码都在百行以内，没调用什么函数，而且通用性很强，拿去让 AI 解释，效果挺好。

一、RLS

RLS（递归最小二乘估计）是一种常用的自适应滤波算法，用于估计线性系统的参数1。

RLS估计的基本思想是通过对输入信号进行迭代最小化误差平方和，来逐步调整估计器的增益系数，从而不断优化估计器的性能。具体来说，RLS估计的步骤包括1：

1. 初始化估计器的增益系数和误差方差。
2. 对于每个输入信号采样，计算输出信号的预测值和实际值之间的误差。
3. 根据误差和当前增益系数，计算出新的误差方差和增益系数。
4. 重复执行步骤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/2013
    if ~isfield(kf, 'Rk') 
        kf.Rk = eye(size(kf.Hk,1));
    end
    kf.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.Pxk+kf.Pxk')*0.5;

这是一个递归最小二乘（Recursive Least Square，RLS）滤波器的函数。它用于更新滤波器结构体数组 kf，并根据测量向量 zk 进行滤波操作。

函数的输入参数有 kf 和 zk，其中 kf 是滤波器的结构体数组，zk 是测量向量。函数的输出参数也是 kf，表示经过滤波操作后的滤波器结构体数组。

この関数は、最初に Rk フィールドが kf 構造体配列に存在するかどうかを判断し、存在しない場合は、Rk を単位行列として初期化します。次に、Pxzk = Pxk * Hk'、Pzk = Hk * Pxzk + Rk を計算します。次に、ゲイン行列 Kk = Pxzk * Pzk^-1 を計算します。次に、状態ベクトル xk = xk + Kk * (zk - Hk * xk) を更新します。次に、Pxk = Pxk - Kk * Pxzk' を更新します。最後に、Pxk を対称化して対称行列にします。

このコードの目的は、システム状態を推定するための再帰的最小二乗フィルターの更新プロセスを実装することです。

RLSUD：OUT分解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/2023
    if 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;
    end
    if nargin<4, R=1; end
    n = length(D);
    f = (H*U)';  g = D.*f;  afa = f'*g+R;
    for j=n:-1:1
        afa0 = afa - f(j)*g(j); lambda = -f(j)/afa0;
        D(j) = afa0/afa*D(j);   afa = afa0;
        for i=(j-1):-1:1
            s = (i+1):(j-1);
            U(i,j) = U(i,j) + lambda*(g(i)+U(i,s)*g(s));
        end
    end
    if nargout>2
        K = U*(D.*(H*U)')/R;
        if nargout>3, X = X + K*(Z-H*X); end
    end

このコードは、UD 分解手法を使用した再帰的最小二乗フィルターの実装です。その機能の説明は次のとおりです。

1. 入力パラメータ:
   • U: UD 分解における左特異行列。
   • D: UD 分解における右特異行列。
   • H: 1 行の測定マトリックス。
   • R: 測定ノイズの分散。
   • Z:測定。
   • X: ステータス値。
2. 出力パラメータ:
   • U: 更新された左特異行列。
   • D: 更新された右特異行列。
   • X: 更新された状態値 (出力パラメーターにのみ存在します)。
   • K: ゲイン (出力パラメータにのみ存在します)。
3. 機能説明:
   1. まず、コードは入力パラメータが構造体かどうかをチェックします。それが構造体の場合、関数を呼び出す方法は、kf = RLSUD(kf, z);現在の状態値kf.xkと測定値でz構造体変数を更新することであることを意味しますkf。この場合、関数はkf追加の計算を行わずに、更新された構造体変数を直接返します。
   2. 入力パラメータが構造体でない場合は、引き続き以下のコードの実行を続けます。fまず、中間変数、g、 、が入力パラメータ から計算されますafa。このうち、との行列積の転置f = (H*U)'を意味し、 と の要素ごとの乗算を意味し、と の要素ごとの乗算を意味し、その結果を加算します。HUg = D.*fDfafa = f'*g+Rf'gR
   3. 次に、行列の列を 1 つずつループし、最後の列から逆の順序で繰り返します。各列の処理中に、まず新しいゲイン係数 を計算しlambda、再帰式に従って新しい対角要素を計算しD(j)、U上三角行列の対応する要素を更新します。具体的な計算プロセスは次のとおりです。
      1. まず、現在の列との積項を減算するafa0 = afa - f(j)*g(j)一時変数が計算されます。afa
      2. 次に、ゲイン係数 を計算しますlambda = -f(j)/afa0。ここで、負の符号は最小二乗法の使用を示します。
      3. 次に、再帰式に従って新しい対角要素が計算されますD(j) = afa0/afa*D(j)。
      4. 最後に、ゲイン係数lambdaと中間変数を使用してg(i)、U(i,s)上三角行列Uの対応する要素を更新します。
   4. すべての列が処理された後、出力パラメーターでゲインの計算が必要な場合K、K = U*(D.(H*U)')/Rゲインは次の式に従って計算されます。ここで、.は行列積の転置を表します。
   5. 出力パラメータが状態値の更新を必要とする場合X、X = X + K*(Z-H*X)状態値は式に従って更新されます。Z-H*Xここで、 は予測誤差です。
4. 概要: このコードは、UD 分解手法を使用して行列を更新し、ゲインを計算する再帰的最小二乗フィルターを実装します。行列を列ごとに処理することでフィルタの状態とゲイン係数を徐々に更新し、フィルタのフィルタリング機能を実現します。

シガレット

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/2016
    n = length(P);
    U = eye(n); D = zeros(n,1);  trPn = trace(P)/n*1e-40;
    for j=n:-1:1
        k = (j+1):n;
        D(j) = P(j,j) - (U(j,k).^2)*D(k);
        if D(j)<=trPn, continue; end
        for i=(j-1):-1:1
            U(i,j) = (P(i,j)-(U(i,k).*U(j,k))*D(k)) / D(j);
        end
    end

このコードは、非負定対称行列 P を U * diag(D) * U' の形式に分解する UDUT 分解 (対角相似変換による上三角分解) を実装します。2. 最後の行から始めて、各行が上方向に処理されます。3. まず現在の行 j の対角要素 D(j) を計算し、対角要素 P(j ,j) を減算します。4. D(j) が trPn 以下の場合、要素が非常に小さく、分解結果にほとんど影響を与えないことを意味するため、この行の処理はスキップされます。5. D(j) が trPn より大きい場合、現在の行の要素 P(i,j) と前の行 i を使用して、現在の行 j と前の行 i の要素 U(i,j) を計算する必要があります。前の上三角要素 U(i,k) と U(j,k) および D(k) が補間され、D(j) で除算されます。6. ループ内のすべての行を処理した後、出力 U と D は UDUT 分解の結果になります。

このコードの目的は、後続の計算と分析のために、非負の定対称行列を UDUT に分解することです。UDUT 分解は、一次方程式の解法、行列の行列式の計算、行列の固有値と固有ベクトルの計算などのアプリケーションに使用できます。

2、カルマンフィルター

カルマン フィルターは、一連の不完全でノイズの多い測定から動的システムの状態を推定できる高効率の再帰フィルター (自己回帰フィルター) です。

カルマン フィルタリングの主なアイデアは、状態遷移行列と観測行列を現在の状態と直前の状態と組み合わせて使用​​し、現在の最適な状態を推定することです1。具体的には、カルマン フィルタリングの手順には次の 1 つが含まれます。

1. フィルターの状態ベクトルと共分散行列を初期化します。
2. 時間ステップごとに、次の手順が実行されます。
   1. 状態遷移行列と現在の状態ベクトルを用いて、次の瞬間の状態ベクトルと共分散行列を計算する。
   2. 観測行列と現在の観測ベクトルを組み合わせて、現時点での状態推定値と共分散行列が計算されます。
   3. 新しい状態推定と共分散行列に基づいて、フィルターの状態ベクトルと共分散行列を更新します。
3. 収束条件が満たされるまで、ステップ 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 Gongmin
    if nargin<9, s=1; end
    Xkk_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 = (Pxk+Pxk')/2;

このコードは、単純なカルマン フィルターの実装です。カルマン フィルターは、以前の推定値と最新の観測値を組み合わせて現在の状態の推定値を更新することにより、動的システムの状態を推定する方法です。

コードの機能の説明は次のとおりです。

1. 入力パラメータ:
   • Phikk_1: システム状態の遷移モードを記述する状態遷移マトリックス。
   • Gammak: システム プロセスに導入されたノイズを表すプロセス ノイズ共分散行列。
   • Qk: システム プロセスに導入されたノイズを表すプロセス ノイズ共分散行列。
   • Xk_1: 以前の状態の推定値。
   • Zk:データ観察。
   • s: カルマン ゲインの平滑化係数を調整するために使用されるオプションのパラメーター。デフォルトは 1 です。
2. 初期化:
   • Xkk_1: 状態遷移行列と以前の状態推定に基づいて、現在の状態の予測値を計算します。
   • Pxkk_1: 状態遷移行列、プロセス ノイズ共分散行列、および以前の状態推定誤差共分散行列に従って、現在の状態予測値の誤差共分散行列を計算します。
3. 予測する：
   • Pxykk_1: 現在の状態の予測値の誤差共分散擬似行列を、予測された状態と観測値の間の共分散行列に変換します。
   • Pykk_1: 観測行列、予測状態と観測の間の共分散行列、および観測ノイズ共分散行列に従って、予測観測ノイズ共分散行列を計算します。
4. 更新する:
   • Kk: カルマン ゲインは、予測された状態と観測値の間の共分散行列の逆行列と、予測された観測ノイズ共分散行列から計算されます。
   • Xk: 現状予測値と観測データのカルマンゲインに従って現状を更新します。
   • Pxk: 現在の状態の更新された誤差共分散行列から、カルマン ゲインと予測された観測ノイズ共分散行列の積の転置を減算することで、更新された誤差共分散行列が得られます。
   • Pxkk_1: 現在の状態の予測値の誤差共分散行列。

このコードは、入力パラメーターに従って状態の推定と予測を実行し、現在の状態の推定値と誤差共分散行列などの関連情報を出力できる単純なカルマン フィルターを実装します。

KFUD：UD分解カルマンフィルター

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/2016
    n = length(U);
    W = [Phi*U, Tau];   D1 = [D; Q];  % NOTE: D,Q are both vector
    meanD = mean(D)/n*1e-40;
    for j=n:-1:1   % time updating
        D(j) = (W(j,:).*W(j,:))*D1;
        for i=1:(j-1)
            if D(j)<=meanD, U(i,j) = 0;
            else           U(i,j) = (W(i,:).*W(j,:))*D1/D(j);  end
            W(i,:) = W(i,:) - U(i,j)*W(j,:);
        end
    end
    if ~exist('R', 'var'), return; end
    f = (H*U)';  g = D.*f;  afa = f'*g+R;
    for j=n:-1:1   % measurement updating
        afa0 = afa - f(j)*g(j); lambda = -f(j)/afa0;
        D(j) = afa0/afa*D(j);   afa = afa0;
        for i=1:(j-1)
            s = (i+1):(j-1);
            U(i,j) = U(i,j) + lambda*(g(i)+U(i,s)*g(s));
        end
    end
    K = U*(D.*(H*U)')/R;

この関数は、ユニバーサル対角カルマン フィルター (UDUT 平方根カルマン フィルター) と呼ばれるアルゴリズムを実装します。UD 分解カルマン フィルターは、非線形システムの処理に適した拡張状態推定アルゴリズムです。

この関数は次の入力パラメータを受け入れます。

• U: 状態遷移行列を表すために使用される単位上三角行列。
• D: 対角行列の要素を表すベクトル。
• Phi: システムの動的モデルを表すシステム マトリックス。
• Tau: プロセス ノイズ (プロセス ノイズとも呼ばれます) の分布行列を表すベクトル。
• Q: プロセス ノイズの分散を表すベクトル (ベクトルとして表現)。
• H: 状態から測定値を取得する方法を示す測定行列。
• R: 測定ノイズの分散。

この関数は次の出力パラメータを返します。

• U: 更新された単位上三角行列。
• D: 更新された対角行列の要素のベクトル。
• K: カルマンフィルターゲイン。

アルゴリズムの主なステップは次のとおりです。

1. 初期化: 入力パラメータに従って、計算中の一時変数を含むいくつかの変数を初期化します。
2. 時間更新: 各タイム ステップを後ろから前にトラバースし、システムの動的モデルとプロセス ノイズに従って、各タイム ステップでの状態予測と対角行列の要素を計算します。予測された対角要素がしきい値 ( meanD) より小さい場合、小さすぎる値によって引き起こされる数値の不安定性を避けるために、その要素はゼロに設定されます。
3. 測定値の更新: 測定値のノイズ分散が存在する場合 (つまり、Rゼロでない場合)、測定値の更新ステップが実行されます。まず、予測された測定残差 ( f) と残差の重み ( g) を計算します。次に、各時間ステップが後ろから前にトラバースされ、測定残差と重みに従って対角行列の要素と対角要素の重みが更新されます。次に、更新された対角要素に従ってカルマン フィルターのゲイン () が更新されますK。
4. 出力を返す: 更新された単位上三角行列、対角行列の要素のベクトル、およびカルマン フィルター ゲインを返します。

この関数の出力は、さらなる状態推定または制御アプリケーションで使用できます。

kfc2d: 連続時間カルマン フィルターの離散化

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/2012
    Phikk_1 = eye(size(Ft)) + Ts*Ft;
    Qk = Qt*Ts;
    if nargin<4 % n==1
        return;
    end
    Tsi = Ts; facti = 1; Fti = Ft; Mi = Qt;
    for i=2:1:n
        Tsi = Tsi*Ts;        
        facti = facti*i;
        Fti = Fti*Ft;
        Phikk_1 = Phikk_1 + Tsi/facti*Fti;  % Phikk_1
        FtMi = Ft*Mi;
        Mi = FtMi + FtMi';
        Qk = Qk + Tsi/facti*Mi;  % Qk
    end    

このコードは、連続時間カルマンフィルターの差分方程式系を離散時間モデルに変換する関数です。関数の名前はkfc2dで、その入力パラメータには、連続時間システムの遷移行列Ft、システム ノイズ分散行列Qt、離散化時間間隔Ts、およびテイラー級数展開の次数が含まれますn。関数の出力には、離散時間遷移行列Phikk_1とノイズ分散行列が含まれますQk。

コードの主なロジックは次のとおりです。

1. まず、離散時間遷移行列がPhikk_1単位行列と 1 次近似項を加算したものとして初期化されますTs*Ft。
2. ノイズ分散行列をQk1 次近似として初期化しますQt*Ts。
3. 入力nが 1 (デフォルト値) 以下の場合は、直接返されます。
4. ループに入り、2 次から開始して高次の近似を繰り返し計算します。
5. 各反復において、更新間隔はTsi現在の間隔のべき乗、つまり ですTsi = Tsi*Ts。
6. 次数係数をfacti現在の次数係数の累乗に更新します。つまり、 ですfacti = facti*i。
7. 連続時間システムの遷移行列をFti現在の遷移行列の累乗、つまり に更新しますFti = Fti*Ft。
8. 現在の時間間隔と順序係数の積を現在の遷移行列に加算すると、離散時間遷移行列が得られますPhikk_1 = Phikk_1 + Tsi/facti*Fti。
9. ノイズ分散行列の計算を更新するには、現在の時間間隔と現在のノイズ分散行列の積、および転置演算によって新しいノイズ分散行列が取得されますMi = FtMi + FtMi'。
10. 現在の時間間隔と次数係数の積を新しいノイズ分散行列に加算すると、離散時間ノイズ分散行列が得られますQk = Qk + Tsi/facti*Mi。
11. ループの最後に、離散時間遷移行列Phikk_1とノイズ分散行列が返されますQk。

このコードは主に、連続時間モデルの差分方程式に基づくテイラー級数展開法を通じて離散時間遷移行列とノイズ分散行列を計算し、離散状態での状態推定と予測にカルマン フィルターを適用するために使用されます。 -時間環境。

3. ekf: 拡張カルマン フィルター

EKF (拡張カルマン フィルター、略して拡張カルマン フィルター) は、一連の不完全でノイズの多い測定から動的システムの状態を推定できる、高効率の再帰フィルター (自己回帰フィルター) です。

EKF フィルタリングの主なアイデアは、状態遷移行列と観測行列を現在の状態と直前の状態と組み合わせて使用​​し、現在の最適な状態を推定することです1。具体的には、EKF フィルタリングの手順には 1 が含まれます。

1. フィルターの状態ベクトルと共分散行列を初期化します。
2. 時間ステップごとに、次の手順が実行されます。

a. 状態遷移行列と現在の状態ベクトルを使用して、次の瞬間の状態ベクトルと共分散行列を計算します。

b. 観測行列と現在の観測ベクトルを組み合わせて、現時点での状態推定値と共分散行列を計算します。

c. 新しい状態推定値と共分散行列に基づいて、フィルターの状態ベクトルと共分散行列を更新します。3. 収束条件が満たされるまで、ステップ 2 を繰り返して最適化を繰り返します。

従来のフィルターと比較して、EKF フィルター処理は数値安定性と計算効率が優れており、状態変数の値をより正確に推定でき、高次元で強い非線形性を持つシステムでより優れたパフォーマンスを発揮します。

function kf = ekf(kf, yk, TimeMeasBoth)
    if nargin==1;
        TimeMeasBoth = 'T';
    elseif nargin==2
        TimeMeasBoth = 'B';
    end

    if TimeMeasBoth=='T' || TimeMeasBoth=='B'
        if 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; end
        else
            kf.xkk_1 = kf.Phikk_1*kf.xk;
        end
        kf.Pxkk_1 = kf.Phikk_1*kf.Pxk*kf.Phikk_1' + kf.Gammak*kf.Qk*kf.Gammak';
        if TimeMeasBoth=='T'    % time updating only
            kf.xk = kf.xkk_1; kf.Pxk = kf.Pxkk_1;
            return;
        end
    end

    if TimeMeasBoth=='M' || TimeMeasBoth=='B'
        if TimeMeasBoth=='M'    % meas updating only
            kf.xkk_1 = kf.xk; kf.Pxkk_1 = kf.Pxk;
        end
        if 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; end
        else
            kf.ykk_1 = kf.Hk*kf.xkk_1;
        end
        kf.Pxykk_1 = kf.Pxkk_1*kf.Hk';    kf.Pykk_1 = kf.Hk*kf.Pxykk_1 + kf.Rk;
        % filtering
        kf.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.Pxk+kf.Pxk')/2;
    end

このコードは、拡張カルマン フィルター (EKF) の実装です。コードの動作については次のとおりです。

1. この関数は、(拡張カルマン フィルターの状態)、(測定値)、(時間測定の種類を示す文字) のekf3 つの入力引数を受け入れます。kfykTimeMeasBoth

2. 入力引数の数に応じて、TimeMeasBothの値を決定します。入力パラメータの数が 1 の場合はTimeMeasBoth文字「T」に設定され、入力パラメータの数が 2 の場合はTimeMeasBoth文字「B」に設定されます。

3. TimeMeasBothの値に応じて、次の操作を実行します。

   1. TimeMeas Both が文字「T」または文字「B」の場合:

      1. kfフィールド 'fx' (非線形状態ヤコビ行列) が に存在する場合、関数を呼び出して の 'Phikk_1' と 'xkk_1' (それぞれ状態遷移行列と次の瞬間の状態推定) を計算ekfJcbします。kf

         function [Jcb, y] = ekfJcb(hfx, x, tpara)
             [Jcb, y] = feval(hfx, x, tpara);
         

      2. 空の場合は、現在の状態を乗算するxkk_1ように設定されます。Phikk_1xk

      3. fx存在しない場合は、 xkk_1「Phikk_1」に現在の状態を乗算した値に設定しますxk。

      4. 、、、および の値Pxkk_1を使用して (次の瞬間の共分散行列推定値)を計算します。Phikk_1PxkGammakQkGammak

      5. がTimeMeasBoth文字の場合は時間更新のみを行い、T次の瞬間の状態推定値xkk_1と共分散行列Pxkk_1を現在の状態xkと共分散行列に代入してPxk返します。

   2. TimeMeasBothが文字の場合はB、次のように処理します。

      1. 次の瞬間の状態推定値xkk_1と共分散行列Pxkk_1、および測定値ykとノイズ共分散行列に従ってRk、カルマン ゲイン (カルマン ゲイン) と更新された状態推定値xkと共分散行列を計算しますPxk。
      2. 更新された状態推定値xkと共分散行列をPxk現在の状態xkと共分散行列に割り当てますPxk。

4. 関数の実行後、更新された拡張カルマン フィルターの状態が出力されますkf。

全体として、このコードは拡張カルマン フィルターの時間更新および測定値更新プロセスを実装しており、入力状態、測定値、および時間測定タイプに従ってフィルターの状態と共分散行列を更新します。

4. UKF: 無香料カルマンフィルター

UKF (Unscented Kalman Filter)、中国語の解釈では、無香料カルマン フィルター、無香料カルマン フィルター、または脱芳香カルマン フィルターと呼ばれ、非線形非ガウス システムの状態推定用のフィルター アルゴリズムです1。

UKF フィルターの主なアイデアは、Unscented 変換を使用して、線形仮定の下で非線形システム方程式を標準のカルマン フィルター システムに変換することです。具体的には、UKF フィルタリングの手順には次のものが含まれます。

1. Unscented 変換のパラメータのセットを選択し、Unscented 変換の重みと平均を計算します。
2. システム モデルと観測モデルに対して 1 次テイラー展開を実行して、非線形残差項を取得します。
3. これらの残差項は、状態推定を更新するために使用されます。
4. 収束条件が満たされるまで、ステップ 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,
%            TimeMeasBoth='T' (or nargin==1) for time updating only, 
%            TimeMeasBoth='M' for measurement updating only, 
%            TimeMeasBoth='B' (or nargin==2) 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/2012
    if nargin==1;
        TimeMeasBoth = 'T';
    elseif nargin==2
        TimeMeasBoth = 'B';
    end

    if ~isfield(kf, 'alpha')
        kf.alpha = 1e-3; kf.beta = 2; kf.kappa = 0;
    end
    
    if TimeMeasBoth=='T' || TimeMeasBoth=='B'
        if 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';
        else
            kf.xkk_1 = kf.Phikk_1*kf.xk;
            kf.Pxkk_1 = kf.Phikk_1*kf.Pxk*kf.Phikk_1' + kf.Gammak*kf.Qk*kf.Gammak';
        end
        if TimeMeasBoth=='T'    % time updating only
            kf.xk = kf.xkk_1; kf.Pxk = kf.Pxkk_1;
            return;
        end
    end
    
    if TimeMeasBoth=='M' || TimeMeasBoth=='B'
        if TimeMeasBoth=='M'    % meas updating only
            kf.xkk_1 = kf.xk; kf.Pxkk_1 = kf.Pxk;
        end
        if 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;
        else
            kf.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
        % filtering
        kf.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.Pxk+kf.Pxk')/2;
    end

このコードは UKF の実装です。コードの動作については次のとおりです。

1. 入力パラメータ:
   • kfxk: 状態ベクトル、状態共分散行列Pxk、時間更新係数fx、測定更新係数hxなどのカルマン フィルターの状態およびパラメーター情報を含む構造体の配列。
   • yk: 測定ベクトル、つまりシステムの観測値。
   • TimeMeasBoth: 時刻更新と計測更新の方法を指定する文字列で、オプション値は'T'(時刻更新のみ)、'M'(計測更新のみ)、'B'(時刻と計測の同時更新)です。
2. 入力パラメータに従って更新方法を決定します。
   • 入力パラメータが指定されてTimeMeasBothいる場合'T'でも指定されていない場合でも、時刻の更新が実行されます。
   • 入力パラメータが指定されてTimeMeasBothいる'M'か指定されていない場合、測定値の更新が実行されます。
3. 時間の更新:
   1. 構造体配列kfにalphaおよびbetaフィールドがない場合kappa、それらのフィールドにはデフォルト値が割り当てられます。
   2. 構造体配列kfにフィールドがあるfx場合は、非線形状態遷移関数があることを意味し、入力された観測値とフィルター パラメーターに従って時間更新を実行する必要があります。
      1. まず、関数 (このコード ファイル内の関数も) を呼び出して、ukfUT状態遷移後の状態ベクトルと状態共分散行列を計算します。
      2. 次に、状態共分散行列の補正項がフィルター パラメーターに従って計算され、状態共分散行列に追加されます。
   3. 構造体配列kfにフィールドがない場合はfx、状態遷移が線形であることを意味し、予測された状態ベクトルと共分散行列が直接使用されます。
4. 測定値の更新:
   1. 測定更新が行われる場合、まず測定更新のみであるかどうかが判断されます。
   2. 測定値のみが更新される場合は、予測された状態ベクトルと共分散行列を現在の状態ベクトルと共分散行列にそれぞれ割り当てます。
   3. 時間と測定の更新が同時に行われる場合、測定の更新はフィルター パラメーターに従って実行されます。
      1. 構造体配列kfにフィールドがあるhx場合は、非線形測定伝達関数が存在することを意味し、入力された観測値とフィルター パラメーターに従って測定値の更新を実行する必要があります。
         1. まず、ukfUT関数 を呼び出して、測定シフト後の測定ベクトル、測定共分散行列、および相互共分散行列を計算します。
         2. 次に、フィルタパラメータに基づいて測定共分散行列の補正項が計算され、測定共分散行列に追加されます。
      2. 構造体配列kfにフィールドがない場合はhx、測定値の伝達が線形であり、予測された測定値ベクトルと共分散行列が直接使用されることを意味します。
5. 戻り結果: 更新されたカルマン フィルター構造配列がkf出力として返されます。

全体として、このコードは、非線形システムのカルマン フィルターの時間更新および測定値更新プロセスを実装します。入力された観測値とフィルタパラメータに応じてフィルタの状態ベクトルと共分散行列を更新し、フィルタの状態推定機能を実現します。

英国時間

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/2012
    n = length(x);
    lambda = alpha^2*(n+kappa) - n;
    gamma = sqrt(n+lambda);
    Wm = [lambda/gamma^2; repmat(1/(2*gamma^2),2*n,1)];  
    Wc = [Wm(1)+(1-alpha^2+beta); Wm(2:end)];
    sPxx = gamma*chol(Pxx)';    % Choleskey decomposition
    xn = repmat(x,1,n); 
    X = [x, xn+sPxx, xn-sPxx];
    Y(:,1) = feval(hfx, X(:,1), tpara); m=length(Y); y = Wm(1)*Y(:,1);
    Y = repmat(Y,1,2*n+1);
    for k=2:1:2*n+1     % Sigma points nolinear propagation
        Y(:,k) = feval(hfx, X(:,k), tpara);
        y = y + Wm(k)*Y(:,k);
    end
    Pyy = zeros(m); Pxy = zeros(n,m);
    for k=1:1:2*n+1
        yerr = Y(:,k)-y;
        Pyy = Pyy + Wc(k)*(yerr*yerr');  % variance
        xerr = X(:,k)-x;
        Pxy = Pxy + Wc(k)*xerr*yerr';  % covariance
    end

このコードは、Unscented Transformation (UT) の機能を実装します。UT は非線形システムの状態推定手法であり、非線形方程式の一連のサンプリングと伝播を通じて状態変数の推定結果を取得します。

この関数の入力パラメータには次のものがあります。

• x：状態ベクトル（状態ベクトル）。
• Pxx: 状態ベクトルの分散行列。
• hfx: 非線形状態方程式を処理するためのハンドル。
• tpara: に渡すためのいくつかの時変パラメータhfx。
• alpha、beta、kappa: UT 変換のパラメータ。

この関数の出力パラメータには次のものが含まれます。

• y: UT 変換後の状態ベクトル。
• Pyy: UT 変換後の状態ベクトルの分散行列。
• Pxy: 状態ベクトルと UT 変換後の状態ベクトルの間の共分散行列。
• X、Y: UT 変換の前後のシグマ ポイント ベクトル。

コードの主な手順は次のとおりです。

1. n(状態ベクトルの長さ)、lambda、gamma、Wmおよび を含む、入力パラメーターに従っていくつかの中間変数を計算しますWc。
2. コレスキー分解を実行して、入力状態分散行列をPxx下三角行列に変換しますsPxx。
3. X状態ベクトルそのもの、状態ベクトルに状態分散行列のコレスキー分解行列を加えたもの、および引いたものである 3 つの部分の点を含むシグマ点ベクトル を構築します。
4. 各シグマ点に対して非線形伝播を実行し、ハンドル関数を呼び出してhfx各点の出力値を計算し、結果を行列に格納しますY。
5. UT 変換の公式に従って、推定状態ベクトルyと分散行列Pyy、および状態ベクトルと推定状態ベクトルの間の共分散行列を計算しますPxy。
6. UT 変換前後の推定状態ベクトル、分散行列、共分散行列、シグマ点ベクトルを返します。

このコードでは、ハンドル関数 hfx を呼び出すために MATLAB 関数 feval が使用されており、ループ内で複数の呼び出しが行われることに注意してください。したがって、このコードを使用するときは、ハンドル関数 hfx の正しい実装と呼び出し可能性を保証する必要があります。

アウトポイント

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/2022
    if nargin<4, kappa=0; end
    if nargin<3, beta=2; end
    if nargin<2, afa=1e-3; end
    lambda = afa^2*(n+kappa)-n;  gamma = sqrt(n+lambda);
    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^2+beta);

このコードは、3 次または 5 次の立方体点の重みを計算する MATLAB 関数です。コードの機能の説明は次のとおりです。

1. 関数の入力パラメータは次のとおりです。
   • n: 次元 (整数)
   • afa、beta、kappa: パラメーター (スカラー、ベクトル、または行列にすることができます)
2. 関数の出力パラメータは次のとおりです。
   • U: 立方体点のベクトル ((3*n, 1)サイズの列ベクトル)
   • wm: ( size の列ベクトル)Uの各点に対応する重み(3*n, 1)
   • wc: ( size の列ベクトル)Uの各点に対応する重み(3*n, 1)
3. コードロジックの説明:
   • まず、入力パラメータの数を確認し、4 未満の場合はkappa0、3 未満の場合はbeta2、2 未満の場合はafa1e-3 に設定します。
   • lambda次に、パラメータに基づいて変数と変数の値が計算されますgamma。
   • 次に、gammaおよび 他のパラメータに従って立方体点の行列を計算しますU。
   • wm最後に、パラメータと計算結果に基づいて重みベクトルの合計が計算されますwc。このうち、 のwm最初の 2 つの要素は同じであり、(2*n, 1)サイズの行列の各要素は であり1/(2*gamma^2)、3 番目の要素は ですlambda/gamma^2。wcの最初の 2 つの要素は とwm同じで、最後の要素は(3*n, 1)値の列ベクトルです(1-afa^2+beta)。

这个函数的作用是通过输入的参数计算 cubature 点的位置和权重。 cubature 点是一种用于数值积分的方法，通过在低维度空间中计算一些点并将它们的权重相加以估计高维度空间的积分值。

五、ssukf

SSUKF是一种非线性滤波算法，全称为Unscented Particle Filtering with Square-Root Unscented Transform。它结合了Unscented变换和粒子滤波的优点，用于处理非线性非高斯系统的状态估计问题。SSUKF的主要步骤包括：

1. 选择一组Unscented变换的参数，计算Unscented变换中的权重和均值。
2. 对系统模型和观测模型进行一阶Taylor展开，得到非线性的残差项。
3. 使用粒子滤波的方法，通过对残差项进行加权和估计，得到新的状态估计值。
4. 重复执行步骤2和3，进行迭代优化，直到满足收敛条件为止。同时，对于大规模或实时性要求高的应用场景，还需要考虑算法的效率和可扩展性。

UKF 和 SSUKF 区别：

1. Unscented变换的方式不同：UKF使用标准的Unscented变换，而SSUKF使用Square-Root Unscented变换，这种变换可以更快地计算权重和均值，提高了算法的计算效率。
2. 粒子滤波的方式不同：在UKF中，使用标准粒子滤波来估计状态变量，而在SSUKF中，使用一种称为序贯 Importance Sampling的粒子滤波方法来估计状态变量。这种滤波方法可以更好地处理非线性较强的系统，并且可以提高算法的数值稳定性。
3. 适用场景不同：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,
%            TimeMeasBoth='T' (or nargin==1) for time updating only, 
%            TimeMeasBoth='M' for measurement updating only, 
%            TimeMeasBoth='B' (or nargin==2) 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/2022
    if nargin==1;
        TimeMeasBoth = 'T';
    elseif nargin==2
        TimeMeasBoth = 'B';
    end

    if TimeMeasBoth=='T' || TimeMeasBoth=='B'
        if 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';
        else
            kf.xkk_1 = kf.Phikk_1*kf.xk;
            kf.Pxkk_1 = kf.Phikk_1*kf.Pxk*kf.Phikk_1' + kf.Gammak*kf.Qk*kf.Gammak';
        end
        if TimeMeasBoth=='T'    % time updating only
            kf.xk = kf.xkk_1; kf.Pxk = kf.Pxkk_1;
            return;
        end
    end
    
    if TimeMeasBoth=='M' || TimeMeasBoth=='B'
        if TimeMeasBoth=='M'    % meas updating only
            kf.xkk_1 = kf.xk; kf.Pxkk_1 = kf.Pxk;
        end
        if 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;
        else
            kf.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
        % filtering
        kf.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.Pxk+kf.Pxk')/2;
    end

このコードは、非線形システム用のアンセンテッド カルマン フィルターの関数です。これは、カルマン フィルター構造の初期配列kf、測定値のベクトルyk、および時間更新と測定値の更新を示すフラグを受け取りますTimeMeasBoth。

コードの主なロジックは次のとおりです。

1. 入力パラメータの数に応じて、TimeMeasBoth入力パラメータが 1 つだけの場合、決定された値TimeMeasBothは時間更新のみを意味する「T」に設定され、入力パラメータが 2 つの場合、TimeMeasBoth決定された値は「B」に設定されます。時間の更新と測定値の更新を同時に行います。
2. 「T」または「B」の場合TimeMeasBoth、時刻更新ステップを実行します。kfカルマン フィルター構造配列に非線形状態遷移関数がある場合はfx、状態予測に SSUT 関数を使用し、状態と状態共分散行列を更新します。時刻のみが更新された場合は、更新された時刻が直接返されますkf。
3. 「M」または「B」の場合TimeMeasBoth、測定更新ステップを実行します。測定値のみが更新される場合は、状態および状態共分散行列を以前の値に設定します。カルマン フィルター構造の配列kfに非線形測定伝達関数がある場合はhx、測定予測に SSUT 関数を使用し、測定値と測定共分散行列を更新します。
4. フィルタリング手順を実行します。カルマン ゲインを計算し、測定および予測された状態行列と共分散行列を使用して状態推定値と状態共分散行列を更新します。

最後に、関数は更新されたカルマン フィルター構造の配列を返しますkf。

このコードは他の関数に依存しており、他の関数を定義または提供する必要がある場合があることに注意することが重要ですSSUT。さらに、コードは、カルマン フィルター構造の入力配列で提供される必要がある、Phikk_1、Gammak、Qkなどのいくつかの変数の定義にも依存します。Hkkf

この関数を使用する場合は、正しい入力パラメーターと正しい関数の依存関係を指定していることを確認してください。

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_w
    if isempty(SSUT_w), SSUT_w(1)=0; end
    if nargin<5, w0=0; end
    n = length(x);
    if n~=size(SSUT_s,1) || SSUT_w(1)~=w0
        w1 = (1-w0)/(n+1);
        s = [0, -1/sqrt(2), 1/sqrt(2)];
        for j=2:n
            s1 = [s(:,1); 0];
            for i=1:j
                s1(:,i+1) = [s(:,i+1); -1/sqrt(j*(j+1))];
            end
            s1(:,j+2) = [zeros(j-1,1);j/sqrt(j*(j+1))];
            s = s1;
        end
        SSUT_s = s/sqrt(w1);  SSUT_w = [w0, repmat(w1,1,n+1)];
    end
    %%
    X = repmat(x,1,n+2) + chol(Pxx)'*SSUT_s;
    y = SSUT_w(1)*feval(hfx, X(:,1), tpara);  m = length(y);
    Y = repmat(y,1,2*n);
    for k=2:1:n+2
        Y(:,k) = feval(hfx, X(:,k), tpara);
        y = y + SSUT_w(k)*Y(:,k);
    end
    Pyy = zeros(m); Pxy = zeros(n,m);
    for k=1:1:n+2
        yerr = Y(:,k)-y;
        Pyy = Pyy + SSUT_w(k)*(yerr*yerr');  % variance
        xerr = X(:,k)-x;
        Pxy = Pxy + SSUT_w(k)*xerr*yerr';  % covariance
    end

これは、Spherical Simplex Unscented Transformation (SSUT) を実装する MATLAB 関数です。SSUT は、非線形システムの状態推定と予測のための非線形フィルタリング手法です。

関数の入力パラメータには次のものがあります。

• x: 状態ベクトル (列ベクトル)。
• Pxx: 状態ベクトルの分散行列。
• hfx: 非線形状態方程式を記述する関数へのハンドル。
• tparahfx:関数に渡すための時変引数ベクトル。
• w0: 重みパラメータ。

関数の出力パラメータには次のものがあります。

• y: UT 変換後の状態ベクトル (列ベクトル)。
• Pyy: UT 変換後の状態ベクトルの分散行列。
• Pxy: 状態ベクトルと UT 変換後の状態ベクトルの間の共分散行列。
• XおよびY: UT 変換の前後に生成されたシグマ ポイント ベクトル。

この関数はまず、一部のグローバル変数がすでに初期化されているかどうかSSUT_s、およびSSUT_wデフォルト値に従って初期化されていない場合をチェックします。次に、入力パラメータと に従ってx対応Pxxするシグマ点を生成します。次に、非線形状態方程式を呼び出してhfxこれらのシグマ点に対応する出力値を計算し、これらの出力値を に保存しますY。次に、球面単純カルマン フィルター アルゴリズム に従って、UT 変換された状態ベクトルyとその分散Pyy行列、および状態ベクトルと UT 変換された状態ベクトルの間の共分散行列を計算しますPxy。

最後に、この関数は、UT 変換された状態ベクトルyとその分散行列Pyy、状態ベクトルと UT 変換された状態ベクトルの間の共分散行列、およびUT 変換のPxy前後に生成されたシグマ点ベクトルの合計を返します。XY

この機能は、非線形システムの状態推定と予測に使用できる SSUT を実現したものです。

6. CKF: 体積カルマンフィルター

Cube-based Radial Basis Function (CKF) は、非線形非ガウス システムの状態推定のためのフィルタリング アルゴリズムです。

CKF フィルタリングの中心となる考え方は、体積基準を使用して状態の事後平均と共分散を近似し、非線形ガウス状態の事後平均と分散を理論的に 3 次多項式で近似できるようにすることです。 。

従来のカルマン フィルタリングと比較して、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,
%            TimeMeasBoth='T' (or nargin==1) for time updating only, 
%            TimeMeasBoth='M' for measurement updating only, 
%            TimeMeasBoth='B' (or nargin==2) 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/2022
    if nargin==1;
        TimeMeasBoth = 'T';
    elseif nargin==2
        TimeMeasBoth = 'B';
    end

    if TimeMeasBoth=='T' || TimeMeasBoth=='B'
        if 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';
        else
            kf.xkk_1 = kf.Phikk_1*kf.xk;
            kf.Pxkk_1 = kf.Phikk_1*kf.Pxk*kf.Phikk_1' + kf.Gammak*kf.Qk*kf.Gammak';
        end
        if TimeMeasBoth=='T'    % time updating only
            kf.xk = kf.xkk_1; kf.Pxk = kf.Pxkk_1;
            return;
        end
    end
    
    if TimeMeasBoth=='M' || TimeMeasBoth=='B'
        if TimeMeasBoth=='M'    % meas updating only
            kf.xkk_1 = kf.xk; kf.Pxkk_1 = kf.Pxk;
        end
        if 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;
        else
            kf.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
        % filtering
        kf.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.Pxk+kf.Pxk')/2;
    end

このコードは、Cubature Kalman Filter (CKF) を実装する関数です。CKF は、動的システムの状態を推定するための非線形フィルタリング手法です。

この関数の役割はckf、入力フィルター構造配列kf、測定ベクトルyk、および時間/測定更新フラグに従ってTimeMeasBothフィルターを時間更新または測定更新することです。

コードの主なロジックは次のとおりです。

1. 入力されたパラメータに基づいて時間/測定更新フラグを決定しますTimeMeasBoth。入力パラメータが 1 つだけの場合、デフォルトの時間更新フラグは「T」です。入力パラメータが 2 つある場合、更新フラグは 2 番目のパラメータに従って決定されます。
2. 時間更新フラグが「T」または「B」（時間更新、または時間更新と測定更新の両方を示す）の場合は、次の手順を実行します。
   1. kf非線形状態伝播関数がフィルター構造配列 に存在する場合はfx、ckfCTその関数を状態予測と共分散予測に使用します。
   2. 時間更新フラグが「T」の場合、フィルターの状態と共分散を更新し、結果を返します。
3. 測定更新フラグが「M」または「B」の場合（測定更新、または時間更新と測定更新の両方が必要であることを示します）、次の手順を実行します。
   1. 測定更新フラグが「M」の場合、フィルターの状態と共分散を現在の状態と共分散に設定します。
   2. kf非線形測定拡散関数がフィルター構造配列に存在する場合はhx、ckfCTその関数を測定予測と共分散予測に使用します。
   3. フィルタリングが実行され、カルマン ゲイン、状態推定値、共分散推定値が計算されます。

最後に、関数はフィルター構造の更新された配列を返します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/2022
    n = length(x);
    sPxx = sqrt(n)*chol(Pxx)';    % Choleskey decomposition
    xn = repmat(x,1,n); 
    X = [xn+sPxx, 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 k=2:1:2*n
        Y(:,k) = feval(hfx, X(:,k), tpara);
        y = y + Y(:,k);
        Pyy = Pyy + Y(:,k)*Y(:,k)';
        Pxy = Pxy + X(:,k)*Y(:,k)';
    end
    y = 1/(2*n)*y;  % y mean
    Pyy = 1/(2*n)*Pyy - y*y';  Pxy = 1/(2*n)*Pxy - x*y';
 
%     Y(:,1) = feval(hfx, X(:,1), tpara); m=length(Y); y = Y(:,1);
%     Y = repmat(Y,1,2*n);
%     Pyy = zeros(m); Pxy = zeros(n,m);
%     for k=2: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 k=1: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;

このコードは、非線形状態推定のための球面 - 放射状立方体変換 (CKF-CT) を実装しています。以下はコードの機能説明です。

1. コード入力:
   • x: 現在の状態のベクトル。
   • Pxx: 現在の状態ベクトルの分散行列。
   • hfx: 非線形状態方程式のハンドル。
   • tpara:hfxに渡される時間変化するパラメータ。
2. コード出力:
   • y: Unscented 変換後の状態ベクトル。
   • Pyy: Unscented 変換後の状態ベクトルの分散行列。
   • Pxy: Unscented 変換後の状態ベクトルと初期状態ベクトルの間の共分散行列。
   • X: Unscented 変換前のシグマ点ベクトル。
   • Y: Unscented 変換後のシグマ ポイント ベクトル。
3. コード：
   • まず、入力分散行列はコレスキー分解によりPxx上三角行列に変換されますsPxx。
   • 次に、現在の状態ベクトルxと上三角行列に従ってsPxx2 つのシグマ点、つまり行列 を生成しますX。
   • 次に、非線形状態方程式のハンドルを呼び出して、hfx初期シグマ点に対応する出力ベクトルを計算しますY。
   • 次に、ループを使用して各シグマ ポイントの出力を計算し、それらを合計出力ベクトルyと共分散行列に累積しますPyy。同時に、初期状態ベクトルと合計出力ベクトルの間の共分散行列が計算されますPxy。
   • 最後に、最終的な状態ベクトルと共分散行列が、合計出力ベクトルと共分散行列から計算されます。
4. 予防：
   • このコードでは、MATLAB の関数 feval を使用して、計算用の非線形状態方程式のハンドルを呼び出しますhfx。このコードを使用する前に、非線形状態方程式のハンドルhfxと関連パラメーターが定義され、適切に設定されていることを確認してくださいtpara。
   • コード内のコメントには、コードの機能と実装を理解するのに役立ついくつかの手順と参照が記載されています。

全体として、このコードは非線形状態推定のために球面放射状立方体変換 (CKF-CT) を実装し、アンセンテッド変換 (UT) を通じて状態を推定し、推定結果の分散と共分散を計算します。

7、PF：パーティクルフィルター

粒子フィルターは、ランダム プロセスの推定と予測に使用できるノンパラメトリック ベイジアン フィルター手法です。重み付けされた粒子 (サンプル) のグループを使用してランダム プロセスの状態を表現し、ランダム プロセスの推定と予測を実現します。

粒子フィルターの主なアイデアは、既知の事前確率分布と観測データを使用して粒子の重みと状態を繰り返し更新し、事後確率分布を取得することです。具体的には、粒子フィルタリングには次の手順が含まれます。

1. 初期化: 事前確率分布に従って、初期粒子のセットを生成し、対応する重みを割り当てます。
2. 予測: 既知の動的モデルに従って、各粒子を 1 タイム ステップだけ前方に進めて、次の瞬間の粒子セットを取得します。
3. リサンプリング: 各パーティクルの重みに従って、パーティクルがリサンプリングされるため、より大きな重みを持つパーティクルがサンプリングに現れる確率が高くなります。
4. 出力: リサンプリングされた粒子セットに従って、事後確率分布の推定値を取得できます。

粒子フィルタリングは、オブジェクト追跡、姿勢推定、ナビゲーション、音声認識、画像処理など、多くの分野に応用できます。ただし、粒子フィルタリングには、粒子の劣化、ノイズ感度など、さらなる研究と改善が必要ないくつかの問題もあります。

function [pf, Pxk] = pfupdate(pf, Zk, TimeMeasBoth)
% Particle filter updating for additive normal distribution process&measure noise.
%
% Prototype: [pf, Pxk] = pfupdate(pf, Zk, TimeMeasBoth)
% Inputs: pf - particle filter structure array
%         Zk - measurement vector
%         TimeMeasBoth - described as follows,
%            TimeMeasBoth='T' (or nargin==1) for time updating only, 
%            TimeMeasBoth='M' for measurement updating only, 
%            TimeMeasBoth='B' (or nargin==2) 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/2022
    if nargin==1;
        TimeMeasBoth = 'T';
    elseif nargin==2
        TimeMeasBoth = 'B';
    end

    if TimeMeasBoth=='T' || TimeMeasBoth=='B'
        sQ = chol(pf.Qk+eps*eye(pf.n));
    end
	if TimeMeasBoth=='M' || TimeMeasBoth=='B'
        vRv = zeros(pf.Npts, 1);  invR = pf.Rk^-1;
    end
    for k=1:pf.Npts
        if TimeMeasBoth=='T' || TimeMeasBoth=='B'
            if isfield(pf,'fx')
                pf.particles(:,k) = feval(pf.fx, pf.particles(:,k), pf.tpara) + sQ*randn(pf.n,1);
            else
                pf.particles(:,k) = pf.Phikk_1*pf.particles(:,k) + sQ*randn(pf.n,1);
            end
        end
        if TimeMeasBoth=='M' || TimeMeasBoth=='B'
            if isfield(pf,'hx')
                v = Zk - feval(pf.hx, pf.particles(:,k), pf.tpara)';
            else
                v = Zk - pf.Hk*pf.particles(:,k);
            end
            vRv(k) = v'*invR*v;
        end
    end
    if TimeMeasBoth=='M' || TimeMeasBoth=='B'
%         [vRv, idx] = sort(vRv);  pf.particles = pf.particles(:,idx);
        weight = exp(-vRv/2);%/sqrt((2*pi)^pf.m*det(pf.Rk)); % Normal distribution
        cumw = cumsum(weight+0.01/pf.Npts);  cumw = cumw/cumw(end);
%         idx = interp1(cumw, (1:pf.Npts)', linspace(cumw(1),cumw(end),pf.Npts+10), 'nearest');  % resampling index
        idx = interp1(cumw, (1:pf.Npts)', cumw(1)+(cumw(end)-cumw(1))*rand(pf.Npts+10,1), 'nearest');  % resampling index
        pf.particles = pf.particles(:,idx(3:pf.Npts+2)); % myfig,hist(pf.particles(2,:));
    end
    pf.xk = mean(pf.particles,2);
    xk = pf.particles - repmat(pf.xk,1,pf.Npts);
	pf.Pxk = diag(sum(xk.^2,2)/pf.Npts);
%     for k=1:pf.n
%         pf.particles(k,:) = pf.xk(k)+sqrt(pf.Pxk(k,k))*randn(1,pf.Npts);
%     end
    if nargout==2  % if using full matrix Pxk
        pf.Pxk = zeros(pf.n);
        for k=1:pf.Npts
            pf.Pxk = pf.Pxk + xk(:,k)*xk(:,k)';
        end
        pf.Pxk = pf.Pxk/pf.Npts;
        Pxk = pf.Pxk;
    end

function 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/2022
    if size(P0,2)>1, P0 = diag(P0); end
    pf.n = length(x0);  pf.m = length(Rk);
    for k=1:pf.n
        pf.particles(k,:) = x0(k)+sqrt(P0(k))*randn(1,N);
    end
    pf.Npts = N;
	pf.Pxk = P0; pf.Qk = Qk; pf.Rk = Rk;

8、akfupdate: 適応フィルタリング

このコードは、適応カルマン フィルターの実装です。コードの機能の説明は次のとおりです。

1. この関数はakfupdate5 つの入力パラメータを受け入れます。
   • kf: カルマン フィルター構造に関する情報を含む配列。
   • yk: 測定ベクトル。
   • TimeMeasBoth: 時間の更新と測定の更新を説明するフラグ文字。次の 3 つの値を取ることができます。
     • 'T'：時刻更新のみを行います。
     • 'M': 測定値の更新のみを実行します。
     • 'B'：時刻更新と測定更新を同時に行います。
   • kftype: 適応カルマン フィルターの種類。
   • para: いくつかのパラメータ。
2. 入力パラメータの数を決定し、異なるフラグ文字に従って対応する時間更新または測定更新を実行します。
3. TimeMeasBoth「はい」の場合は'T'、時刻の更新を実行します。
   1. kf.Phikk_1を乗算してkf.xk、状態推定値を更新しますkf.xk。
   2. kf.Phikk_1を乗算しkf.Pxk、 を乗算しkf.Phikk_1'、加算してkf.Gammak * kf.Qk * kf.Gammak'状態推定誤差共分散行列を更新しますkf.Pxk。
4. TimeMeasBoth「はい」の場合'M'、測定値の更新を実行します。
   • 現在の状態の推定値と誤差共分散行列を と に保存しkf.xkk_1ますkf.Pxkk_1。
5. TimeMeasBoth「はい」の場合'B'、時間の更新と測定の更新を実行します。
   1. 現在の状態の推定値と誤差共分散行列を と に保存しkf.xkk_1ますkf.Pxkk_1。
   2. を計算しますkf.Pxykk_1。つまり、現在の推定値と測定値の間の共分散行列を計算します。
   3. を計算しますkf.Py0。つまり、測定ノイズに の転置を乗算した共分散行列を計算しますHk。
   4. kf.rk実際の測定値から予測された測定値を差し引いた残差を計算します。
6. のタイプに応じて、kftypeさまざまな適応フィルタリング アルゴリズムが実装されます 。
   1. kftypeが文字列'KF'または の場合、 およびなどの変数は、'MSHAKF'指定された引数に基づいて評価されます。bs
   2. kftypeが数値の場合2、MCKF (最小曲率フィルター) アルゴリズムが実行され、1 つのパラメーターが使用されますsigma。
   3. kftypeが数値の場合3、RSTKF (残差二乗合計最小二乗フィルター) アルゴリズムが 1 つのパラメーターで実行されますv。
   4. kftypeが数値の場合4、SSMKF (平方根分割演算子行列カルマン フィルター) アルゴリズムを実行します。
7. 更新された状態推定値、状態推定誤差共分散行列、およびその他の関連情報を入力構造体配列 に格納しますkf。
8. 更新されたカルマン フィルター構造の配列を出力します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 b==0, 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/2022
    m = length(r2);  maxflag = zeros(m,1);
    beta = beta./(beta+b);
    for k=1:m
        if 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);  end
        else
            if r2(k)<Rmin(k), r2(k)=Rmin(k);
            elseif r2(k)>Rmax(k), r2(k)=Rmax(k); beta(k)=1; maxflag(k)=1; end
            Rk(k,k) = (1-beta(k))*Rk(k,k) + beta(k)*r2(k);
        end
    end

このコードは、Sage-Husa 適応フィルターの実装です。コードの機能の説明は次のとおりです。

1. 入力パラメータ:
   • Rk: 測定ノイズ分散、列ベクトル。
   • r2:Rk平均の 2 乗から初期残差の 2 乗を引いたもの。これも列ベクトルです。
   • RminとRmax: は、それぞれ測定ノイズ分散の下限と上限、つまり 2 つの列ベクトルです。
   • betaおよびb: それぞれ忘却係数と獲得係数を表す 2 つの列ベクトル。
   • s: b0 に等しい場合に使用する増幅率 (列ベクトル)。
2. 出力パラメータ:
   • Rk: 測定ノイズ分散、列ベクトルを更新しました。
   • beta: 更新された忘却係数、列ベクトル。
   • maxflag: r2(k) が Rmax(k) より大きいときの最大符号 (列ベクトル)。
3. コードロジック:
   • まず、入力パラメーターの長さに応じて、長さ m の maxflag ベクトルを初期化し、すべての要素を 0 に設定します。
   • 次に、ベータを (ベータ + b) で除算して、新しいベータ値を取得します。これは忘却係数の計算式に基づいた更新です。
   • k の各値について、次の操作を実行します。
     • b(k) が 0 に等しい場合、r2(k) がゲイン係数 s(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) を更新します。
4. 最後に、更新された Rk、beta、maxflag が結果として出力されます。

全体として、このコードは Sage-Husa 適応フィルターのコア機能を実装し、入力パラメーターと現在の状態に従って測定ノイズ分散の値を適応的に調整し、上限を超えているかどうかを監視します。

9. ロバストな等価重量関数

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/2022
    if nargin==1, k0=1.0; k1=3.0; end  % w = igg1(err)
    if nargin==2, k1=k0(2); k0=k0(1); end  % w = igg1(err, k0k1)
    if k1<2*k0, k1=2*k0; end
    err = abs(err);  w = err;
    for k=1:length(err)
        if err(k)<=k0,    w(k)=1;
        elseif err(k)>k1, w(k)=0;
        else,             w(k)=k0/err(k);  end
    end

このコードは、測地学地球物理研究所による区分関数アプローチを使用して重みを計算する関数です。この関数は、正規化された誤差の測定値 (err) と 2 つのセグメンテーション ポイント (k0 と k1) の 2 つの入力引数を受け入れます。出力は重量値 (w) です。

コードは最初に入力パラメータの数をチェックします。入力パラメーター (err) が 1 つだけの場合は、k0 と k1 をそれぞれ 1.0 と 3.0 に設定します。入力引数が 2 つある場合 (err と k0 または k1)、k1 を k0 の 2 番目の要素に設定し、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/2022
    if nargin==1, k0=2.0; k1=6.0; end  % w = igg3(err)
    if nargin==2, k1=k0(2); k0=k0(1); end  % w = igg3(err, k0k1)
    if k1<2*k0, k1=2*k0; end
    err = abs(err);  w = err;
    for k=1: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;  end
    end

このコードは、 という名前の MATLAB 関数ですigg3。これは、特定の測定誤差と 2 つのパラメーターに基づいて重みを計算するために使用されますk0。k1

関数の入力パラメータは次のとおりです。

• err: 正規化された測定誤差の値。
• k0: 分割点の 1 つ。
• k1: セグメンテーションの 2 番目のポイント。

関数の出力は次のとおりです。

• w: 計算された重量。

この関数は、「Institute of Geodesy & Geophysics」の picewise メソッドを使用して重みを計算します。この方法では、測定誤差の大きさに応じて、2 つの分割点間で異なる計算方法を使用して重量値を取得します。

この関数は最初に入力パラメータの数をチェックします。入力パラメータが 1 つだけの場合err、デフォルトk0は 2.0、k1つまり 6.0 です。さまざまな誤差範囲に応じて、重量値の計算にさまざまな式が使用されます。

• 誤差が 以内の場合k0、重みは 1 です。
• 誤差が より大きい場合k1、重みは 0 になります。
• それ以外の場合、重量値は次の式を使用して計算されます。w(k) = k0/err(k) * ((k1-err(k))/(k1-k0))^2

最後に、計算された重みベクトルを関数の出力として返します。

この関数は主に標高フィッティングにおけるロバスト推定に使用され、測定誤差の大きさに応じて重みを調整してフィッティング結果への誤差の影響を軽減します。

10. データ融合

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/2014
    if nargin<5
        ratio = 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,n2+1:end-1);
    r2 = rr(i2,1:end-1); x2 = xpr(i2,1:n2); p2 = xpr(i2,n2+1:end-1);
    x1(:,1:9) = x1(:,1:9)*ratio; x2(:,1:9) = x2(:,1:9)*ratio;  % ratio
    %  common fusion
    r10 = [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 )./(p1+p2);  
%     p = p1.*p2./(p1+p2);
    [r, p] = fusion(r10, p1, r20, p2);
    % attitude fusion
    for k=1: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)'))';
    end
    r(:,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(位置・姿勢)データ融合用の関数です。5 つの入力パラメータを受け入れます: rf (順方向 AVP (擬似距離))、xpf (順方向状態推定と共分散)、rr (逆方向 AVP)、xpr (逆方向状態推定と共分散)、および比率 (状態の比率の変更に使用)見積もり）。この関数の出力は、次のフィールドを持つ構造体 psf です。

• rf および pf: 融合後の AVP と共分散
• r1 および p1: 順方向 AVP と共分散
• r2 と p2: 逆 AVP と共分散

コードの機能は次のとおりです。

1. 入力パラメータの数を確認し、5 未満の場合は比率を 1 に設定します。
2. 最後の次元で rf と rr の交点を見つけて、共通の瞬間 t、インデックス i1 と i2 を決定します。
3. r1、x1、p1、r2、x2、p2 を抽出します。これらは、順方向 AVP、逆方向 AVP、状態推定値、共分散にそれぞれ対応します。
4. 比率の値に従って、x1 と x2 の最初の 9 フィールドがスケーリングされます。
5. 融合演算を実行して、融合された AVP と共分散 r および p を生成します。ここではフュージョンと呼ばれる機能を使用しており、特定のフュージョン方法はこの機能内で実装される場合があります。
6. rf と pf の NaN 値を 0 に置き換えます。
7. 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 Vel&Pos input array = [vn,pos,t]
%         gnss - GNSS Vel&Pos input array = [vn,pos,t]
%         nSen - smooth span in second
%         isfig - figure flag
% Output: vpOut - INS Vel&Pos 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/2021
    if nargin<4, isfig=1; end
    if nargin<3, nSec=10; end
	err = 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 points
    for k=1:size(gnss,2)-1
        err(:,end-k) = gnss(:,end-k) - interp1(vp(:,end), vp(:,end-k), gnss(:,end));
        [~, errs(:,end-k)] = smoothol(err(:,end-k), nSec, 2, 0);
    end
    idx = ~isnan(interp1(errs(:,end), errs(:,end-k), vp(:,end)));
    vpOut = vp;
    for k=1:size(gnss,2)-1
        vpOut(idx,end-k) = vp(idx,end-k) + interp1(errs(:,end), errs(:,end-k), vp(idx,end), 'spline');
    end
    if isfig==1
        eth = earth(gnss(1,end-3:end-1)');
        myfig
        if size(gnss,2)==7, subplot(211); end
        plot(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)==7
            subplot(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');
        else
            avpcmpplot(gnss, vpOut(:,end-3:end), 'p');
        end
    end

この機能は、INS-VelPos および GNSS-VelPos データを介して位置を平滑化する機能です。その入力には、INS Vel&Pos データ配列 vp、GNSS Vel&Pos データ配列 gnss、平滑化タイム スパン nSec、およびグラフィック フラグ isfig が含まれます。その出力は、平滑化された INS Vel&Pos データ配列 vpOut です。

この関数の具体的な実装手順は次のとおりです。

1. まず、入力パラメータに応じて図形フラグ isfig と平滑化時間 nSec が指定されているかどうかを判定します。指定しない場合、デフォルトの isfig は 1、nSec は 10 です。
2. 次に、GNSS データと INS データ間の誤差 err を計算します。この誤差は、INS データの最後のポイントと GNSS データの最後のポイントの間を線形補間することによって計算されます。次に、gnss 配列の NaN 値を err 配列に置き換え、errs 配列を err 配列に初期化します。
3. 平滑化時間スパン nSec に基づいて、平滑化するポイントの数を計算します。これは、nSec を gnss 配列内のポイントの数に変換することによって行われます。
4. 平滑化される各点 k について、err(:,end-k) の値を計算します。これは、INS データの最後のポイントと GNSS データの前のポイントの間を線形補間することによって計算されます。次に、smoothhol 関数を使用して err(:, end-k) を平滑化し、errs(:, end-k) を取得します。
5. インジケーター インデックス idx は interp1 関数に従って計算され、どのポイントを平滑化する必要があるかを決定するために使用されます。
6. vpOut 配列を元の vp 配列として初期化します。
7. 平滑化される各点 k について、errs(:,end-k) と vp(idx,end) の値に従って、interp1 関数を使用して内挿し、結果を vpOut(idx,end-k) に追加します。 、平滑化された位置データを取得します。
8. isfig が 1 に等しい場合、平滑化された結果をプロットします。これには、誤差曲線と平滑化された位置データ曲線が含まれます。

この関数は、interp1、smoothhol、eth などの外部ライブラリの一部の関数を使用することに注意してください。これらの機能が正しく機能するには、システム上で適切なセットアップと構成が必要な場合があります。

POS処理

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*3+1+9+6=34)
%
% 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 ifbstr=0 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/2015
    if ~exist('fbstr','var'),  fbstr = 'avp';  end
    if ~exist('ifbstr','var'),  ifbstr = fbstr;  end
    len = length(imu); [lenGPS,col] = size(vpGPS);
    if col==4||col==7, vpGPS = [vpGPS(:,1:end-1),ones(lenGPS,1),vpGPS(:,end)]; end % add tag
    imugpssyn(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.n+1, 2*kf.n+1, 2*kf.m+1); ki = 1;
    Qk = zeros(length(vpGPS), kf.n+1);  Rk = zeros(length(vpGPS), 8);  kki = 1;  zk = zeros(size(kf.Hk,1),1);
    timebar(nn, len, 'SINS/GPS POS forward processing.');
    for k=1:nn:(len-nn+1)
        k1 = k+nn-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 kgps>0 
            if vpGPS(kgps,end-1)>=1 % tag OK
                kf.Hk = kfhk(ins);
                if size(kf.Hk,1)==6
                    zk = [ins.vnL-ins.an*dt; ins.posL-ins.Mpvvn*dt]-vpGPS(kgps,1:6)';
                else
                    zk = ins.posL-ins.Mpvvn*dt-vpGPS(kgps,1:3)';
                end
            	kf = kfupdate(kf, zk, 'M');
            end
            Qk(kki,:) = [diag(kf.Qk); t]';
            Rk(kki,:) = [diag(kf.Rk); kf.beta; t]';  kki = kki+1;
        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 = ki+1;
        timebar;
    end
    avp(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.n+1, 2*kf.n+1, 2*kf.m+1); ki = 1;
    timebar(nn, len, 'SINS/GPS POS reverse processing.');
    for k=k1:-nn:(1+nn)
        ik1 = k-nn+1; 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 kgps>0 
            if vpGPS(kgps,end-1)>=1 % tag OK
                ikf.Hk = kfhk(iins);
                if size(ikf.Hk,1)==6
                    zk = [iins.vnL-iins.an*dt; iins.posL-iins.Mpvvn*dt]-[-vpGPS(kgps,1:3),vpGPS(kgps,4:6)]';
                else
                    zk = iins.posL-iins.Mpvvn*dt-vpGPS(kgps,1:3)';
                end
            	ikf = kfupdate(ikf, zk, 'M');
            end
        end
        [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 = ki+1;
        timebar;
    end
    iavp(ki:end,:)=[]; ixkpk(ki:end,:)=[]; izkrk(ki:end,:)=[];  
    iavp = flipud(iavp); ixkpk = flipud(ixkpk); izkrk = flipud(izkrk); % reverse inverse sequence
    iavp(:,idx) = -iavp(:,idx);  ixkpk(:,idx) = -ixkpk(:,idx);
    ps.iavp = iavp; ps.ixkpk = ixkpk; ps.izkrk = izkrk;
    if nargout==2
        psf = POSFusion(ps.avp, ps.xkpk, ps.iavp, ps.ixkpk);
    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/2014
    iins = ins;
    iins.eth.wie = -iins.eth.wie;
    iins.vn = -iins.vn; iins.eb = -iins.eb; iins.tDelay = -iins.tDelay;
    ikf = kf;
    Pd=diag(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 glv
    ts = 1; nts = 1; nn = 1; T = 5000; len = T/ts;
    psinstypedef(186);
    %% forward navigation
    ts = 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 k=1:nn:(len-nn+1)
        k1 = k+nn-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 = ki+1;
        timebar;
    end
    err(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 k=k1:-nn:(1+nn)
        ik1 = k-nn+1; 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 = ki+1;
        timebar;
    end
    ierr(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/2014
    iins = 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; n=1000; 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/2022
    if length(wp)>1  % [wn, s] = htwn([wp1;wp2;...], [s1;s2;...], [len1;len2;...]);
        s1 = s;
        wn = []; s = [];
        for k=1:length(wp)
            [wnk,sk] = htwn(wp(k), s1(k), len(k));
            wn = [wn; wnk];  s = [s; sk];
        end
        return;
    end
    if nargin<3, len=1; end
    if nargin<2, s=100; end
    if nargin<1, wp=0.01; end
    if wp>1, len=wp; wp=0.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;

このコードはヘビーテールホワイトノイズを生成する関数です。コードの機能の説明は次のとおりです。

1. 関数の入力パラメータには次のものがあります。
   • wp: 生成されたノイズを増幅するかどうかを決定するために使用される確率を表すベクトルまたはスカラー。
   • s: 生成されたノイズを増幅する増幅量を表すベクトルまたはスカラー。
   • len: ノイズを生成する要素の数を表すベクトルまたはスカラー。
2. 関数の出力には次のものが含まれます。
   • wn: ヘビーテール分布からのランダム値を含む、生成されたノイズのベクトルまたは行列。
   • s：ノイズ成分ごとの増幅量。
3. コード内の主なロジックは次のとおりです。
   • まず、入力パラメータの長さを判断することで、複数のノイズ シーケンスの同時生成をサポートします。wporsの長さが 1 より大きい場合len、ループに入り、ノイズ シーケンスを 1 つずつ生成し、結果を返します。
   • 次に、入力引数に対して十分な引数が指定されているかどうかを確認します。引数が指定されない場合はlenデフォルトの 1、s引数が指定されない場合はデフォルトの 100、wp引数が指定されない場合はデフォルトの 0.01 になります。
   • 次に、標準正規分布からのランダムな値を含む、長さ のlenランダム ノイズ ベクトルを生成します。wn
   • 次に、wnと同じ長さでr01、値が 0 から 1 までのランダム ベクトルを生成します。
   • 生成されたランダム ベクトルr01としきい値を使用してwp、どのノイズ要素を増幅する必要があるかを決定します。満たす確率が倍率未満のwp要素を倍率倍しs、対応する倍率を更新しますs。
4. 最後に、結果のノイズ ベクトルと増幅量を関数の出力として返します。

全体として、このコードは、ランダム生成と増幅を使用して、ヘビーテール分布特性を持つノイズの多いデータを生成します。

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 test
    gamma = tpara.gamma;  g = tpara.g;  Ts = tpara.Ts;
    x1 = X0(1); x2 = X0(2); x3 = X0(3);
    %% X0->X1
    egx1 = exp(-gamma*x1);
    X1 = [ x1 - x2*Ts;   % Euler1 discretization
           x2 + (g-egx1*x2^2*x3)*Ts;
           x3 ];
%     k1 = [-X0(2);  g-exp(-gamma*X0(1))*X0(2)^2*X0(3);    0];  X01 = X0+k1*Ts/2;  % RK4
%     k2 = [-X01(2); g-exp(-gamma*X01(1))*X01(2)^2*X01(3); 0];  X02 = X0+k2*Ts/2;
%     k3 = [-X02(2); g-exp(-gamma*X02(1))*X02(2)^2*X02(3); 0];  X03 = X0+k3*Ts;
%     k4 = [-X03(2); g-exp(-gamma*X03(1))*X03(2)^2*X03(3); 0];
%     X1 = X0 + Ts/6*(k1+2*(k2+k3)+k4);
    %% Jacobian Phik
    if nargout>1
        Phik = [ 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 D
    if nargout>2
        D{
    
    1} = zeros(3);
        D{
    
    2} = [ [-gamma*x2*x3, 2*x3,       x2]*gamma*egx1*x2;
                  0,            -2*egx1*x3, -2*egx1*x2
                  0,            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); % symmetric
        D{
    
    3} = zeros(3);
    end

このコードは垂直落下する物体の状態方程式をシミュレーションする関数です。初期状態ベクトル X0 とパラメーターを含むベクトル tparana を入力として受け取り、予測された状態ベクトル X1、ヤコビアン Phik、およびヘッセ行列 D を返します。

コードの機能の説明は次のとおりです。

1. パラメータを定義します。
   • gamma：OK
   • g: 重力加速度
   • Ts: 離散時間ステップ
2. 変数の初期化：X0からx1、x2、x3の値を抽出し、それぞれの変数に格納します。
3. 状態ベクトル X1 の予測: オイラー離散化法を使用して状態方程式を予測し、次の瞬間の状態ベクトル X1 を取得します。
4. ヤコビアン Phik を計算します。関数に複数の出力パラメーターがある場合、ヤコビアン Phik を計算します。Phik は状態方程式を線形化するために使用され、状態方程式の導関数を数値的に近似します。
5. ヘッセ行列 D を計算する: 関数に 3 つ以上の出力パラメーターがある場合は、ヘッセ行列 D を計算します。D は、状態方程式を線形化するために使用されるヤコビ行列の導関数です。つまり、D は状態方程式の 2 次導関数です。

このコードでは、オイラー離散化法を使用して状態方程式を予測し、状態方程式のヤコビアン行列とヘッセ行列を計算します。これらの行列は、状態推定やパラメータ識別などのタスクにとって重要です。

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 nargin==3, Z=Z+V*randn(1); end
    %     return;  % for linear test
    M = tpara.M; H = tpara.H;
    %% Z
    Z = sqrt(M^2+(X(1)-H)^2);
    %% Jacobian Hk
    if nargout>1,  Hk = [(X(1)-H)/Z, 0, 0];  end
    %% Hessian D
    if nargout>2,  D{
    
    1} = [M^2/Z^3, 0, 0; 0, 0, 0; 0, 0, 0];  end
    %% Measurement noise
    if nargin==3, Z=Z+rk*randn(1); end

このコードは、垂直に落下する物体の測定方程式を計算する MATLAB 関数です。コードの機能の説明は次のとおりです。

1. 関数名: vfbhx。3 つの入力パラメータ、Xおよびtparaを受け入れますrk。
2. Xは、高度、速度、軌道パラメータの 3 つの要素を含む列ベクトルです。
3. tparaは 2 要素ベクトルで、最初の要素は質量パラメータM、2 番目の要素は基準高さですH。
4. rk実際の測定のノイズをシミュレートするために使用される測定ノイズの倍数です。
5. コードの先頭のコメントは、参考文献や著作権表示などの参考情報を提供します。
6. コードの最初のコメント行はテスト対象の線形モデルで、関数に 4 つの入力引数 (つまり ) がある場合、nargin==3結果を計算する前に乗算されZたVランダム ノイズが追加されます。
7. Zの計算は、高度と弾道パラメータを使用し、距離の影響を考慮して、測定方程式に従って実行されます。
8. 関数がヤコビ行列Hk( nargout==2) を出力する必要がある場合、ヤコビ行列の値は測定方程式に従って計算されます。
9. 関数がヘッセ行列D( nargout==3) を出力する必要がある場合、ヘッセ行列の要素値は測定式に従って計算されます。
10. 最後に、入力引数に 3 番目の要素が含まれている場合は、rk結果を計算する前に乗算さZれたランダム ノイズがそれに追加されrk、測定時のノイズがシミュレートされます。
11. 関数の最後の行はオプションであり、コマンド ラインに関数の出力を表示するために使用されます。

要約すると、このコードは垂直落下物体の測定方程式を計算する関数を実装します。入力された高度、速度、弾道パラメータ、測定ノイズに基づいて測定結果を計算し、ヤコビ行列とヘッセ行列（必要な場合）を出力できます。