PSINS Toolbox Learning (3) Let AI explain various Kalman filter codes in PSINS: RLS, UDRLS, KF, UDKF, EKF, CKF, UKF, SSUKF, PF, AKF, IGG1, IGG3

The Kalman filtering code in PSINS is within a hundred lines, does not call any functions, and is very versatile. Let AI explain it, and the effect is very good.

1. RLS

RLS (Recursive Least Squares Estimation) is a commonly used adaptive filtering algorithm for estimating parameters of linear systems1.

The basic idea of ​​RLS estimation is to gradually adjust the gain coefficient of the estimator by iteratively minimizing the sum of squared errors on the input signal, thereby continuously optimizing the performance of the estimator. Specifically, the steps of RLS estimation include 1:

1. Initialize the gain coefficient and error variance of the estimator.
2. For each input signal sample, compute the error between the predicted and actual value of the output signal.
3. Based on the error and the current gain factor, a new error variance and gain factor are calculated.
4. Repeat steps 2 and 3 for iterative optimization until the convergence condition is met.

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;

This is a function of a Recursive Least Square (RLS) filter. It is used to update the filter structure array kf and perform filtering operations according to the measurement vector zk.

The input parameters of the function are kf and zk, where kf is the filter structure array and zk is the measurement vector. The output parameter of the function is also kf, which represents the filter structure array after the filtering operation.

The function first judges whether the Rk field exists in the kf structure array, if not, then initializes Rk as the identity matrix. Then calculate Pxzk = Pxk * Hk', Pzk = Hk * Pxzk + Rk. Then calculate the gain matrix Kk = Pxzk * Pzk^-1. Then update the state vector xk = xk + Kk * (zk - Hk * xk). Then update Pxk = Pxk - Kk * Pxzk'. Finally, symmetrize Pxk to make it a symmetric matrix.

The purpose of this code is to implement the update process of the recursive least squares filter for estimating the system state.

RLSUD：OUT decompose 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

The code is an implementation of a recursive least squares filter using the UD decomposition technique. Here is an explanation of its function:

1. Input parameters:
   • U: left singular matrix in UD decomposition.
   • D: Right singular matrix in UD decomposition.
   • H: 1-row measurement matrix.
   • R: The variance of the measurement noise.
   • Z:Measurements.
   • X: Status value.
2. Output parameters:
   • U: The updated left singular matrix.
   • D: The updated right singular matrix.
   • X: The updated state value (only present in output parameters).
   • K: Gain (only present in output parameters).
