CN117057159A - Tricycle motion model-based state estimation method under periodic scheduling protocol - Google Patents

Tricycle motion model-based state estimation method under periodic scheduling protocol Download PDF

Info

Publication number
CN117057159A
CN117057159A CN202311169570.9A CN202311169570A CN117057159A CN 117057159 A CN117057159 A CN 117057159A CN 202311169570 A CN202311169570 A CN 202311169570A CN 117057159 A CN117057159 A CN 117057159A
Authority
CN
China
Prior art keywords
tricycle
motion model
time
moment
matrix
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
CN202311169570.9A
Other languages
Chinese (zh)
Other versions
CN117057159B (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.)
Harbin University of Science and Technology
Original Assignee
Harbin University of Science and Technology
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 Harbin University of Science and Technology filed Critical Harbin University of Science and Technology
Priority to CN202311169570.9A priority Critical patent/CN117057159B/en
Publication of CN117057159A publication Critical patent/CN117057159A/en
Application granted granted Critical
Publication of CN117057159B publication Critical patent/CN117057159B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F30/00—Computer-aided design [CAD]
    • G06F30/20—Design optimisation, verification or simulation
    • 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
    • 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/15—Correlation function computation including computation of convolution operations
    • 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
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04L—TRANSMISSION OF DIGITAL INFORMATION, e.g. TELEGRAPHIC COMMUNICATION
    • H04L67/00—Network arrangements or protocols for supporting network services or applications
    • H04L67/01—Protocols
    • H04L67/12—Protocols specially adapted for proprietary or special-purpose networking environments, e.g. medical networks, sensor networks, networks in vehicles or remote metering networks

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Mathematical Physics (AREA)
  • Theoretical Computer Science (AREA)
  • Data Mining & Analysis (AREA)
  • Pure & Applied Mathematics (AREA)
  • Computational Mathematics (AREA)
  • Mathematical Analysis (AREA)
  • Mathematical Optimization (AREA)
  • General Engineering & Computer Science (AREA)
  • Computing Systems (AREA)
  • Databases & Information Systems (AREA)
  • Software Systems (AREA)
  • Algebra (AREA)
  • Computer Hardware Design (AREA)
  • Evolutionary Computation (AREA)
  • Geometry (AREA)
  • Operations Research (AREA)
  • Health & Medical Sciences (AREA)
  • General Health & Medical Sciences (AREA)
  • Medical Informatics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Signal Processing (AREA)
  • Management, Administration, Business Operations System, And Electronic Commerce (AREA)

Abstract

The invention discloses a state estimation method based on a tricycle motion model under a periodic scheduling protocol, which comprises the following steps: 1. establishing a tricycle motion model with measurement deletion; 2. designing a state estimator according to a tricycle motion model; 3. calculating the upper limit lambda of one-step prediction error covariance matrix of tricycle motion model at the s-th moment s+1|s The method comprises the steps of carrying out a first treatment on the surface of the 4. Calculating an estimated iterative correction matrix K of the tricycle motion model at the s+1th moment s+1 The method comprises the steps of carrying out a first treatment on the surface of the 5. Will K s+1 Obtaining an estimate of the s+1 time instant in substitution twoJudging whether s+1 reaches the estimated total duration U, and executing six if s+1 is less than U; 6. calculating the upper bound lambda of the estimated error covariance matrix of the tricycle motion model at the s+1st moment s+1|s+1 The method comprises the steps of carrying out a first treatment on the surface of the Let s=s+1, perform two until the stop condition s+1=u is reached. The invention solves the problem that the existing state estimation method can not process the nonlinear state estimation with measurement deletion under the periodic scheduling protocol.

Description

