CN111024083A - Observability numerical analysis method for navigation system - Google Patents

Observability numerical analysis method for navigation system Download PDF

Info

Publication number
CN111024083A
CN111024083A CN201911238453.7A CN201911238453A CN111024083A CN 111024083 A CN111024083 A CN 111024083A CN 201911238453 A CN201911238453 A CN 201911238453A CN 111024083 A CN111024083 A CN 111024083A
Authority
CN
China
Prior art keywords
matrix
observability
state
decoupling
navigation system
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN201911238453.7A
Other languages
Chinese (zh)
Other versions
CN111024083B (en
Inventor
刘沛佳
卢丙举
秦丽萍
马永
李广华
侯冬冬
陈飞宇
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
713th Research Institute of CSIC
Original Assignee
713th Research Institute of CSIC
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 713th Research Institute of CSIC filed Critical 713th Research Institute of CSIC
Priority to CN201911238453.7A priority Critical patent/CN111024083B/en
Publication of CN111024083A publication Critical patent/CN111024083A/en
Application granted granted Critical
Publication of CN111024083B publication Critical patent/CN111024083B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
    • G06F17/13—Differential equations
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization

Abstract

The invention provides a navigation system observability numerical analysis method, which comprises the steps of firstly constructing an observability matrix of a system, realizing state variable decoupling by carrying out real transformation on the observability matrix, obtaining relational expressions of state variables after decoupling, and visually seeing the coupling relation between the state variables through the relational expressions so as to carry out observability analysis. The method does not require the system to be completely observable, purposefully selects key state variables for decoupling by using the priori knowledge of the navigation system, does not solve the specific form of the decoupled state variable relational expression when the state variables are decoupled, substitutes known parameter values of the system into the observability matrix by using software such as Matlab and the like as a mathematic auxiliary tool, then carries out row transformation on the observability matrix in the numerical form to obtain the numerical form of the decoupled state variable relational expression, and finally carries out observability analysis on the system by using the state variable relational expression in the numerical form.

Description

Observability numerical analysis method for navigation system
Technical Field
The invention relates to a navigation system observability numerical value analysis method, which is suitable for observability analysis of a multi-dimensional complex navigation system.
Background
When a filtering algorithm is used to estimate the state of the navigation system, whether each state variable can be accurately estimated or not and the speed and accuracy of the estimation are determined by the observability thereof. If the system is completely observable, the decision elements influencing the state estimation precision are only system noise and measurement noise; if the system is not completely observable, all state variables cannot be accurately estimated even if the influence of noise is not considered, and only observable state variables can obtain a good estimation effect. Therefore, prior to state estimation, it is necessary to perform observability analysis on the system to determine whether the system is fully observable, and for incompletely observable systems to determine observable state variables and unobservable state variables.
The existing observability analysis methods mainly comprise observability analysis methods and observability degree analysis methods based on singular values. The observability analysis method realizes state decoupling by performing row transformation on an observability matrix of the system to obtain a decoupling equation of a state variable, and judges which states can be accurately estimated through the decoupling equation. However, for a multidimensional complex system, the decoupling process is very difficult, the expression after decoupling is also very complex, and the mutual relation between state variables is difficult to reflect intuitively. In addition, the observability analysis method lacks pertinence when selecting a decoupling state, and cannot be applied to all systems. For example, if the elements of a corresponding column of a state variable in the observability matrix are all zero, row transformation cannot be performed, and the observability analysis method cannot be used. The observability degree analysis method based on the singular value is a quantitative analysis method, the observability degree of each state variable is calculated through the singular value of the observability matrix of the system, the larger the observability degree is, namely the faster the convergence speed of the corresponding state is considered, the higher the estimation precision is, and the method is simple and intuitive in use. However, in a multidimensional complex system, a linear independent row of an observability matrix usually corresponds to a combination of a plurality of state variables, and the observability degree analysis method based on singular values simply identifies the state variable with the largest coefficient corresponding to the singular value obtained by singular value decomposition, which is not comprehensive in the stripping method of the combined state variables. Therefore, in actual use, the analysis result and the filter estimation result do not completely coincide with each other. Navigation systems are usually multi-dimensional complex systems, and the above two observability analysis methods may be difficult or inapplicable in application.
Disclosure of Invention
The invention provides a navigation system observability numerical analysis method, which comprises the steps of firstly constructing an observability matrix of a system, realizing state variable decoupling by carrying out real transformation on the observability matrix, obtaining relational expressions of state variables after decoupling, and visually seeing the coupling relation between the state variables through the relational expressions so as to carry out observability analysis. The method does not require the system to be completely observable, purposefully selects key state variables for decoupling by using the priori knowledge of the navigation system, does not solve the specific form of the decoupled state variable relational expression when the state variables are decoupled, substitutes known parameter values of the system into the observability matrix by using software such as Matlab and the like as a mathematic auxiliary tool, then carries out row transformation on the observability matrix in the numerical form to obtain the numerical form of the decoupled state variable relational expression, and finally carries out observability analysis on the system by using the state variable relational expression in the numerical form.
The invention adopts the following technical scheme:
a navigation system observability numerical analysis method, comprising the steps of:
step 1: establishing a state equation and a measurement equation of a navigation system, and constructing an observability matrix Q of the system;
step 2: substituting the known parameter values of the navigation system into the observability matrix Q to obtain the numerical form of the observability matrix Q;
and step 3: calculating the rank of the observable matrix Q to obtain rank (Q) ═ r, where r is the number of linearly independent rows in the observable matrix Q;
and 4, step 4: r linear independent rows are selected from the observable matrix Q to construct a matrix Q1Using a matrix Q1Performing observability analysis instead of the observability matrix Q;
and 5: selecting p (p is less than or equal to r) key state variables for decoupling according to prior knowledge of a navigation system;
step 6: constructing a row transformation matrix T, Q1The left multiplication row transformation matrix T realizes state decoupling to obtain a matrix U, namely U is equal to TQ1；
And 7: the system state vector X and the observation vector Z have a relation Y ═ QX, the vector Y is composed of the observation vector Z and the differential of each time of the observation vector Z, there are
Figure BDA0002304982090000031
n is the dimension of the system. Selecting the constructed matrix Q in the step 4 from the vector Y1Corresponding row construction vector Y of time1Having Y of1＝Q1X, the two sides of which are left-multiplied by a row transformation matrix T to obtain TY1The equation is a decoupled state variable relational equation, and equations corresponding to p decoupling state variables can be obtained from the equation;
and 8: judging whether a certain state variable or some state variables can be estimated through the remaining r-p equations in the TY-UX;
and step 9: removing the coupling terms of the estimable state variables determined in the step 8 from the p equations selected in the step 7;
step 10: and (4) according to the p equations obtained in the step (9), judging which states can be accurately estimated by combining the prior knowledge of the navigation system, calculating the estimation precision, and finishing the observability analysis of the system.
Further, in the above step 1, the observability matrix Q refers to observability of a linear constancy systemObservability matrix Q or total observability matrix Q of piecewise linear constancy systemT。
Further, in the step 4, the matrix Q is obtained1Is to ensure the matrix Q1The first r-1 singular values of (a) are equal to the first r-1 singular values of an observability matrix Q, the matrix Q1Is less than a set value and the absolute value of the difference between the r-th singular value of the observability matrix Q and the r-th singular value of the observability matrix Q is less than the set value.
Further, in the step 5, when selecting the decoupling state variable, the selection order of magnitude smaller than 10 is avoided-3The state variable of (2) is used for avoiding a mathematical assistant tool from introducing large calculation errors in the numerical calculation process.
The invention has the beneficial effects that:
(1) the invention purposefully selects the key state variable of the navigation system as the decoupling state variable and realizes decoupling through numerical calculation, thereby effectively overcoming the problems that the selection of the decoupling state by the observability analysis method lacks pertinence and the calculation is difficult when a multidimensional complex system is analyzed.
(2) The invention realizes state decoupling by implementing transformation on the observability matrix in a numerical form, and performs observability analysis based on the decoupled state variable relational expression. Because the decoupling process is rigorous mathematical calculation, the condition that the analysis result and the filtering estimation result are not completely consistent when the observation degree analysis method based on singular values analyzes the multidimensional complex system is avoided.
(3) The invention does not require the system to be completely observable, can effectively distinguish observable state variables and unobservable state variables in the system for the incompletely observable system, is suitable for observability analysis of a multidimensional complex system, and is convenient for engineering realization because the analysis process is based on numerical calculation.
Drawings
FIG. 1 is a flow chart of the steps of the present invention.
FIG. 2 is a diagram of velocity error and attitude misalignment angle estimation results in an exemplary embodiment of the invention.
FIG. 3 shows the result of the estimation of the SINS and DVL constant errors in an embodiment of the present invention.
Detailed Description
The invention is described in detail below with reference to the figures and specific embodiments.
The initial alignment technique is one of the most important key techniques of an Inertial Navigation System (INS), and the INS needs to complete the initial alignment before being used for navigation, and the accuracy of the initial alignment directly affects the navigation accuracy of the INS. In this embodiment, the method provided by the present invention is used to perform observability analysis on a strap-down inertial navigation system (SINS) moving base initial alignment system with the assistance of a Doppler Velocity Log (DVL).
This example models the earth as having a radius ReThe associated coordinate system, vector, variable and matrix to which are applied are defined as follows:
inertial coordinate system i: origin OiAt the center of the earth, XiThe axis points to the spring equinox, ZiThe axis points to the north pole, Y, along the earth's axis of rotationiThe axis is located in the equatorial circle and is connected with XiAxis and ZiThe axes form a right-hand coordinate system;
terrestrial coordinate system e: origin OeAt the center of the earth, XeAxis and YeThe axes being perpendicular to one another in the equatorial plane, XeAxial direction of greenwich meridian, ZeThe axis points to the north pole along the earth rotation axis, e is fixedly connected with the earth, and i is relative to the earth rotation angular rate omegaieRotating;
navigation coordinate system n: in the embodiment, the northeast space is selected as the navigation coordinate system, i.e. the origin OnIn the floating center of the carrier, XnAxis, YnAxis and ZnThe axes point to the east, north and sky, respectively;
a carrier coordinate system b: origin ObIn the floating center of the carrier, XbWith axis pointing in the starboard direction of the carrier, Y, along the transverse axis of the carrierbThe axis being directed along the longitudinal axis of the carrier towards the bow of the carrier, ZbAxis perpendicular to XbObYbThe plane points upwards;
navigation coordinate system velocity Vn＝[VEVNVU]T：VE、VNAnd VUEast, north and sky velocity components, respectively;
navigation coordinate system speed error delta Vn＝[δVEδVNδVU]T：δVE、δVNAnd δ VUEast, north and sky speed errors, respectively;
speed V of carrier coordinate systemb＝[VbxVbyVbz]T：Vbx、VbyAnd VbzRight, forward and upward velocity components, respectively;
speed error delta V of carrier coordinate systemb＝[δVbxδVbyδVbz]T：δVbx、δVbyAnd δ VbzRight, forward and upward speed errors, respectively;
the carrier position p ═ L λ h]T: l, λ and h are longitude, latitude and altitude, respectively;
carrier position error δ p ═ δ L δ λ δ h]T: δ L, δ λ and δ h are longitude error, latitude error and altitude error, respectively;
attitude misalignment angle phi [ [ phi ] ]xφyφz]T：φx、φyAnd phizAttitude misalignment angles for the x-axis, y-axis, and z-axis, respectively;
accelerometer output fb＝[fxfyfz]T：fx、fyAnd fzThe outputs of the accelerometers for the x-axis, y-axis and z-axis, respectively;
attitude matrix
Figure BDA0002304982090000051
A transformation matrix from the carrier coordinate system b to the navigation coordinate system n;
equivalent accelerometer output fn：
Figure BDA0002304982090000052
fE、fNAnd fUAre respectively provided withEquivalent accelerometer outputs for east, north and sky;
accelerometer constant zero offset
Figure BDA0002304982090000053
And
Figure BDA0002304982090000054
the accelerometers are respectively the x-axis, the y-axis and the z-axis and have constant zero offset;
gyro constant drift epsilonb＝[εxεyεz]T：εx、εyAnd εzGyro constant drift of the x-axis, the y-axis and the z-axis respectively;
constant error of DVL δ bD＝[δbxδbyδbz]T：δbx、δbyAnd δ bzConstant errors of the DVL in the x-axis, y-axis and z-axis directions, respectively;
δsD: DVL scale factor error.
In the above definition and hereinafter [. cndot.. cndot.)]TRepresenting the transpose of the matrix.
Based on the above definition, the method of the present invention comprises the following steps:
step 1: establishing a state equation and a measurement equation of a navigation system, and constructing an observability matrix Q of the system;
the method is suitable for linear constant systems and linear time-varying systems which can be approximated by piecewise linear constant systems, and when an observable matrix is constructed, if the system is a linear constant system, the observable matrix Q is directly constructed; if the system is a linear time-varying system, the system is set to be approximate to a linear constant system or a piecewise linear constant system through the working state of the system, the linear constant system is constructed into an observability matrix Q, and the piecewise linear constant system is constructed into a total observability matrix QT. The invention does not distinguish the observability matrix Q from the total observability matrix Q in the analysis processTTherefore, for convenience of description, Q is used collectively.
In this embodiment, DVL is first createdThe model of the SINS moving base initial alignment system under assistance comprises a state equation and a measurement equation. Selecting a position error delta p and a speed error delta V of the SINSnAttitude misalignment angle phi, accelerometer constant zero offset
Figure BDA0002304982090000064
Gyro constant drift epsilonbConstant error of DVL, δ bDAnd scale factor error δ sDConstituting the state vector X, there are:
Figure BDA0002304982090000061
in the above-described state variables, the state variables,
Figure BDA0002304982090000065
εb、δbDand δ sDFor sensor errors, they are modeled as constant values or vectors composed of constant values in the present embodiment, i.e. the sensor errors are considered to remain unchanged during one start-up of the system, some
Figure BDA0002304982090000062
The system equation of this embodiment is
Figure BDA0002304982090000063
In the formula, a is a system matrix, and ω is system noise. Based on a linearized SINS error model, a system matrix A has a specific form
Figure BDA0002304982090000071
Each sub-matrix in A is in the form of
Figure BDA0002304982090000072
Figure BDA0002304982090000073
Figure BDA0002304982090000074
Figure BDA0002304982090000075
Figure BDA0002304982090000076
Figure BDA0002304982090000081
Figure BDA0002304982090000082
Figure BDA0002304982090000083
This embodiment measures DVL
Figure BDA0002304982090000084
Modeling as true value VbConstant error δ bDScale factor error δ sDAnd white noise error omegaDBy superposition of, i.e.
Figure BDA0002304982090000085
The measurement equation of this embodiment is
Figure BDA0002304982090000086
Where the coupling of the error terms is omitted approximately equal to the sign, Z is the observation vector,
Figure BDA0002304982090000087
speed in n series, I, output for SINS3×3Is a 3 x 3 dimensional unit matrix, v is measurement noise, and the measurement matrix H is of a specific form
Figure BDA0002304982090000091
(3) Equations (5) and (5) form a model of the initial alignment system of the SINS movable base with the assistance of DVL. Before using this model for filter estimation, it is necessary to perform observability analysis on the system to determine whether the attitude misalignment angle can be accurately estimated, and thus to determine whether the SINS attitude parameters can be corrected with it. As can be seen from the expressions (3) and (5), the system is a linear time-varying system. At present, observability analysis is still very difficult to perform for a linear time-varying system, so that the system is firstly approximated to a linear steady system or a piecewise linear steady system through setting the working state of the system. In this embodiment, the operating state is set as follows: the uniform linear motion track is the most frequently used navigation track of the aircraft in a task, and under the uniform linear motion track, the initial alignment system of the SINS movable base under the assistance of the DVL is similar to a linear stable system, so that the uniform linear motion track is set for performing the initial alignment of the SINS of the aircraft.
Under the uniform linear motion track, the system is approximate to a linear steady system, and an observability matrix Q of the linear steady system is obtained by a formula (7), wherein n is the dimension of the system, and n is 19 in the embodiment. And substituting the system matrix A, the measurement matrix H and the system dimension n into the formula (7) to obtain an observability matrix Q of the system under the constant linear motion track.
Figure BDA0002304982090000092
Step 2: substituting the known parameter values of the navigation system into the observability matrix Q to obtain the numerical value form of the observability matrix;
as can be seen from step 1, the observability matrix Q obtained in this embodiment is very complex in form, and it is difficult to directly transform it to obtain the specific form of the decoupling equation. Therefore, in the step, the known parameter values of the navigation system are substituted into the observability matrix Q to obtain the numerical form of the observability matrix Q, and the state variable decoupling is realized through numerical calculation.
In this embodiment, the sensor parameter values and the navigation state of the aircraft during the initial alignment of the SINS are set as follows:
the initial position of the aircraft is [39 degrees 120-50 m ]]TThe speed and attitude of the aircraft are respectively Vb＝[0 1 0]TAnd [0 DEG 60 ]]TThe SINS initial alignment is performed at uniform velocity straight line 1200s, and the parameters of the SINS and DVL used are shown in table 1. The initial value of the attitude misalignment angle is set to [0.2 DEG 2]T。
TABLE 1 sensor accuracy parameters
Figure BDA0002304982090000101
And (7) substituting the navigation system parameter values given above into the value form (7) to obtain the numerical form of Q.
And step 3: calculating the rank of the observable matrix Q to obtain rank (Q) ═ r, where r is the number of linearly independent rows in the observable matrix Q;
the rank of the observability matrix Q obtained in step 2 in this embodiment is calculated to obtain rank (Q) ═ 9, and it can be known that the observability matrix Q includes 9 linearly independent rows, that is, 9 equations can be used for state estimation. Therefore, only 9-dimensional states at most in the 19-dimensional states of the system can be accurately estimated under the uniform linear motion track.
And 4, step 4: r linear independent rows are selected from the observable matrix Q to construct a matrix Q1Using a matrix Q1Performing observability analysis instead of the observability matrix Q;
in this step, there may be multiple selection schemes for selecting r linear independent rows from the observability matrix Q, so that the matrix Q1Is to ensure the matrix Q1The first r-1 singular values of (a) are equal to the first r-1 singular values of an observability matrix Q, the matrix Q1R th ofThe absolute value of the difference between the singular value and the r-th singular value of the observability matrix Q is less than a set value, thereby making the matrix Q1Observability analysis may be performed instead of the observability matrix Q. In the present embodiment, the set value is set to 0.01. In the embodiment, the 1 st to 8 th rows and the 11 th row of the observability matrix Q in the step 2 are selected to form the matrix Q1The matrix Q1The singular value pair ratio with the observability matrix Q is shown in table 2. As can be seen from Table 2, the constructed matrix Q1Is substantially the same as the singular value of the observability matrix Q, satisfies the set value, so that the matrix Q may be used hereinafter1Observability analysis is performed instead of the observability matrix Q.
TABLE 2 singular value comparison
Figure BDA0002304982090000111
And 5: according to the priori knowledge of the navigation system, p (p is less than or equal to r) key state variables are selected for decoupling.
The step determines a decoupling variable by using prior knowledge of a system, and avoids selecting an order of magnitude smaller than 10 when selecting a decoupling state variable-3To avoid introducing large calculation errors in the numerical calculation.
In this embodiment, the specific implementation process of this step is as follows: for the SINS initial alignment system, the attitude misalignment angle is the most critical state variable, and furthermore, under the moving-base condition, the position and the velocity are important navigation parameters, so the position error and the velocity error are also the system critical state variables. However, since the position error is the integral of the velocity error in the error equation of the SINS, when the external observation information of the system is the velocity (as in this embodiment), the velocity error and its respective differentials are hardly reflected in the position error, which results in that the elements in the first three columns of the observability matrix Q are all zero or minimal values, and if the position error is decoupled, a large calculation error is introduced. Therefore, the position error is not selected as the decoupling state, and only the speed error and the attitude misalignment angle are selected as the decoupling state. Meanwhile, the rest state variables of the system are sensor errors, and the values of the state variables are generally considered to be kept unchanged or changed very little in the one-time starting working process of the system, so that the state variables do not change along with the filtering period. In the present embodiment, nominal values for these state variables have been given in table 1. As can be seen from table 1, most of them are very small in magnitude, and if they are selected as the key state for decoupling, a large calculation error is introduced, so they are not selected as the decoupling state, but their nominal values are used as the system prior knowledge to calculate the estimation accuracy of other decoupling states. Based on the above analysis, the present embodiment selects the three-dimensional velocity error and the three-dimensional attitude misalignment angle as the decoupling state variables, i.e., p is equal to 6. As can be seen from the above analysis process, in the present embodiment, prior knowledge of the system, such as characteristics of a position error and a sensor error, is used when the decoupling state is selected.
Step 6: constructing a row transformation matrix T, Q1The left multiplication row transformation matrix T realizes state decoupling to obtain a matrix U, namely U is equal to TQ1；
In the step, the row transformation matrix T is used for obtaining a matrix U in a specific form, the form of the matrix U is directly related to the state variables selected in the step 5, the step 5 selects p state variables from the state vector X for decoupling, each row of p rows of elements corresponding to the p state variables in the matrix U has an element 1, the rest elements are 0, the 1 element in each row is located in different rows of the matrix U, and the row sequence number of the 1 element is required to be increased along with the increase of the sequence numbers of the p state variables in the state vector X; during a specific row conversion process, the matrix Q is aligned1Gradually implementing a plurality of times of primary row transformation to finally reach the form of a matrix U; therefore, the row transformation matrix T is obtained by multiplying a series of elementary row transformation matrices, and the final form thereof can be represented by T ═ UQ1 -1And (6) obtaining.
In this embodiment, the matrix Q obtained in step 4 is subjected to1The specific form of the matrix U obtained by implementing the row transformation is shown as the formula (8)
Figure BDA0002304982090000121
It can be found that the 4 th to 9 th columns of the matrix U are composed of a 3 × 3 dimensional zero matrix and a 6 × 6 dimensional identity matrix, and satisfy the requirement that each column has only one 1 element, and each column has 1 element in different rows, and the row number increases with the increase of the state variable number. After the matrix U is multiplied by the state vector X, the matrix Q can be made1The equations corresponding to the first 3 linearly independent rows no longer contain three-dimensional velocity errors and three-dimensional attitude misalignment angles, and the matrix Q1The equations corresponding to the last 6 linearly independent rows all contain only 1 of the 6 state variables, so that the decoupling of the 6 state variables is realized. (8) In the formula, P1 to P4 represent the row-transformed submatrix block, and the specific value of each element is a coefficient corresponding to the state variable in the formulas (10) and (11). Taking P1 as an example, each element corresponds to the coefficient of δ L, δ λ, δ h in formula (11), so the element in the first row and the first column is the coefficient of δ L in the first equation of formula (11), i.e., -9.4 × 10-8(ii) a The P1 first row second column element is the coefficient of δ λ in the equation (11) second equation, where the equation (11) first equation does not contain the term δ λ, since the coefficient of δ λ is 0, i.e., the value of P1 first row second column element is 0. The values of the other elements P1-P4 are not described in detail.
And 7: the system state vector X and the observation vector Z have a relation Y ═ QX, the vector Y is composed of the observation vector Z and the differential of each time of the observation vector Z, there are
Figure BDA0002304982090000122
n is the dimension of the system; selecting the constructed matrix Q in the step 4 from the vector Y1The corresponding row of time constitutes the vector Y1Having Y of1＝Q1X, the two sides of which are left-multiplied by a row transformation matrix T to obtain TY1The equation is a decoupled state variable relational equation, and equations corresponding to p decoupling state variables can be obtained from the equation;
in this embodiment, corresponding to step 4, lines 1-8 and 11 of vector Y are selected to form vector Y1In the relation Y1＝Q1X two-terminal left-multiplication transformation matrix T, having
TY1＝UX (9)
Equation (9) is the decoupled state variable relationship, which contains 9 equations corresponding to 9 linearly independent rows of the observability matrix. Wherein, the decoupled three-dimensional speed error and the three-dimensional attitude misalignment angle correspond to the 4 th to 9 th lines of the formula (9), and the specific form is
Figure BDA0002304982090000131
(10) The equation is the equation corresponding to the 6 decoupling state variables in this embodiment, where Y is4-Y9As a vector TY1Lines 4-9. (9) The other 3 equations of formula are given in formula (11). It can be seen that after the three-dimensional velocity error and the three-dimensional attitude misalignment angle are decoupled, they exist in the corresponding 1 equation respectively, and the other 8 equations in the equation (9) will not contain the 6 state variables any more. Therefore, when the observability analysis is carried out on the system by using the formula (9), 6 decoupling states are not influenced mutually any more, so that the analysis process is more intuitive. This is to put the matrix Q in step 61The matrix U shown in the formula (8) is constructed through line transformation.
And 8: judging whether a certain state variable or some state variables can be estimated through the remaining r-p equations in the TY-UX;
(10) the equation gives 6 equations for 6 decoupling states, from which it can be seen that since the decoupled velocity error and attitude misalignment angle can only be estimated by one of their corresponding equations, their accuracy of estimation is determined by the coupling terms in their corresponding equations. With east speed error delta VEFor the sake of example, it corresponds to the first equation of the formula (10), where δ V is no longer presentEAppear, so the filter algorithm is on δ VECan only be realized by this equation, where there is also an
Figure BDA0002304982090000141
εx、εy、εz、δbbx、δbby、δbbz、δsDδ L, δ h 12 state coupling terms which determine uniform speedDelta V under linear motion trackEThe accuracy of the estimation of. Similarly, the estimation accuracy of the other 5 decoupling states is also determined by the coupling terms in their corresponding equations.
(9) There are 9 equations corresponding to the 9 linearly independent rows in the observability matrix Q, and (10) contains only 6 equations corresponding to the velocity error and attitude misalignment angle, and there are 3 equations in (9) that can be used for state estimation, and if one or some of the un-decoupled states can be accurately estimated by these 3 equations, their coupling terms will not affect the estimation of other state variables, i.e., they can be removed from (10). In this embodiment, the specific form of the other 3 equations in the formula (9) is
Figure BDA0002304982090000151
By substituting the sensor parameter values given in Table 1 into equation (11), the terms with smaller orders in the equation are omitted, and the sensor parameter values can be obtained
Figure BDA0002304982090000152
(12) Equations omit orders of magnitude smaller terms than (11), making the relationships between state variables more intuitive. It should be particularly noted that, unlike sensor errors, the values of the two state variables δ L and δ h change continuously during the filtering estimation process and cannot be obtained by the system, but the coupling terms of the two states are omitted in equation (12), because the coefficients of δ L and δ h are very small in order of magnitude in equation (11) and the coupling terms of δ L and δ h themselves are small in order of magnitude according to a priori knowledge of the system, so that the coupling terms can be removed. As can be seen from the formula (12),
Figure BDA0002304982090000153
is dominant in the third expression of the formula (12), so
Figure BDA0002304982090000154
Can be estimated accurately by this equation. And front of the formula (12)Two expressions cannot estimate other state variables because there are terms in the two expressions that are orders of magnitude close and there are no dominant states. Taking the first equation of formula (12) as an example, include
Figure BDA0002304982090000155
εx、εy、εzThe 6 coupling terms of the same order of magnitude cannot be truncated, so the equation cannot accurately estimate any of the 6 state variables.
And step 9: removing the coupling terms of the state variables determined in the step 8 from the p equations selected in the step 7;
due to the fact that
Figure BDA0002304982090000162
Can be accurately estimated by equation (12), so 6 equations of equation (10) are eliminated
Figure BDA0002304982090000163
Is coupled term of to obtain
Figure BDA0002304982090000161
Step 10: and (4) according to the p equations obtained in the step (9), and by combining the prior knowledge of the navigation system, judging which states can be accurately estimated, and calculating the estimation precision, thereby completing the observability analysis of the system.
Equation (13) is obtained through step 9, and the remaining coupling terms in equation (13) are factors that restrict the accuracy of the velocity error and attitude misalignment angle estimation, and whether the velocity error and attitude misalignment angle can be accurately estimated can be calculated through these coupling terms. In order to more intuitively represent the correlation between the state variables, the sensor parameter values given in Table 1 are also substituted into equation (13), and the terms with smaller orders in the equations are omitted to obtain
Figure BDA0002304982090000171
It can be seen that: delta VEAnd δ VNThe estimation precision of the method is mainly determined by constant drift of two horizontal gyros, constant error of DVL horizontal velocity and scale factor error; delta VUThe estimation accuracy of (d) is mainly determined by the constant error of the z-axis velocity of the DVL; phi is axAnd phiyThe estimation precision of the accelerometer is mainly determined by the zero offset of the constant values of the two horizontal accelerometers; phi is azThe estimation accuracy of (a) is mainly determined by the constant drift of the two horizontal gyros.
Next, it is determined whether the velocity error and attitude misalignment angle can be accurately estimated using equation (14), and using the first equation of equation (14) as an example, ε is given in Table 1x、εy、δbbx、δbbyAnd δ sDSubstituting the parameter value of (a) into the formula to obtain delta VEThe sum of all coupling terms of (a) is-4.3 x 10-3I.e. these coupling terms result in δ VEThe estimated value of (A) is present-4.3X 10-3Error of (d), this error being compared with δ VEIs small, so that δ V is judgedECan be accurately estimated by this equation. Similarly, δ V is obtained by the second and third equations of equation (14)NAnd δ VUThe sum of the coupling terms is 1.5 × 10-3m/s and 2X 10-3m/s, can determine delta VNAnd δ VUCan be accurately estimated. The initial alignment system is most concerned with the accuracy of the estimate of the attitude misalignment angle, and the sum of the 3 misalignment angle coupling terms, which can be calculated from the last 3 equations of equation (14), is 0.002 °, -0.008 °, and-0.065 °, respectively, and these errors are small compared to the true values of the corresponding state variables, so it can be assumed that the 3 attitude misalignment angles can be accurately estimated.
Through the above analysis, it was determined that 7 states could be accurately estimated, respectively: three-dimensional velocity error, three-dimensional attitude misalignment angle and z-axis plus table zero offset. Fig. 2 and fig. 3 show the estimation results of the kalman filter, and it can be seen from the diagrams that the simulation result is consistent with the observability analysis result of this embodiment, only three-dimensional velocity errors exist among 19 state variables, the three-dimensional attitude misalignment angle and the z-axis plus table zero offset 7 state variables are accurately estimated, and the filtering estimation precision of the 7 state variables is substantially consistent with the value of the coupling term calculated by the present invention, which verifies that the present invention can judge the observability of each state variable of the navigation system before the filtering estimation, and even predict the filtering estimation precision.
It should be noted that, the present invention needs to have a certain priori knowledge about the system status, and the acquisition of the priori knowledge may be the knowledge about the system during the use or test process, or may be a parameter index queried through an instrument and device data manual. For a system with neither knowledge nor data manual, the observability of each state of the system can be obtained first by using an observability analysis method based on singular values (the method is an existing method) as a priori knowledge for reference. In addition, when the numerical calculation such as parameter value substitution, matrix operation and the like is carried out, software such as Matlab and the like can be used as a mathematic auxiliary tool, so that the method has strong engineering practicability.
The above description is only an embodiment of the present invention, but the scope of the present invention is not limited thereto, and any changes or substitutions that can be easily conceived by those skilled in the art within the scope of the present invention should be covered by the present invention.