3. Function explanation:
   1. First, the code checks to see if the input parameter is a structure. If it is a structure, it means that the way to call the function is to update the structure variable kf = RLSUD(kf, z);with the current state value kf.xkand measurement value . In this case, the function directly returns the updated structure variable without further calculations.zkfkf
   2. If the input parameter is not a structure, continue to execute the code below. fFirst, the intermediate variables , gand , are calculated from the input parameters afa. Among them, f = (H*U)'it means the transpose of the matrix product of Hand , means the element - wise multiplication of and , means the element-wise multiplication of and , and adds the result .Ug = D.*fDfafa = f'*g+Rf'gR
   3. Next, loop through the columns of the matrix one by one, iterating in reverse order starting with the last column. During the processing of each column, first calculate the new gain coefficient lambda, calculate the new diagonal elements according to the recursive formula D(j), and update Uthe corresponding elements of the upper triangular matrix . The specific calculation process is as follows:
      1. First, a temporary variable is computed afa0 = afa - f(j)*g(j)that will afasubtract the product term with the current column.
      2. Then, calculate the gain factor lambda = -f(j)/afa0, where the negative sign indicates the use of least squares.
      3. Next, new diagonal elements are calculated according to the recursive formula D(j) = afa0/afa*D(j).
      4. Finally, using the gain factor lambdaand intermediate variables g(i), update the corresponding elements of U(i,s)the upper triangular matrix .U
   4. After all columns have been processed, if the output parameter requires computing a gain K, K = U*(D.(H*U)')/Rthe gain is computed according to the formula , where .represents the transpose of the matrix product.
   5. If the output parameter requires an update of the state value X, X = X + K*(Z-H*X)the state value is updated according to the formula . where is Z-H*Xthe prediction error.
4. Summary: This code implements a recursive least squares filter, using UD decomposition techniques to update the matrix and calculate the gain. It gradually updates the state and gain coefficient of the filter by processing the matrix column by column, so as to realize the filtering function of the filter.

CIGARETTE

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

The code implements UDUT decomposition (Upper-Triangular Decomposition with Diagonal Similarity Transform), which decomposes a non-negative definite symmetric matrix P into the form of U * diag(D) * U'. 2. Starting with the last row, each row is processed upwards. 3. First calculate the diagonal element D(j) of the current row j, and subtract the diagonal element P(j ,j). 4. If D(j) is less than or equal to trPn, it means that the element is very small and has little effect on the decomposition result, so the processing of this row is skipped. 5. If D(j) is greater than trPn, you need to calculate the element U(i,j) of the current row j and the previous row i, by using the element P(i,j) of the current row and the previous upper triangular element U(i ,k) and U(j,k) and D(k) are interpolated and divided by D(j). 6. After processing all rows in a loop, output U and D are the results of UDUT decomposition.

The purpose of this code is to decompose a non-negative definite symmetric matrix into UDUT for subsequent calculation and analysis. UDUT decomposition can be used in applications such as solving linear equations, calculating the determinant of a matrix, and calculating the eigenvalues ​​and eigenvectors of a matrix.

Two, kalman filtering

Kalman filter is a high-efficiency recursive filter (autoregressive filter) that can estimate the state of a dynamic system from a series of incomplete and noisy measurements.

The main idea of ​​Kalman filtering is to use the state transition matrix and observation matrix, combined with the current state and the state of the previous moment, to estimate the current optimal state1. Specifically, the steps of Kalman filtering include 1:

1. Initialize the state vector and covariance matrix of the filter.
2. For each time step, the following steps are performed:
   1. Using the state transition matrix and the current state vector, the state vector and covariance matrix at the next moment are calculated.
   2. Combining the observation matrix and the current observation vector, the state estimate and covariance matrix at the current moment are calculated.
   3. Based on the new state estimate and covariance matrix, update the filter's state vector and covariance matrix.
3. Repeat step 2 for iterative optimization until the convergence condition is met.

Compared with traditional filters, Kalman filtering has better numerical stability and computational efficiency, and can estimate the value of state variables more accurately.

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;

This code is a simple Kalman filter implementation. The Kalman filter is a method for estimating the state of a dynamic system by combining previous estimates with the latest observations to update an estimate of the current state.

Here is a functional explanation of the code:

1. Input parameters:
   • Phikk_1: State transition matrix, which describes the transition mode of the system state.
   • Gammak: Process noise covariance matrix, describing the noise introduced in the system process.
   • Qk: Process noise covariance matrix, describing the noise introduced in the system process.
   • Xk_1: The previous state estimate.
   • Zk:data observation.
   • s: Optional parameter, used to adjust the smoothing factor of the Kalman gain, the default is 1.
2. initialization:
   • Xkk_1: Calculate the predicted value of the current state based on the state transition matrix and the previous state estimation.
   • Pxkk_1: Calculate the error covariance matrix of the current state prediction value according to the state transition matrix, process noise covariance matrix and previous state estimation error covariance matrix.
3. predict:
   • Pxykk_1: converts the error covariance pseudo-matrix of the current state prediction value into a covariance matrix between the predicted state and the observation.
   • Pykk_1: Calculate the predicted observation noise covariance matrix according to the observation matrix, the covariance matrix between the predicted state and observation, and the observation noise covariance matrix.
4. renew:
   • Kk: The Kalman gain is computed from the inverse of the covariance matrix between the predicted state and observations and the predicted observation noise covariance matrix.
   • Xk: Update the current state according to the current state prediction value and the Kalman gain of the observed data.
   • Pxk: From the updated error covariance matrix of the current state, the updated error covariance matrix is ​​obtained by subtracting the transpose of the product of the Kalman gain and the predicted observation noise covariance matrix.
   • Pxkk_1: current state prediction value error covariance matrix.

The code implements a simple Kalman filter, which can perform state estimation and prediction according to the input parameters, and output the estimated value of the current state and related information such as the error covariance matrix.

KFUD: UD decomposition Kalman filter

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;

This function implements an algorithm called the Universal Diagonal Kalman Filter (UDUT square root Kalman filter). The UD decomposed Kalman filter is an extended state estimation algorithm suitable for dealing with nonlinear systems.

The function accepts the following input parameters:

• U: unit upper triangular matrix, used to represent the state transition matrix.
• D: A vector representing the elements of the diagonal matrix.
• Phi: System matrix, representing the dynamic model of the system.
• Tau: A vector representing the distribution matrix of the process noise (also called process noise).
• Q: A vector representing the variance of the process noise (expressed as a vector).
• H: a measurement matrix, indicating how to obtain measurements from the state.
• R: The variance of the measurement noise.

The function returns the following output parameters:

• U: The updated unit upper triangular matrix.
• D: Vector of elements of the updated diagonal matrix.
• K: Kalman filter gain.

The main steps of the algorithm are as follows:

1. Initialization: According to the input parameters, initialize some variables, including temporary variables during calculation.
2. Time update: traverse each time step from back to front, calculate the state prediction and the elements of the diagonal matrix at each time step according to the dynamic model of the system and the process noise. If the predicted diagonal element is smaller than a threshold ( meanD), it is set to zero to avoid numerical instability caused by too small values.
3. Measurement update: If measurement noise variance is present (i.e. Rnon-zero), then a measurement update step is performed. First compute the predicted measurement residuals ( f) and the weights of the residuals ( g). Each time step is then traversed from back to front, updating the elements of the diagonal matrix and the weights on the diagonal elements according to the measurement residuals and weights. The Kalman filter gain ( ) is then updated according to the updated diagonal elements K.
4. Return Output: Returns the updated unit upper triangular matrix, a vector of elements of the diagonal matrix, and the Kalman filter gains.

The output of this function can be used in further state estimation or control applications.

kfc2d: continuous-time Kalman filter discretization

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    

This code is a function for converting a system of difference equations for a continuous-time Kalman filter into a discrete-time model. The function is named kfc2d, and its input parameters include the transition matrix of the continuous-time system Ft, the system noise variance matrix Qt, the discretization time interval Ts, and the order of the Taylor series expansion n. The output of the function includes the discrete-time transition matrix Phikk_1and the noise variance matrix Qk.

The main logic of the code is as follows:

1. First, the discrete-time transition matrix is ​​initialized Phikk_1as the identity matrix plus the first-order approximation term Ts*Ft.
2. Initialize the noise variance matrix Qkas a first-order approximation Qt*Ts.
3. If the input nis less than or equal to 1 (the default value), it will be returned directly.
4. Enter a loop to iteratively compute higher order approximations starting from the second order.
5. In each iteration, the update interval Tsiis a power of the current interval, ie Tsi = Tsi*Ts.
6. Update the order factor factito the power of the current order factor, ie facti = facti*i.
7. Updates the transition matrix of the continuous-time system Ftito the power of the current transition matrix, ie Fti = Fti*Ft.
8. Adding the product of the current time interval and the order factor to the current transition matrix yields the discrete-time transition matrix, ie Phikk_1 = Phikk_1 + Tsi/facti*Fti.
9. To update the calculation of the noise variance matrix, a new noise variance matrix is ​​obtained through the product of the current time interval and the current noise variance matrix and the transpose operation, ie Mi = FtMi + FtMi'.
10. Adding the product of the current time interval and the order factor to the new noise variance matrix yields the discrete-time noise variance matrix, ie Qk = Qk + Tsi/facti*Mi.
11. At the end of the loop, the discrete-time transition matrix Phikk_1and noise variance matrix are returned Qk.

This code is mainly used to calculate the discrete-time transition matrix and noise variance matrix through the Taylor series expansion method based on the difference equation of the continuous-time model, so as to apply the Kalman filter for state estimation and prediction in a discrete-time environment.

3. ekf: Extended Kalman Filter

EKF (Extended Kalman Filter, Extended Kalman Filter for short) is a high-efficiency recursive filter (autoregressive filter), which can estimate the state of a dynamic system from a series of incomplete and noisy measurements. .

The main idea of ​​EKF filtering is to use the state transition matrix and observation matrix, combined with the current state and the state of the previous moment, to estimate the current optimal state1. Specifically, the steps of EKF filtering include 1:

1. Initialize the state vector and covariance matrix of the filter.
2. For each time step, the following steps are performed:

a. Using the state transition matrix and the current state vector, calculate the state vector and covariance matrix at the next moment.

b. Combining the observation matrix and the current observation vector, calculate the state estimate and covariance matrix at the current moment.

c. Update the state vector and covariance matrix of the filter based on the new state estimate and covariance matrix. 3. Repeat step 2 for iterative optimization until the convergence condition is met.

Compared with traditional filters, EKF filtering has better numerical stability and computational efficiency, can more accurately estimate the value of state variables, and performs better in systems with high dimensions and strong nonlinearity.

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

This code is an implementation of the Extended Kalman Filter (EKF). Here is an explanation of what the code does:

1. The function ekfaccepts three input arguments: kf(state of the extended Kalman filter), yk(measurement value), TimeMeasBoth(a character indicating the type of time measurement).

2. Depending on the number of input arguments, determine TimeMeasBoththe value of . If the number of input parameters is 1, TimeMeasBothset to the character 'T'; if the number of input parameters is 2, set TimeMeasBothto the character 'B'.

3. Depending on TimeMeasBoththe value of , do the following:

   1. If TimeMeasBoth is the character 'T' or the character 'B':

      1. If kfthe field 'fx' (non-linear state Jacobian matrix) exists in , then call the function ekfJcbto compute kf'Phikk_1' and 'xkk_1' (the state transition matrix and the state estimate for the next moment, respectively) of .

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

      2. If xkk_1empty, it is set to Phikk_1multiplied by the current state xk.

      3. If fxnot present, xkk_1set to 'Phikk_1' multiplied by the current state xk.

      4. Compute Pxkk_1(the covariance matrix estimate for the next instant), using the values ​​of Phikk_1, Pxk, Gammak, Qkand Gammak.

      5. If TimeMeasBothis a character , only time update is performed, and the state estimated value and covariance matrix Tat the next moment are assigned to the current state and covariance matrix , and then returned.xkk_1Pxkk_1xkPxk

   2. If TimeMeasBothis a character B, proceed as follows:

      1. xkk_1According to the state estimate and covariance matrix at the next moment Pxkk_1, as well as the measured value ykand noise covariance matrix Rk, calculate the Kalman gain (Kalman gain) and the updated state estimate xkand covariance matrix Pxk.
      2. Assign the updated state estimate xkand covariance matrix Pxkto the current state xkand covariance matrix Pxk.

4. After the function is executed, the updated extended Kalman filter state is output kf.

Overall, this code implements the time update and measurement update process of the Extended Kalman Filter, which updates the state and covariance matrix of the filter according to the input state, measurement value, and time measurement type.

4. UKF: Unscented Kalman Filter

UKF (Unscented Kalman Filter), the Chinese interpretation is unscented Kalman filter, unscented Kalman filter or dearomatic Kalman filter, which is a filtering algorithm for state estimation of nonlinear non-Gaussian systems1.

The main idea of ​​the UKF filter is to use the Unscented transformation to convert the nonlinear system equation into a standard Kalman filter system under the linear assumption. Specifically, the steps of UKF filtering include:

1. Select a set of parameters for the Unscented transformation, and calculate the weight and mean in the Unscented transformation.
2. The first-order Taylor expansion is performed on the system model and the observation model to obtain the nonlinear residual term.
3. These residual terms are used to update the state estimate.
4. Repeat steps 2 and 3 for iterative optimization until the convergence condition is met.

Compared with traditional Kalman filtering, UKF filtering has better numerical stability and computational efficiency, can more accurately estimate the value of state variables, and performs better in systems with high dimensions and strong nonlinearity.

It should be noted that in practical applications, the implementation and parameter settings of the UKF filtering algorithm need to be adjusted according to the specific system model and observation model, and experimental verification and optimization should be carried out. At the same time, for large-scale or high-real-time application scenarios, the efficiency and scalability of the algorithm also need to be considered.

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

This code is an implementation of UKF. Here is an explanation of what the code does:

1. Input parameters:
   • kf: An array of structures containing the state and parameter information of the Kalman filter, such as state vector xk, state covariance matrix Pxk, time update coefficient fx, measurement update coefficient hx, etc.
   • yk: The measurement vector, that is, the observed value of the system.
   • TimeMeasBoth: A character string used to specify the method of time update and measurement update. The optional values ​​are 'T'(time update only), 'M'(only measurement update) and 'B'(time and measurement update at the same time).
2. Determine the update method according to the input parameters:
   • If the input parameter TimeMeasBothis 'T'or is not specified, a time update is performed.
   • If the input parameter TimeMeasBothis 'M'or is not specified, a measurement update is performed.
3. Time update:
   1. If there are no , and fields kfin the structure array , they are assigned default values.alphabetakappa
   2. kfIf there is a field in the structure array fx, it means that there is a nonlinear state transition function, and the time update needs to be performed according to the input observations and filter parameters.
      1. ukfUTFirst, calculate the state vector and state covariance matrix after the state transition by calling the function (also a function in this code file).
      2. Then, the correction term of the state covariance matrix is ​​calculated according to the filter parameters and added to the state covariance matrix.
   3. kfIf there is no field in the structure array fx, it means that the state transition is linear, and the predicted state vector and covariance matrix are used directly.
4. Measurement update:
   1. If measurement update is performed, it is first determined whether there is only measurement update.
   2. If only measurements are updated, assign the predicted state vector and covariance matrix to the current state vector and covariance matrix, respectively.
   3. If time and measurement updates are done simultaneously, measurement updates are performed according to the filter parameters.
      1. kfIf there is a field in the structure array hx, it means that there is a nonlinear measurement transfer function, and measurement updates need to be performed according to the input observations and filter parameters.
         1. First, ukfUTthe measurement vector after the measurement shift, the measurement covariance matrix, and the cross-covariance matrix are calculated by calling the function .
         2. Then, a correction term for the measurement covariance matrix is ​​calculated based on the filter parameters and added to the measurement covariance matrix.
      2. kfIf there is no field in the structure array hx, it means that the measurement transfer is linear, and the predicted measurement vector and covariance matrix are used directly.
5. Return result: The updated Kalman filter structure array kfwill be returned as output.

Overall, this code implements the time update and measurement update process of the Kalman filter for nonlinear systems. It updates the state vector and covariance matrix of the filter according to the input observation value and filter parameters to realize the state estimation function of the filter.

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/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

This code implements the function of Unscented Transformation (UT). UT is a method for state estimation of nonlinear systems. It obtains the estimation results of state variables through a series of sampling and propagation of nonlinear equations.

The input parameters of this function include:

• x: state vector (state vector).
• Pxx: The variance matrix of the state vector.
• hfx: A handle to handle nonlinear state equations.
• tpara: Some time-varying parameters for passing to hfx.
• alpha, beta, kappa: parameters of UT transformation.

The output parameters of this function include:

• y: The state vector after UT transformation.
• Pyy: The variance matrix of the state vector after UT transformation.
• Pxy: The covariance matrix between the state vector and the state vector after UT transformation.
• X, Y: Sigma point vectors before and after UT transformation.

The main steps of the code are as follows:

1. Calculate some intermediate variables according to the input parameters, including n(the length of the state vector), lambda, gamma, Wmand Wc.
2. Perform Cholesky decomposition to convert the input state variance matrix Pxxinto a lower triangular matrix sPxx.
3. Construct the Sigma point vector X, including the points of three parts, which are the state vector itself, the state vector plus and minus the Cholesky decomposition matrix of the state variance matrix.
4. hfxPerform nonlinear propagation for each sigma point, calculate the output value of each point by calling the handle function , and store the result in the matrix Y.
5. Calculate the estimated state vector yand variance matrix Pyy, and the covariance matrix between the state vector and estimated state vector according to the formula of UT transformation Pxy.
6. Returns the estimated state vector, variance matrix, covariance matrix, and sigma point vector before and after the UT transformation.

It should be noted that the MATLAB function feval is used in this code to call the handle function hfx, and multiple calls are made in the loop. Therefore, when using this code, it is necessary to ensure the correct implementation and callability of the handle function 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/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);