Tricycle motion model-based state estimation method under periodic scheduling protocol
Technical Field
The invention relates to a state estimation method, in particular to a state estimation method based on a tricycle motion model under a periodic scheduling protocol.
Background
The tricycle motion model is a mathematical model describing the steering relationship between the front wheels and the rear axle of the tricycle. The model is based on the ackerman steering principle, is used for predicting and controlling the steering angle of the front wheels when the vehicle turns, and is widely used in a plurality of application fields.
Considering the large number of uses of low cost sensors in actual engineering, measurement deletions often occur in various applications. In addition, in order to improve the transmission efficiency of data and reduce the network bandwidth, communication protocols are receiving attention because of their important roles in data transmission. The periodic scheduling protocol can remarkably improve the utilization efficiency and reliability of data. Therefore, it is extremely interesting to study the estimation method of the tricycle motion model with measurement deletion under the periodic scheduling protocol.
The current existing estimation method can not solve the problem of state estimation of the tricycle motion model with measurement deletion under the periodic scheduling protocol.
Disclosure of Invention
The invention provides a state estimation method based on a tricycle motion model under a periodic scheduling protocol, which aims to solve the state estimation problem with measurement deletion and other phenomena based on the tricycle motion model under the periodic scheduling protocol.
The invention aims at realizing the following technical scheme:
a state estimation method based on a tricycle motion model under a periodic scheduling protocol comprises the following steps:
step one, based on five-dimensional state variables consisting of vehicle coordinates, vehicle path angle deviation, linear speed and steering angle, establishing a tricycle motion model with measurement deletion, wherein the tricycle motion model with measurement deletion is as follows:
wherein x is s ＝[λ s β s η s μ s ξ s ] T The superscript T denotes a transpose; sin (-), cos (-) and tan (-) represent the sine function, cosine function and tangent function of "(") respectively; lambda (lambda) s 、β s 、η s 、μ s And xi s Respectively representing the original coordinates of the vehicle, the vehicle path angle deviation, the linear speed and the vehicle steering angle of the tricycle motion model at the s-th moment; Δh is the sampling interval of the model, P represents the distance from the front wheel to the rear axle of the tricycle, x b And y b Measuring a beacon position on behalf of a sensor installed in the vehicle; omega s Represents zero-mean Gaussian process white noise at time s and the variance is Q s ＞0，v s White noise is measured for zero-mean Gaussian at time s and variance is R s ＞0，D s Is a known time-varying noise distribution matrix having five dimensions;is the original measurement output;is the output vector after deletion, y s ＝diag{γ 1,s ,γ 2,s ,…,γ 5,s (where diag represents a diagonal matrix, γ) m , s (m=1, 2, …, 5) is a bernoulli random variable for judging whether or not the measured value of the mth node at the mth time is subject to deletion, and satisfies +.>P (-) represents the probability that "·" occurs,for the probability of no deletion of the mth node at the s-th moment, the xi (·) is a distribution function of standard normal distribution, and τ m (m=1, 2,.,. 5) is +.>Is the mth component of->Representing a measurement erasure threshold vector,>for one-step prediction of the state of the tricycle movement model at time s-1, +.>Is->Is the mth component of->In a one-step prediction form of a nonlinear function corresponding to measurement output based on a tricycle motion model at the s-1 time,for the measurement noise covariance matrix R at the s-th moment s I is a five-dimensional identity matrix;
step two, designing a state estimator according to the tricycle motion model constructed in the step one under a periodic scheduling protocol, and carrying out state estimation on the tricycle motion model through a state estimator equation, wherein the specific structure of the state estimator is as follows:
in the method, in the process of the invention,for one-step prediction of the state of the movement model of the tricycle at time s,/for the time of day>For the estimation of the s-th moment of the state of the tricycle movement model, a>For the estimate at time s+1, +.>Nonlinear function of state based on tricycle motion model +.>Estimate at time s, +.>K is a one-step prediction form of a nonlinear function corresponding to measurement output based on a tricycle motion model at the s-th moment s+1 Iterative correction matrix for the estimation at time s+1, y s+1 For the actual measured output of the tricycle movement model at time s+1, < >>α s+1 =mod (s, 5) +1 denotes the sensor node used for transmitting the measurement data at time s+1, mod (s, 5) being defined as the remainder of the integer s divided by the positive integer 5, +.>Represents the Croneck function with values 0 and 1,/for> Is the probability that the mth node has not been deleted at time s +1, wherein φ (·) is the probability density function of a standard normal distribution, ++> Measurement noise covariance matrix R for s+1st moment s+1 The (m, m) th element of (c),representation pair->The transposition process is carried out, the image is processed, is->The m-th element of (2);
step three, calculating an upper bound lambda of a one-step prediction error covariance matrix of the tricycle motion model at the s-th moment s+1|s The upper limit lambda of the one-step prediction error covariance matrix of the tricycle motion model at the s-th moment s+1|s The expression is:
wherein the superscript T represents the transpose of the matrix, the superscript-1 represents the inverse of the matrix or value,nonlinear function of the corresponding state of the tricycle motion model at the s-th moment +.>At->Partial derivative at, Λ s|s For the upper bound of the estimated error covariance matrix at time s,/>And->Based on a non-linear function->Error term obtained using Taylor expansion, < ->Represents a known time-varying coefficient greater than 0 and satisfies the condition +.>
Step four, utilizing the lambda obtained in the step three s+1|s Calculating an estimated iterative correction matrix K of the tricycle motion model at the (s+1) th moment s+1 Estimating iterative correction matrix K of tricycle motion model at s+1th moment s+1 The expression of (2) is:
wherein:
wherein, Γ s+1 As an intermediate parameter matrix, p m,s+1 (m=1, 2, 3) are positive weighting coefficients,for a nonlinear function at time s+1 corresponding to the measured output based on the tricycle motion model, +.>Is the state of the tricycle motion modelOne-step prediction at time s, ε s+1 Is->At->Partial derivative of>Is a positive scalar and satisfies the condition +.> And +.>By->Error matrix, Λ, obtained using taylor formula s+1|s For the upper bound of the one-step prediction error covariance matrix at the s-th moment, "°" represents the Hadamard product between the matrices, tr {. Cndot. } represents the trace operation of the matrices, ρ 1,s+1 ρ is 2,s+1 Is satisfied->Is used for the normal number of (a), II indicates Euclidean norm of ", and->And->Respectively represent ρ 1,s+1 And ρ 2,s+1 Square value of (2);
step five, utilizing the estimated iterative correction matrix K obtained in the step four s+1 Substituting the same into the second step to obtain an estimate of the (s+1) th momentThen judging whether s+1 reaches the estimated total duration U, if s+1 is less than U, executing step six, and if s+1=U, stopping the state estimation of the tricycle motion model with measurement deletion under the periodic scheduling protocol;
step six, utilizing the estimated iterative correction matrix K calculated in the step four s+1 Calculating the upper limit lambda of the estimated error covariance matrix of the tricycle motion model at the s+1 time s+1|s+1 The method comprises the steps of carrying out a first treatment on the surface of the Let s=s+1, execute step two until the stop condition s+1=u is reached, estimate the upper bound Λ of the error covariance matrix s+1|s+1 The calculation formula of (2) is as follows:
wherein:
wherein, lambda s+1|s+1 Is the upper bound of the estimated error covariance matrix at time s+1.
Compared with the prior art, the invention has the following advantages:
1. the invention provides a state estimation method based on a tricycle motion model under a periodic scheduling protocol. Compared with the state estimation method under the conventional periodic scheduling protocol, the method adopts a recursive method to process the nonlinear state estimation problem with measurement deletion and the periodic scheduling protocol, can realize the advantage of easy solution, and solves the nonlinear state estimation problem that the conventional state estimation method cannot process the measurement deletion under the periodic scheduling protocol.
2. The invention obtains the upper bound of the estimation error covariance matrix by considering the effective information of the estimation error covariance by means of matrix theory, and designs proper estimation iteration gain to minimize the trace of the upper bound of the estimation error covariance matrix. These methods ensure that estimation errors are minimized and good estimation performance is maintained in the event that measurement erasure and periodic scheduling protocols occur simultaneously. In the experiment of the invention, aiming at the tricycle motion model with the deletion measurement under the periodic scheduling protocol, the accuracy ratio of the state estimation method provided by the invention is improved by about 70 percent compared with that of the extended Kalman filtering method.
3. The state estimation algorithm of the invention can accurately estimate the state information of the tricycle motion model.
Drawings
FIG. 1 is a flow chart of a state estimation method based on a tricycle motion model under a cyclic scheduling protocol proposed by the present invention;
FIG. 2 is a true state of a tricycle motion modelAnd its estimation under periodic scheduling protocol>Trajectory comparison graph of (2);
FIG. 3 is a true state of a tricycle motion modelAnd its estimation under periodic scheduling protocol>Trajectory comparison graph of (2);
FIG. 4 is a true state of a tricycle motion modelAnd its estimation under periodic scheduling protocol>Trajectory comparison graph of (2);
FIG. 5 is a true state of a tricycle motion modelAnd its estimation under periodic scheduling protocol>Trajectory comparison graph of (2);
FIG. 6 is a true state of a tricycle motion modelAnd its estimation under periodic scheduling protocol>Trajectory comparison graph of (2);
FIG. 7 is a log of the mean square error MSE (MSE) and the upper bound tr of the estimation error covariance matrix (Λ) s|s ) Log of trace (tr (Λ) s|s ) A) a relationship diagram;
fig. 8 is a graph comparing the MSE of case I (state estimation method proposed by the present invention) with the MSE of case II (extended kalman filter method).
Detailed Description
The following description of the present invention is provided with reference to the accompanying drawings, but is not limited to the following description, and any modifications or equivalent substitutions of the present invention should be included in the scope of the present invention without departing from the spirit and scope of the present invention.
The invention provides a state estimation method based on a tricycle motion model under a periodic scheduling protocol, as shown in fig. 1, the method comprises the following steps:
the first step, the state parameters are respectively defined as vehicle coordinates, vehicle path angle deviation, linear speed and vehicle steering angle, and a tricycle motion model with measurement deletion is established based on the parameters.
In the step, the established tricycle motion model is as follows:
wherein x is s ＝[λ s β s η s μ s ξ s ] T The superscript T denotes a transpose; sin (-), cos (-) and tan (-) represent the sine function, cosine function and tangent function of "(") respectively; lambda (lambda) s 、β s 、η s 、μ s And xi (xi) s Respectively representing the original coordinates of the vehicle, the vehicle path angle deviation, the linear speed and the vehicle steering angle of the tricycle motion model at the s-th moment; Δh is the sampling interval of the model, P represents the distance from the front wheel to the rear axle of the tricycle, x b And y b Measuring a beacon position on behalf of a sensor installed in the vehicle; omega s Represents zero-mean Gaussian process white noise at time s and the variance is Q s ＞0，v s White noise is measured for zero-mean Gaussian at time s and variance is R s ＞0，D s Is a known time-varying noise distribution matrix having five dimensions,is the raw measurement output.
Due to the existence of measurement erasure, we introduce an observable vectorTo represent the measured output after suffering from the erasure.
Definition of the definitionAndwherein superscript T stands forWatch movement, meter (R)/(B)>Representing a measurement erasure threshold vector,>represents the original measurement of the mth node at the s-th moment,/or->Representing the measured output after the mth node suffered a erasure at time s.
The well-known Tobit first model can be derived:
wherein τ m (m=1, 2,., 5) represents a known deletion threshold for the mth node.
Next, for judgmentWhether or not deleted, we introduce a set of mutually independent Bernoulli random variables γ m , s (m=1, 2,.,. 5) to manage the deletion phenomenon, it can be expressed as follows:
γ m,s has the following probability distribution:
wherein P (·) represents the probability of occurrence of "·",representing the probability that the mth node has not been deleted at the s-th timeThe rate.
To facilitate the subsequent derivation and some a priori statistical properties, we willThe approximation is:
wherein, the xi (·) is a distribution function of standard normal distribution, τ m Is thatIs the mth component of->For one-step prediction of the state of the tricycle movement model at time s-1, +.>Is->Is the mth component of->In a one-step prediction form of a nonlinear function corresponding to measurement output based on a tricycle motion model at the s-1 time,for the measurement noise covariance matrix R at the s-th moment s (m, m) th element of (c).
From the formulas (3) and (4):
make gamma s ＝diag{γ 1,s ,γ 2,s ,...,γ 5,s Sum ofWhere diag represents the diagonal matrix. Then the equation (7) can be expressed as:
wherein I is a five-dimensional identity matrix.
And step two, designing a state estimator according to the tricycle motion model constructed in the step one under a periodic scheduling protocol, and carrying out state estimation on the tricycle motion model through a state estimator equation. The method comprises the following specific steps:
in order to reduce network bandwidth, a periodic scheduling protocol is introduced to schedule the sensor nodes so as to achieve the aim of improving data transmission efficiency.
Variable alpha s E {1, 2..5 } represents the sensor node at time s for transmitting measurement data, then α s Expressed as:
α s ＝mod(s-1,5)+1 (9)
where mod (s-1, 5) is defined as the remainder of the integer s-1 divided by the positive integer 5.
Definition y s ＝[y 1,s y 2,s ... y 5,s ] T Is thatThe actual measurement values produced after the periodic scheduling protocol, where the superscript T represents the transpose. By combining with zero input strategy, the measured value y of the mth node at the s-th moment m,s (m=1, 2,., 5) can be expressed as:
next, we introduceWherein->Representing a kronecker function with values 0 and 1. The formula (10) can be expressed as:
step two, facilitate the theoretical deduction of follow-up, the invention introduces the following definitions:
wherein,is a nonlinear function based on the state of the tricycle motion model, +.>Is a nonlinear function of the measurement based on the tricycle motion model.
Based on the available measurement information, the following state estimator is designed:
in the method, in the process of the invention,for one-step prediction of the state of the movement model of the tricycle at time s,/for the time of day>For the movement of tricyclesEstimate of the s-th moment of the model state, +.>For the estimate at time s+1, +.>Nonlinear function of state based on tricycle motion model +.>Estimate at time s, +.>K is a one-step prediction form of a nonlinear function corresponding to measurement output based on a tricycle motion model at the s-th moment s+1 Iterative correction matrix for the estimation at time s+1, y s+1 For the actual measured output of the tricycle movement model at time s+1, < >>α s+1 =mod (s, 5) +1 denotes the sensor node used for transmitting the measurement data at time s+1, mod (s, 5) being defined as the remainder of the integer s divided by the positive integer 5, +.>Represents the Croneck function with values 0 and 1,/for> Is the probability that the mth node has not been deleted at time s +1, wherein φ (-) is the probability of a standard normal distributionDensity function-> Measurement noise covariance matrix R for s+1st moment s+1 The (m, m) th element of (c),representation pair->The transposition process is carried out, the image is processed, is->Is the m-th element of (c).
Step three, calculating an upper bound lambda of a one-step prediction error covariance matrix of the tricycle motion model at the s-th moment s+1|s 。
In this step, the upper boundary Λ of the one-step prediction error covariance matrix of the tricycle motion model at the s-th moment is calculated according to the following formula s+1|s ：
Wherein the superscript T represents the transpose of the matrix, the superscript-1 represents the inverse of the matrix or value,nonlinear function of the corresponding state of the tricycle motion model at the s-th moment +.>At->Partial derivative at, Λ s|s For the upper bound of the estimated error covariance matrix at time s,/>And->Based on a non-linear function->Error term obtained using Taylor expansion, < ->Represents a known time-varying coefficient greater than 0 and satisfies the condition +.>
Step four, utilizing the lambda obtained in the step three s+1|s Calculating an estimated iterative correction matrix K of the tricycle motion model at the (s+1) th moment s+1 。
In the step, an estimated iterative correction matrix K based on the s+1st moment of the tricycle motion model s+1 The calculation formula is as follows:
wherein:
wherein, Γ s+1 As an intermediate parameter matrix, p m,s+1 (m=1, 2, 3) are positive weighting coefficients,for a nonlinear function at time s+1 corresponding to the measured output based on the tricycle motion model, +.>For one-step prediction of the state of the tricycle motion model at the s-th moment epsilon s+1 Is->At->Partial derivative of>Is a positive scalar and satisfies the condition +.> And +.>By->Error matrix, Λ, obtained using taylor formula s+1|s For the upper bound of the one-step prediction error covariance matrix at time s,/for the time of day>Representing Hadamard products between matrices, tr {.cndot } represents the trace operation of the matrices, ρ 1,s+1 ρ is 2,s+1 Is satisfied->Is used for the normal number of (a), II indicates Euclidean norm of ", and->And->Respectively represent ρ 1,s+1 And ρ 2,s+1 Square values of (a).
Step five, utilizing the estimated iterative correction matrix K obtained in the step four s+1 Substituting it into step two to obtain an estimate of the s+1 timeAnd then judging whether s+1 reaches the estimated total duration U, if s+1 is less than U, executing the next step, and if s+1=U, stopping the state estimation of the tricycle motion model with measurement deletion under the periodic scheduling protocol.
Step six, utilizing the estimated iterative correction matrix K calculated in the step four s+1 Calculating the upper limit lambda of the estimated error covariance matrix of the tricycle motion model at the s+1 time s+1|s+1 The method comprises the steps of carrying out a first treatment on the surface of the Let s=s+1, execute step two until the stop condition s+1=u is reached.
In this step, the upper bound Λ of the estimated error covariance matrix at time s+1 s+1|s+1 The calculation formula of (2) is as follows:
wherein:
wherein, lambda s+1|s+1 Is the upper bound of the estimated error covariance matrix at time s+1.
In the invention, the theory in the third, fourth and fifth steps is as follows:
first, an optimal upper bound Λ of an estimated error covariance matrix is calculated s+1|s+1 So that P s+1|s+1 ≤Λ s+1|s+1 WhereinFor the estimated error covariance matrix at time s+1,> indicating that the desired operation is performed on ",">Representation pair->And performing transposition operation. On the other hand, since the existence of the uncertain terms in the estimated error covariance matrix cannot determine the specific explicit form thereof, the uncertain terms are processed by a basic inequality processing method to be converted into the form of a known matrix, so as to obtain the upper bound of the estimated error covariance matrix, then the trace of the upper bound of the estimated error covariance matrix is minimized, and the estimated iterative correction matrix K at the (s+1) th moment can be further calculated s+1 。
Examples:
taking a tricycle motion model with a periodic scheduling protocol and measurement deletion as an example, the method of the invention is adopted to carry out the following simulation experiment:
based on non-linear functionsAnd->An error matrix obtained using the taylor formula:
measured erasure threshold:
process noise covariance matrix and measurement noise covariance matrix:
tricycle motion model process noise distribution matrix D s ：
Initial value related to tricycle motion model:
/>
in the method, in the process of the invention,mean value representing initial value of model state, +.>Representing the initial value of the filtering, P 0 Variance matrix, Λ, representing initial values of the model 0|0 Representing the initial value of the upper bound of the estimation error covariance matrix.
Other parameters were selected as: the weight coefficients are p respectively 1,s+1 ＝0.001，p 2,s+1 ＝0.02，p 4,h+1 ＝0.57，ρ 1,s+1 ＝9.4，ρ 2,s+1 =2.7, sampling interval Δh=1, distance p=5 from front wheel to rear axle of tricycle, sensor measuring beacon position x b =2 and y b ＝5。
Defining an estimation method to improve the precision ratio:wherein Σmse 1 In the case of estimating the state of the tricycle motion model in the representative case II (using the extended kalman filter method), the mean square estimation error is the cumulative sum of all values at the times s=1 to s=200, and in the case of estimating the state of the tricycle motion model in the representative case I (using the state estimation method proposed by the present invention), the mean square estimation error is the cumulative sum of all values at the times s=1 to s=200.
State estimator effect:
fig. 2 to 6 show graphs of the track of each component of the state of the tricycle motion model under the periodic scheduling protocol and the track of the estimated value thereof, and it can be seen that the track of each component can track the estimated value thereof in real time, so that the tricycle motion model with measurement deletion under the periodic scheduling protocol can be illustrated.
Fig. 7 is a graph of the trace of the upper bound of the estimation error covariance matrix against the MSE, and fig. 7 shows that the MSE always satisfies the trace below the upper bound of the estimation error covariance matrix, thereby further verifying the effectiveness of the algorithm proposed by the present invention.
Fig. 8 shows a plot of the MSE for the state of the tricycle motion model under different conditions, it can be seen that the MSE for case II is always higher than that for case I, since the Tobit regression model is not used to efficiently handle the existing measurement deletions in case II. Furthermore, we can calculate from the definition of the improvement in the precision ratio: the accuracy ratio of case I is improved by about 70% (i.e. ia=70%) with respect to the accuracy ratio of case II, so fig. 8 can illustrate the feasibility of the state estimation method proposed by the present invention.

Claims (4)

1. A state estimation method based on a tricycle motion model under a periodic scheduling protocol is characterized by comprising the following steps:
step one, based on five-dimensional state variables consisting of vehicle coordinates, vehicle path angle deviation, linear speed and steering angle, establishing a tricycle motion model with measurement deletion, wherein the tricycle motion model with measurement deletion is as follows:
wherein x is s ＝[λ s β s η s μ s ξ s ] T The superscript T denotes a transpose; sin (-), cos (-) and tan (-) represent the sine function, cosine function and tangent function of "(") respectively; lambda (lambda) s 、β s 、η s 、μ s And xi s Respectively representing the original coordinates of the vehicle, the vehicle path angle deviation, the linear speed and the vehicle steering angle of the tricycle motion model at the s-th moment; Δh is the sampling interval of the model, P represents the distance from the front wheel to the rear axle of the tricycle, x b And y b Measuring a beacon position on behalf of a sensor installed in the vehicle; omega s Representing zero-mean Gaussian process white noise at time s, v s To zero-mean Gaussian measure white noise at time s, D s Is a known time-varying noise distribution matrix having five dimensions;is the original measurement output; />Is the output vector after deletion, y s ＝diag{γ 1,s ,γ 2,s ,...,γ 5,s (where diag represents a diagonal matrix, γ) m,s Is a Bernoulli random variable for determining whether or not the measured value of the mth node at the mth time is subject to deletion,/>Representing a measurement erasure threshold vector, I is a five-dimensional identity matrix, m=1, 2, 5;
step two, designing a state estimator according to the tricycle motion model constructed in the step one under a periodic scheduling protocol, and carrying out state estimation on the tricycle motion model through a state estimator equation, wherein the specific structure of the state estimator is as follows:
in the method, in the process of the invention,for one-step prediction of the state of the movement model of the tricycle at time s,/for the time of day>For the estimation of the s-th moment of the state of the tricycle movement model, a>For the estimate at time s+1, +.>Nonlinear function of state based on tricycle motion model +.>Estimate at time s, +.>K is a one-step prediction form of a nonlinear function corresponding to measurement output based on a tricycle motion model at the s-th moment s+1 Iterative correction matrix for the estimation at time s+1, y s+1 For the actual measured output of the tricycle movement model at time s+1, < >>α s+1 =mod (s, 5) +1 denotes the sensor node used for transmitting the measurement data at time s+1, mod (s, 5) being defined as the remainder of the integer s divided by the positive integer 5, +.>Represents the Croneck function with values 0 and 1,/for> Is the probability that the mth node has not been deleted at time s +1,wherein φ (·) is the probability density function of a standard normal distribution, ++> Measurement noise covariance matrix R for s+1st moment s+1 Element (m, m), ∈>Representation pairPerforming transposition treatment> Is->The m-th element of (2);
step three, calculating the upper bound of a one-step prediction error covariance matrix of the tricycle motion model at the s-th momentΛ s+1|s The upper limit lambda of the one-step prediction error covariance matrix of the tricycle motion model at the s-th moment s+1|s The expression is:
in the formula, the superscript-1 represents that the matrix or the numerical value is subjected to inverse operation,nonlinear function f (x) at s-th moment for corresponding tricycle motion model state s ) At->Partial derivative at, Λ s|s For the upper bound of the estimated error covariance matrix at time s,/>And->Is based on a nonlinear function f (x s ) Error term obtained using Taylor expansion, < ->Representing a known time-varying coefficient greater than 0;
step four, utilizing the lambda obtained in the step three s+1|s Calculating an estimated iterative correction matrix K of the tricycle motion model at the (s+1) th moment s+1 Estimating iterative correction matrix K of tricycle motion model at s+1th moment s+1 The expression of (2) is:
wherein:
wherein, Γ s+1 As an intermediate parameter matrix, p m,s+1 Are positive weighting coefficients, m=1, 2,3,for nonlinear function at s+1st moment corresponding to measurement output based on tricycle motion model s+1 Is->At->The partial derivative of the position(s),is a positive scalar, ++>And +.>By->Error matrix, Λ, obtained using taylor formula s+1|s For one-step pre-preparation at time sThe upper bound of the error covariance matrix is measured, the "°" represents the Hadamard product between the matrices, tr {. Cndot. } "represents the trace operation of the matrices, ρ 1,s+1 ρ is 2,s+1 Is satisfied->Is used for the normal number of (a), II indicates Euclidean norm of ", and->And->Respectively represent ρ 1,s+1 And ρ 2,s+1 Square value of (2);
step five, utilizing the estimated iterative correction matrix K obtained in the step four s+1 Substituting the same into the second step to obtain an estimate of the (s+1) th momentThen judging whether s+1 reaches the estimated total duration U, if s+1 is less than U, executing step six, and if s+1=U, stopping the state estimation of the tricycle motion model with measurement deletion under the periodic scheduling protocol;
step six, utilizing the estimated iterative correction matrix K calculated in the step four s+1 Calculating the upper limit lambda of the estimated error covariance matrix of the tricycle motion model at the s+1 time s+1|s+1 The method comprises the steps of carrying out a first treatment on the surface of the Let s=s+1, execute step two until the stop condition s+1=u is reached, estimate the upper bound Λ of the error covariance matrix s+1|s+1 The calculation formula of (2) is as follows:
wherein:
wherein, lambda s+1|s+1 Is the upper bound of the estimated error covariance matrix at time s+1.
2. The method for estimating a state based on a tricycle motion model according to claim 1, wherein in said step one, γ m,s Satisfy the following requirementsP (·) represents the probability of occurrence of "·",>for the probability of no deletion of the mth node at the s-th moment, the xi (·) is a distribution function of standard normal distribution, and τ m Is->Is the mth component of->For one-step prediction of the state of the tricycle movement model at time s-1, +.>Is thatIs the mth component of->In one-step prediction form of nonlinear function at s-1 time based on measurement output of tricycle motion model>For the measurement noise covariance matrix R at the s-th moment s (m, m) th element of (c).
3. The method for estimating the state based on the motion model of the tricycle under the periodic scheduling protocol according to claim 1, wherein in the third step,satisfy condition->
4. The method for estimating the state based on the motion model of the tricycle under the periodic scheduling protocol according to claim 1, wherein in the fourth step,satisfy condition->
CN202311169570.9A 2023-09-11 2023-09-11 Tricycle motion model-based state estimation method under periodic scheduling protocol Active CN117057159B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202311169570.9A CN117057159B (en) 2023-09-11 2023-09-11 Tricycle motion model-based state estimation method under periodic scheduling protocol

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202311169570.9A CN117057159B (en) 2023-09-11 2023-09-11 Tricycle motion model-based state estimation method under periodic scheduling protocol