Claims (4)

1. A method of numerical navigation system observability analysis, comprising the steps of:
step 1: establishing a state equation and a measurement equation of a navigation system, and constructing an observability matrix Q of the system;
step 2: substituting the known parameter values of the navigation system into the observability matrix Q to obtain the numerical form of the observability matrix Q;
and step 3: calculating the rank of the observable matrix Q to obtain rank (Q) ═ r, where r is the number of linearly independent rows in the observable matrix Q;
and 4, step 4: r linear independent rows are selected from the observable matrix Q to construct a matrix Q1Using a matrix Q1Performing observability analysis instead of the observability matrix Q;
and 5: selecting p (p is less than or equal to r) key state variables for decoupling according to prior knowledge of a navigation system;
step 6: constructing a row transformation matrix T, Q1The left multiplication row transformation matrix T realizes state decoupling to obtain a matrix U, namely U is equal to TQ1；
And 7: the system state vector X and the observation vector Z have a relation Y ═ QX, the vector Y is composed of the observation vector Z and the differential of each time of the observation vector Z, there are
Figure FDA0002304982080000011
n is the dimension of the system; selecting the constructed matrix Q in the step 4 from the vector Y1Corresponding row construction vector Y of time1Having Y of1＝Q1X, the two sides of which are left-multiplied by a row transformation matrix T to obtain TY1The equation is a decoupled state variable relational equation, and equations corresponding to p decoupling state variables can be obtained from the equation;
and 8: judging whether a certain state variable or some state variables can be estimated through the remaining r-p equations in the TY-UX;
and step 9: removing the coupling terms of the estimable state variables determined in the step 8 from the p equations selected in the step 7;
step 10: and (4) according to the p equations obtained in the step (9), judging which states can be accurately estimated by combining the prior knowledge of the navigation system, calculating the estimation precision, and finishing the observability analysis of the system.
2. A navigation system observability numerical analysis method according to claim 1, wherein:
in step 1, the observability matrix Q refers to observability matrix Q of linear constancy system or total observability matrix Q of piecewise linear constancy systemT。
3. A navigation system observability numerical analysis method according to claim 1, wherein:
in step 4, the matrix Q1Is to ensure momentsArray Q1The first r-1 singular values of (a) are equal to the first r-1 singular values of an observability matrix Q, the matrix Q1Is less than a set value and the absolute value of the difference between the r-th singular value of the observability matrix Q and the r-th singular value of the observability matrix Q is less than the set value.
4. A navigation system observability numerical analysis method according to claim 1, wherein: in step 5, when the decoupling state variable is selected, the selection order of magnitude smaller than 10 is avoided-3The state variable of (2) is used for avoiding a mathematical assistant tool from introducing large calculation errors in the numerical calculation process.
CN201911238453.7A 2019-12-05 2019-12-05 Observability numerical analysis method for navigation system Active CN111024083B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911238453.7A CN111024083B (en) 2019-12-05 2019-12-05 Observability numerical analysis method for navigation system

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911238453.7A CN111024083B (en) 2019-12-05 2019-12-05 Observability numerical analysis method for navigation system