This code is a MATLAB function for calculating the weight of cubature points of order 3 or 5. Here is a functional explanation of the code:

1. The input parameters of the function are:
   • n: dimension (integer)
   • afa, beta, kappa: parameters (can be scalars, vectors or matrices)
2. The output parameters of the function are:
   • U: vector of cubature points ( (3*n, 1)column vector of size )
   • wm: Uweights corresponding to each point in ( (3*n, 1)column vector of size )
   • wc: Uweights corresponding to each point in ( (3*n, 1)column vector of size )
3. Code logic explanation:
   • First, check the number of input parameters, if less than 4, set kappato 0; if less than 3, betaset to 2; if less than 2, afaset to 1e-3.
   • Next, the values ​​of the variables lambdaand are calculated based on the parameters gamma.
   • Then, gammacalculate the matrix of cubature points according to and other parameters U.
   • Finally, the weight vector sum is calculated based on the parameters and calculation wmresults wc. Among them, wmthe first two elements of are the same, and (2*n, 1)each element of the matrix of size is 1/(2*gamma^2); the third element is lambda/gamma^2. wcThe first two elements of are wmthe same as , and the last element is (3*n, 1)a column vector of values (1-afa^2+beta).

The function of this function is to calculate the position and weight of the cubature point through the input parameters. Cubature points is a method for numerical integration that estimates the integral value in a high-dimensional space by computing some points in a low-dimensional space and summing their weights.

Five, ssukf

SSUKF is a nonlinear filtering algorithm, the full name is Unscented Particle Filtering with Square-Root Unscented Transform. It combines the advantages of Unscented transform and particle filter to deal with the state estimation problem of nonlinear non-Gaussian system. The main steps of SSUKF include:

1. Select a set of parameters for the Unscented transformation, and calculate the weight and mean in the Unscented transformation.
2. The first-order Taylor expansion is performed on the system model and the observation model to obtain the nonlinear residual term.
3. Using the particle filter method, the new state estimation value is obtained by weighting and estimating the residual term.
4. Repeat steps 2 and 3 for iterative optimization until the convergence condition is met. At the same time, for large-scale or high-real-time application scenarios, the efficiency and scalability of the algorithm also need to be considered.

The difference between UKF and SSUKF:

1. The way of Unscented transformation is different: UKF uses standard Unscented transformation, while SSUKF uses Square-Root Unscented transformation, which can calculate weight and mean value faster and improve the computational efficiency of the algorithm.
2. The way of particle filtering is different: In UKF, standard particle filtering is used to estimate state variables, while in SSUKF, state variables are estimated using a particle filtering method called Sequential Importance Sampling. This filtering method can better handle systems with strong nonlinearity and can improve the numerical stability of the algorithm.
3. The applicable scenarios are different: UKF is suitable for dealing with high-dimensional nonlinear systems, while SSUKF is suitable for dealing with low-dimensional nonlinear systems, especially for some application scenarios with high real-time requirements.

It should be noted that whether it is UKF or SSUKF, they are relatively new filtering algorithms. In practical applications, they need to be adjusted and optimized according to the specific system model and observation model, while considering the efficiency and scalability of the algorithm. sex 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

This code is a function for Unscented Kalman Filter for nonlinear systems. It takes an initial array of Kalman filter structures kf, a vector of measurements yk, and a flag indicating the time update and measurement update TimeMeasBoth.

The main logic of the code is as follows:

1. According to the number of input parameters, the determined TimeMeasBothvalue, if there is only one input parameter, TimeMeasBothis set to 'T', which means only time update; if there are two input parameters, TimeMeasBothit is set to 'B', which means simultaneous time update and measurement updates.
2. If TimeMeasBoth'T' or 'B', perform a time update step. If kfthere is a nonlinear state transition function in the Kalman filter structure array fx, use the SSUT function for state prediction, and update the state and state covariance matrix. If only time is updated, the updated one will be returned directly kf.
3. If TimeMeasBoth'M' or 'B', perform a measurement update step. If only measurements are updated, set state and state covariance matrix to previous values. kfIf there is a nonlinear measurement transfer function in the array of Kalman filter structures hx, use the SSUT function for measurement prediction and update the measurement and measurement covariance matrices.
4. Perform the filtering step. Computes the Kalman gain and updates the state estimate and state covariance matrix using the measured and predicted state and covariance matrices.

Finally, the function returns an array of updated Kalman filter structures kf.

It's important to note that this code depends on other functions SSUT, which may need to be defined or provided elsewhere. In addition, the code also depends on the definition of some variables, such as Phikk_1, Gammak, Qk, Hketc., which should be kfprovided in the input array of Kalman filter structures.

If you're going to use the function, make sure you provide the correct input parameters and the correct function dependencies.

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

This is a MATLAB function that implements the Spherical Simplex Unscented Transformation (SSUT). SSUT is a nonlinear filtering method for state estimation and prediction of nonlinear systems.

Function input parameters include:

• x: state vector (a column vector).
• Pxx: Variance matrix of state vectors.
• hfx: A handle to a function describing the nonlinear state equation.
• tpara: A time-varying argument vector for passing to hfxthe function.
• w0: Weight parameter.

Function output parameters include:

• y: State vector after UT transformation (a column vector).
• Pyy: The variance matrix of the state vector after UT transformation.
• Pxy: The covariance matrix between the state vector and the state vector after UT transformation.
• Xand Y: Sigma point vectors generated before and after UT transformation.

The function first checks if some global variables have already been initialized SSUT_sand SSUT_wif not initialized according to default values. Then generate corresponding Sigma points according to the input parameters xand . PxxThen call the nonlinear state equation hfxto calculate the output values ​​corresponding to these Sigma points, and store these output values ​​in Y. yThen calculate the UT-transformed state vector and its variance matrix Pyy, as well as the covariance matrix between the state vector and the UT-transformed state vector, according to the spherical simple Kalman filter algorithm Pxy.

Finally, the function returns the UT-transformed state vector yand its variance matrix Pyy, the covariance matrix between the state vector and the UT-transformed state vector Pxy, and the sum of Sigma point vectors generated before Xand after the UT transformation Y.

This function is the realization of SSUT, which can be used for state estimation and prediction of nonlinear systems.

6. CKF: volumetric Kalman filter

Cube-based Radial Basis Function (CKF) is a filtering algorithm for state estimation of nonlinear non-Gaussian systems.

The core idea of ​​CKF filtering is to use the volume criterion to approximate the posterior mean and covariance of the state, so as to ensure that the posterior mean and variance of any nonlinear Gaussian state can be approximated theoretically by a third-order polynomial.

Compared with traditional Kalman filtering, CKF filtering has better numerical stability and computational efficiency, can more accurately estimate the value of state variables, and performs better in systems with high dimensions and strong nonlinearity.

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

This code is a function that implements the Cubature Kalman Filter (CKF). CKF is a nonlinear filtering method for estimating the state of a dynamic system.

The role of the function is to time update or measure update the filter ckfaccording to the input filter structure array kf, measurement vector ykand time/measurement update flag .TimeMeasBoth

The main logic of the code is as follows:

1. Determines the time/measurement update flags based on the parameters entered TimeMeasBoth. If there is only one input parameter, the default time update flag is 'T'; if there are two input parameters, the update flag is determined according to the second parameter.
2. If the time update flag is 'T' or 'B' (indicating a time update or both a time update and a measurement update), do the following:
   1. If kfa nonlinear state propagation function is present in the filter structure array fx, use ckfCTthe function for state prediction and covariance prediction.
   2. If the time update flag is 'T', update the state and covariance of the filter and return the result.
3. If the measurement update flag is 'M' or 'B' (indicating that a measurement update is required or both a time update and a measurement update), do the following:
   1. If the measurement update flag is 'M', set the state and covariance of the filter to the current state and covariance.
   2. If kfa nonlinear measurement spread function is present in the filter structure array hx, use ckfCTthe function for measurement prediction and covariance prediction.
   3. Filtering is performed, and Kalman gains, state estimates, and covariance estimates are computed.

Finally, the function returns the updated array of filter structures kf.

It should be noted that some functions in this code (such as ckfCT, ukf, ekf, kfupdate) are not defined in the code, and may be auxiliary functions or sub-functions defined elsewhere. This code only shows the main logic of implementing CKF without providing complete implementation details.

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;

This code implements the Spherical-Radial Cubature transformation (CKF-CT) for nonlinear state estimation. Below is the functional explanation of the code:

1. Code entry:
   • x: A vector of current states.
   • Pxx: the variance matrix of the current state vector.
   • hfx: Handle to the nonlinear state equation.
   • tpara: hfxsome time-varying parameter passed to .
2. Code output:
   • y: The state vector after Unscented transformation.
   • Pyy: The variance matrix of the state vector after Unscented transformation.
   • Pxy: The covariance matrix between the state vector after Unscented transformation and the initial state vector.
   • X: Sigma point vector before Unscented transformation.
   • Y: Sigma point vector after Unscented transformation.
3. Code:
   • First, the input variance matrix is Pxx​​transformed into an upper triangular matrix by Cholesky decomposition sPxx.
   • Then, generate two Sigma points, namely the matrix , according to the current state vector xand the upper triangular matrix .sPxxX
   • hfxNext, calculate the output vector corresponding to the initial Sigma point by calling the handle of the nonlinear state equation Y.
   • Then, use a loop to calculate the outputs for each sigma point and accumulate them into the total output vector yand covariance matrix Pyy. At the same time, the covariance matrix between the initial state vector and the total output vector is calculated Pxy.
   • Finally, the final state vector and covariance matrix are calculated from the total output vector and covariance matrix.
4. Precautions:
   • The code uses the function feval in MATLAB to call the handle of the nonlinear state equation hfxfor calculation. hfxMake sure that the handles and related parameters for the nonlinear equation of state are defined and properly set before using this code tpara.
   • The comments in the code provide some instructions and references, which can help to understand the function and implementation of the code.

Overall, the code implements the Spherical-Radial Cubature Transform (CKF-CT) for nonlinear state estimation, estimates the state through the Unscented Transform (UT), and calculates the variance and covariance of the estimated results.

Seven, PF: particle filter

Particle Filter is a non-parametric Bayesian filtering method that can be used to estimate and predict random processes. It uses a group of weighted particles (ie samples) to represent the state of the random process, so as to realize the estimation and prediction of the random process.

The main idea of ​​the particle filter is to use the known prior probability distribution and observation data to iteratively update the weight and state of the particles, so as to obtain the posterior probability distribution. Specifically, particle filtering includes the following steps:

1. Initialization: According to the prior probability distribution, generate a set of initial particles and assign corresponding weights.
2. Prediction: According to the known dynamic model, advance each particle forward by one time step to obtain the particle set at the next moment.
3. Resampling: According to the weight of each particle, the particles are resampled, so that the probability of particles with larger weights appearing in the sampling is greater.
4. Output: According to the resampled particle set, the estimated value of the posterior probability distribution can be obtained.

Particle filtering has applications in many fields, such as object tracking, pose estimation, navigation, speech recognition, image processing, etc. However, particle filtering also has some problems, such as particle degradation, noise sensitivity, etc., which need further research and improvement.

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;

Eight, akfupdate: adaptive filtering

This code is an implementation of an Adaptive Kalman Filter. Here is a functional explanation of the code:

1. The function akfupdateaccepts five input parameters:
   • kf: An array containing information about the Kalman filter structure.
   • yk: a measurement vector.
   • TimeMeasBoth: A flag character describing time update and measurement update, which can take the following three values:
     • 'T': Only time update is performed.
     • 'M': Only perform measurement updates.
     • 'B': Simultaneously perform time update and measurement update.
   • kftype: Type of adaptive Kalman filter.
   • para: some parameters.
2. Determine the number of input parameters, and perform corresponding time update or measurement update according to different flag characters.
3. If TimeMeasBothyes 'T', perform a time update:
   1. Multiply kf.Phikk_1by kf.xk, to update the state estimate kf.xk.
   2. Multiply kf.Phikk_1by kf.Pxk, multiply by kf.Phikk_1', and add to kf.Gammak * kf.Qk * kf.Gammak'update the state estimation error covariance matrix kf.Pxk.
4. If TimeMeasBothyes 'M', perform a measurement update:
   • Save the current state estimate and error covariance matrix to kf.xkk_1and kf.Pxkk_1.
5. If TimeMeasBothyes 'B', perform time update and measurement update:
   1. Save the current state estimate and error covariance matrix to kf.xkk_1and kf.Pxkk_1.
   2. Compute kf.Pxykk_1, that is, the covariance matrix between the current estimates and measurements.
   3. Compute kf.Py0, that is, the covariance matrix of the measurement noise multiplied by Hkthe transpose of .
   4. Calculate the residual kf.rk, which is the actual measurement minus the predicted measurement.
6. kftypeDifferent adaptive filtering algorithms are implemented depending on the type of :
   1. If kftypeis a string 'KF'or , some variable such as and 'MSHAKF'is evaluated based on the provided arguments .bs
   2. If kftypeis a number 2, the MCKF (Minimum Curvature Filter) algorithm is performed with one parameter sigma.
   3. If kftypeis a number 3, the RSTKF (Residual Square Total Least Squares Filter) algorithm is performed with one parameter v.
   4. If kftypeis a number 4, execute the SSMKF (Square Root Split-Operator Matrix Kalman Filter) algorithm.
7. Store the updated state estimate, state estimate error covariance matrix, and other relevant information into the input structure array kf.
8. Outputs an array of updated Kalman filter structures kf.