Publications (2)

Publication Number Publication Date
CN117057159A true CN117057159A (en) 2023-11-14
CN117057159B CN117057159B (en) 2024-03-01

Family

ID=88666344

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202311169570.9A Active CN117057159B (en) 2023-09-11 2023-09-11 Tricycle motion model-based state estimation method under periodic scheduling protocol

Country Status (1)

Country Link
CN (1) CN117057159B (en)

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US7181323B1 (en) * 2004-10-25 2007-02-20 Lockheed Martin Corporation Computerized method for generating low-bias estimates of position of a vehicle from sensor data
US20230054246A1 (en) * 2020-05-06 2023-02-23 Beijing Institute Of Technology Iterative joint estimation method of vehicle mass and road gradient based on mmrls and sh-stf
CN115865702A (en) * 2022-11-15 2023-03-28 哈尔滨理工大学 Distributed fusion estimation method with data attenuation under network scheduling strategy
CN116527515A (en) * 2023-05-16 2023-08-01 哈尔滨工业大学 Remote state estimation method based on polling protocol

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US7181323B1 (en) * 2004-10-25 2007-02-20 Lockheed Martin Corporation Computerized method for generating low-bias estimates of position of a vehicle from sensor data
US20230054246A1 (en) * 2020-05-06 2023-02-23 Beijing Institute Of Technology Iterative joint estimation method of vehicle mass and road gradient based on mmrls and sh-stf
CN115865702A (en) * 2022-11-15 2023-03-28 哈尔滨理工大学 Distributed fusion estimation method with data attenuation under network scheduling strategy
CN116527515A (en) * 2023-05-16 2023-08-01 哈尔滨工业大学 Remote state estimation method based on polling protocol

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
张文峰;胡振涛;程建兴;: "一种车辆机动目标跟踪的多传感器信息融合估计算法", 广东工业大学学报, no. 01, 15 March 2009 (2009-03-15) *

Also Published As

Publication number Publication date
CN117057159B (en) 2024-03-01

Similar Documents

Publication Publication Date Title
CN111985093B (en) Adaptive unscented Kalman filtering state estimation method with noise estimator
Ge et al. SCKF-STF-CN: a universal nonlinear filter for maneuver target tracking
CN109088749B (en) State estimation method of complex network under random communication protocol
CN103927436A (en) Self-adaptive high-order volume Kalman filtering method
CN110225454B (en) Confidence transfer distributed type volume Kalman filtering cooperative positioning method
CN110958639A (en) Target state estimation method and system
CN103940433A (en) Satellite attitude determining method based on improved self-adaptive square root UKF (Unscented Kalman Filter) algorithm
WO2020052213A1 (en) Iterative cubature unscented kalman filtering method
CN111444474B (en) Target tracking method based on multiplicative noise correlation self-adaptive CKF
CN108313330B (en) Satellite interference torque estimation method based on augmented Kalman filtering
CN111798494A (en) Maneuvering target robust tracking method under generalized correlation entropy criterion
Wanasinghe et al. Decentralized cooperative localization approach for autonomous multirobot systems
CN105654053B (en) Based on the dynamic oscillation signal parameter discrimination method for improving constraint EKF algorithm
CN117057159B (en) Tricycle motion model-based state estimation method under periodic scheduling protocol
Hu et al. IMM fusion estimation with multiple asynchronous sensors
CN109341690A (en) A kind of efficient combined navigation self-adaptive data fusion method of robust
CN116088303B (en) Uncertain complex dynamic network state time-varying recursion estimation method
CN111062139A (en) Method for estimating water flow situation of urban underground drainage pipe network
Fang et al. Robust adaptive cubature Kalman filter for tracking manoeuvring target by wireless sensor network under noisy environment
CN111259340A (en) Saturated load prediction method based on logistic regression
CN111222086A (en) Time-varying system recursive state estimation method based on full-information filtering technology
CN115859626A (en) Self-adaptive unscented Kalman filter design method for periodic moving target
CN113848876B (en) Low-communication and low-calculation-consumption fault detection method for multi-AGV distance cruise system
CN114565010A (en) Adaptive Kalman noise estimation method and system based on data fusion
Tan et al. Gaussian recursive filter for nonlinear systems with finite-step correlated noises and packet dropout compensations

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