Publications (2)

Publication Number Publication Date
CN111024083A true CN111024083A (en) 2020-04-17
CN111024083B CN111024083B (en) 2023-03-28

Family

ID=70208023

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911238453.7A Active CN111024083B (en) 2019-12-05 2019-12-05 Observability numerical analysis method for navigation system

Country Status (1)

Country Link
CN (1) CN111024083B (en)

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2013080183A1 (en) * 2011-11-30 2013-06-06 Applanix Corporation A quasi tightly coupled gnss-ins integration process
JP2015141159A (en) * 2014-01-30 2015-08-03 三菱電機株式会社 Image navigation device, satellite and image navigation method
CN104820758A (en) * 2015-05-20 2015-08-05 江苏华豪航海电器有限公司 Method for analyzing observability of transfer alignment accuracy evaluation based on singular value decomposition
CN108759867A (en) * 2018-06-01 2018-11-06 长光卫星技术有限公司 Extraneous aided inertial navigation system moving alignment Observability Analysis method

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2013080183A1 (en) * 2011-11-30 2013-06-06 Applanix Corporation A quasi tightly coupled gnss-ins integration process
JP2015141159A (en) * 2014-01-30 2015-08-03 三菱電機株式会社 Image navigation device, satellite and image navigation method
CN104820758A (en) * 2015-05-20 2015-08-05 江苏华豪航海电器有限公司 Method for analyzing observability of transfer alignment accuracy evaluation based on singular value decomposition
CN108759867A (en) * 2018-06-01 2018-11-06 长光卫星技术有限公司 Extraneous aided inertial navigation system moving alignment Observability Analysis method

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
帅平 \N\N\N,陈定昌\N\N\N,江涌: "GPS/SINS组合导航系统状态的可观测度分析方法" *