The code implements different algorithms of the adaptive Kalman filter, and selects the corresponding time update or measurement update operation according to the input logo characters.

shamn : Sage-Husa adaptive filtering

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

This code is an implementation of a Sage-Husa adaptive filter. Here is a functional explanation of the code:

1. Input parameters:
   • Rk: Measurement noise variance, a column vector.
   • r2: RkThe square of the mean minus the square of the initial residual, also a column vector.
   • Rminand Rmax: are the lower and upper bounds of the measurement noise variance, respectively, two column vectors.
   • betaand b: Two column vectors representing the forgetting factor and the gaining factor, respectively.
   • s: bAmplification factor to use when equal to 0, a column vector.
2. Output parameters:
   • Rk: Updated measurement noise variance, a column vector.
   • beta: The updated forgetting factor, a column vector.
   • maxflag: The maximum sign when r2(k) is greater than Rmax(k), a column vector.
3. Code logic:
   • First, according to the length of the input parameter, initialize a maxflag vector with a length of m, and set all elements to 0.
   • Then, divide beta by (beta+b) to get the new beta value. This is an update based on the formula for the forgetting factor.
   • For each value of k, do the following:
     • If b(k) is equal to 0, it means that r2(k) is amplified by gain factor s(k). At this time, if r2(k) is smaller than Rmin(k), set Rk(k,k) to Rmin(k); otherwise, set Rk(k,k) to r2(k).
     • If b(k) is not equal to 0, it means that the forgetting factor beta(k) is used to update Rk(k,k). At this point, if r2(k) is less than Rmin(k), set r2(k) to Rmin(k); if r2(k) is greater than Rmax(k), set r2(k) to Rmax(k) , and set beta(k) to 1 and maxflag(k) to 1. Then update Rk(k,k) according to the formula of forgetting factor.
4. Finally, the updated Rk, beta and maxflag are output as the result.

Overall, this code implements the core function of the Sage-Husa adaptive filter, adaptively adjusts the value of the measurement noise variance according to the input parameters and the current state, and monitors whether the upper limit is exceeded.

9. Robust equivalent weight function

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

This code is a function to calculate weights using a piecewise function approach from the Institute of Geodesy & Geophysics. The function accepts two input arguments: a normalized measure of error (err) and two segmentation points (k0 and k1). The output is the weight value (w).

The code first checks the number of input parameters. If there is only one input parameter (err), set k0 and k1 to 1.0 and 3.0, respectively. If there are two input arguments (err and k0 or k1), set k1 to the second element of k0 and k0 to the first element of k0.

Next, the code assigns the error value abs(err) to the variable w. Then, each element in err is traversed through a loop. In the loop, according to the size of the error value, use the conditional statement to calculate the corresponding weight value.

If the error value is less than or equal to k0, the weight value is set to 1.0; if the error value is greater than k1, the weight value is set to 0.0. For error values ​​between k0 and k1, the weight value is calculated by dividing k0 by the error value.

Finally, the function returns the calculated weight value w.

Some example plotting statements are also provided in the code to show how the weights vary with the error value for different parameter settings.

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

This code is a MATLAB function named igg3. It is used to calculate weights based on a given measurement error and two parameters k0.k1

The input parameters of the function are:

• err: The value of the normalized measurement error.
• k0: One of the segmentation points.
• k1: The second point of segmentation.

The output of the function is:

• w: Calculated weight.

This function uses the 'Institute of Geodesy & Geophysics' picewise method to calculate the weights. According to the size of the measurement error, this method uses different calculation methods between two segmentation points to obtain the weight value.

The function first checks the number of input parameters. If there is only one input parameter err, the default k0is 2.0, k1which is 6.0. According to different error ranges, different formulas are used to calculate the weight value:

• If the error is less than or equal to k0, the weight is 1.
• If the error is greater than k1, the weight is 0.
• Otherwise, the weight value is calculated using the following formula:w(k) = k0/err(k) * ((k1-err(k))/(k1-k0))^2

Finally, return the computed weight vector as the output of the function.

This function is mainly used for robust estimation in elevation fitting, by adjusting the weight according to the size of the measurement error to reduce the influence of the error on the fitting result.

10. Data Fusion

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);

This code is a function for POS (position and attitude) data fusion. It accepts five input parameters: rf (forward AVP (pseudorange)), xpf (forward state estimate and covariance), rr (reverse AVP), xpr (reverse state estimate and covariance), and ratio (with in modifying the proportion of the state estimate). The output of this function is a structure psf with the following fields:

• rf and pf: AVP and covariance after fusion
• r1 and p1: forward AVP and covariance
• r2 and p2: reverse AVP and covariance

The function of the code is as follows:

1. Check the number of input parameters, if less than 5, set ratio to 1.
2. Determine the common instant t, indices i1 and i2, by finding the intersection of rf and rr in the last dimension.
3. Extract r1, x1, p1, r2, x2, and p2, which correspond to the forward and reverse AVPs, state estimates, and covariances, respectively.
4. According to the value of ratio, the first 9 fields of x1 and x2 are scaled.
5. Perform the fusion operation to generate the fused AVP and covariance r and p. A function called fusion is used here, and the specific fusion method may be implemented in this function.
6. Replace NaN values ​​in rf and pf with 0.
7. Return psf as output.

It should be noted that some functions in this code (such as insupdate, kfupdate, POSSmooth, POSPlatform, posplot, etc.) are not implemented in the code, so the specific functions of these functions cannot be determined. Also, the implementation of this code may depend on other functions or libraries not included in this code snippet.

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

This function is a function for position smoothing through INS-VelPos and GNSS-VelPos data. Its input includes INS Vel&Pos data array vp, GNSS Vel&Pos data array gnss, smoothed time span nSec and a graphic flag isfig. Its output is the smoothed INS Vel&Pos data array vpOut.

The following are the specific implementation steps of this function:

1. First, judge whether the figure flag isfig and the smoothing time span nSec are specified according to the input parameters. If not specified, the default isfig is 1 and nSec is 10.
2. Next, calculate the error err between the GNSS data and the INS data. This error is calculated by linearly interpolating between the last point of the INS data and the last point of the GNSS data. Then replace the NaN values ​​in the gnss array with the err array, and initialize the errs array to the err array.
3. Based on the smoothing time span nSec, calculate the number of points to smooth. This is done by converting nSec to the number of points in the gnss array.
4. For each point k to be smoothed, compute the value of err(:,end-k). This is calculated by linearly interpolating between the last point in the INS data and the previous point in the GNSS data. Then use the smoothhol function to smooth err(:, end-k) to get errs(:, end-k).
5. An indicator index idx is calculated according to the interp1 function, which is used to determine which points need to be smoothed.
6. Initialize the vpOut array as the original vp array.
7. For each point k to be smoothed, according to the values ​​of errs(:,end-k) and vp(idx,end), use the interp1 function to interpolate and add the result to vpOut(idx,end-k), to obtain smoothed position data.
8. If isfig equals 1, plot the smoothed result. This includes the error curve and the smoothed position data curve.

It should be noted that this function uses some functions in the external library, such as interp1, smoothhol and eth. These functions may require appropriate setup and configuration on your system to function properly.

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*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); % !!!

Eleven: Simulation

htwn: Generate Gaussian white noise

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;

This code is a function for generating heavy-tailed white noise. Here is a functional explanation of the code:

1. The input parameters of the function include:
   • wp: A vector or scalar representing the probability used to determine whether to amplify the generated noise.
   • s: A vector or scalar representing the amount of amplification to amplify the generated noise.
   • len: A vector or scalar representing the number of elements generating noise.
2. The output of the function includes:
   • wn: A vector or matrix of generated noise containing random values ​​from a heavy-tailed distribution.
   • s: The amount of amplification for each noise element.
3. The main logic in the code is as follows:
   • First, by judging the length of the input parameter, it supports simultaneous generation of multiple noise sequences. If the length of wpor sor lenis greater than 1, enter a loop, generate noise sequences one by one and return the result.
   • Then, check whether enough arguments are given against the input arguments. If no lenargument is given, it defaults to 1; if no sargument is given, it defaults to 100; if no wpargument is given, it defaults to 0.01.
   • Next, generate a lenrandom noise vector of length wn, containing random values ​​from the standard normal distribution.
   • Then, generate a wnrandom vector of the same length as r01, with values ​​between 0 and 1.
   • Using the generated random vector r01and a threshold wp, determine which noise elements need to be amplified. wpMultiply the elements whose satisfaction probability is less than by the magnification amount s, and update the corresponding magnification amount s.
4. Finally, return the resulting noise vector and amplification amount as the output of the function.

Overall, this code generates noisy data with heavy-tailed distribution characteristics by using random generation and amplification.

vfbfx: Simulate the state equation of a vertically falling object

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

This code is a function for simulating the equation of state of a vertically falling object. It takes as input an initial state vector X0 and a vector tparana containing parameters, and returns the predicted state vector X1, Jacobian Phik and Hessian matrix D.

Here is a functional explanation of the code:

1. Define parameters:
   • gamma:OK
   • g: acceleration of gravity
   • Ts: discrete time step
2. Initialize variables: Extract the values ​​of x1, x2 and x3 from X0 and store them in these variables respectively.
3. Prediction of state vector X1: Use the Euler discretization method to predict the state equation to obtain the state vector X1 at the next moment.
4. Computes the Jacobian Phik: If the function has more than one output parameter, computes the Jacobian Phik. Phik is used to linearize the equation of state, which numerically approximates the derivative of the equation of state.
5. Calculate the Hessian matrix D: If the function has more than two output parameters, calculate the Hessian matrix D. D is the derivative of the Jacobian matrix used to linearize the state equation, that is, D is the second order derivative to the state equation.

In this code, the Euler discretization method is used to predict the state equation, and the Jacobian matrix and Hessian matrix of the state equation are calculated. These matrices are important for tasks such as state estimation and parameter identification.

vfbhx: Measurement equations for simulating vertically falling objects

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

This code is a MATLAB function to calculate the measurement equation for a vertically falling object. Here is a functional explanation of the code:

1. Function name: vfbhx, which accepts three input parameters: X, tparaand rk.
2. Xis a column vector containing three elements: altitude, velocity, and trajectory parameters.
3. tparais a two-element vector, the first element is the mass parameter M, and the second element is the reference height H.
4. rkis a multiple of the measurement noise used to simulate the noise in real measurements.
5. Comments at the beginning of the code provide some reference information, including bibliography and copyright notice.
6. The first commented line of code is a linear model to test, and if the function has four input arguments (ie ), a multiplied random noise is added to nargin==3it before computing the result .ZV
7. ZThe calculation of is carried out according to the measurement equations, using altitude and ballistic parameters, and taking into account the influence of distance.
8. If the function needs to output the Jacobian matrix Hk( nargout==2), the value of the Jacobian matrix is ​​calculated according to the measurement equation.
9. If the function needs to output a Hessian matrix D( nargout==3), the element values ​​of the Hessian matrix are calculated according to the measurement equation.
10. Finally, if a third element is included in the input argument , a multiplied random noise is added to it rkbefore computing the result , to simulate the noise in the measurement.Zrk
11. The last line of the function is optional, it is used to display the output of the function on the command line.

In summary, this code implements a function for calculating the measurement equation of a vertically falling object. Based on the input altitude, velocity, ballistic parameters and measurement noise, it calculates the measurement results and can output Jacobian matrix and Hessian matrix (if required).