Also Published As

Publication number Publication date
CN111024083B (en) 2023-03-28

Similar Documents

Publication Publication Date Title
CN108759798B (en) Method for realizing precision measurement of high-precision spacecraft
CN105806363B (en) The underwater large misalignment angle alignment methods of SINS/DVL based on SRQKF
CN107690567A (en) The method being tracked using extended Kalman filter for the navigation to mobile vehicle equipment
Schopp et al. Self-calibration of accelerometer arrays
CN110632634B (en) Optimal data fusion method suitable for ballistic missile INS/CNS/GNSS combined navigation system
CN108508463B (en) Fourier-Hermite orthogonal polynomial based extended ellipsoid collective filtering method
CN114777810A (en) Strapdown inertial navigation system-level calibration method based on matrix decomposition
Wang et al. An adaptive multiple backtracking UKF method based on Krein space theory for marine vehicles alignment process
Sanyal Optimal attitude estimation and filtering without using local coordinates part i: Uncontrolled and deterministic attitude dynamics
CN111024083B (en) Observability numerical analysis method for navigation system
Patera Attitude propagation for a slewing angular rate vector
CN117191057A (en) Navigation platform construction method based on space-time registration and multimode vector allocation fusion
CN108871322A (en) Based on the matched model-free deformation of hull measurement method of attitude angle
Mao et al. Strapdown inertial navigation algorithms based on lie group
CN113252029B (en) Astronomical navigation attitude transfer method based on optical gyroscope measurement information
CN112304309B (en) Method for calculating combined navigation information of hypersonic vehicles based on cardiac array
CN112697145A (en) Master-slave type multi-underwater unmanned submersible vehicle cooperative positioning method based on CKF
CN113587926A (en) Spacecraft space autonomous rendezvous and docking relative navigation method
CN111473786A (en) Two-layer distributed multi-sensor combined navigation filtering method based on local feedback
CN112013849A (en) Autonomous positioning method and system for surface ship
Belsky et al. Estimation of Generic Parameters in a Technique for Initial Alignment and Calibration of INS for Space Launch Vehicles
Vodicheva et al. Error analysis technique for indirect method of calibration of a strapdown inertial measurement unit
Xu et al. An Improved Inertial Matching Algorithm for Hull Deformation With Large Misalignment Angle
CN108981752B (en) Transfer alignment method, system and storage medium based on sub-inertial set information cooperation
Pool et al. Optimal Reconstruction of Flight Simulator Motion Cues using Extended Kalman Filtering

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant