CN115580261A - State estimation method based on combination of robust strategy and dynamic enhanced CKF - Google Patents
State estimation method based on combination of robust strategy and dynamic enhanced CKF Download PDFInfo
- Publication number
- CN115580261A CN115580261A CN202211246725.XA CN202211246725A CN115580261A CN 115580261 A CN115580261 A CN 115580261A CN 202211246725 A CN202211246725 A CN 202211246725A CN 115580261 A CN115580261 A CN 115580261A
- Authority
- CN
- China
- Prior art keywords
- state
- volume
- kalman filter
- point
- updating
- 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.)
- Pending
Links
Images
Classifications
-
- H—ELECTRICITY
- H03—ELECTRONIC CIRCUITRY
- H03H—IMPEDANCE NETWORKS, e.g. RESONANT CIRCUITS; RESONATORS
- H03H17/00—Networks using digital techniques
- H03H17/02—Frequency selective networks
- H03H17/0248—Filters characterised by a particular frequency response or filtering method
- H03H17/0255—Filters based on statistics
- H03H17/0257—KALMAN filters
Landscapes
- Physics & Mathematics (AREA)
- Probability & Statistics with Applications (AREA)
- Engineering & Computer Science (AREA)
- Computer Hardware Design (AREA)
- Mathematical Physics (AREA)
- Filters That Use Time-Delay Elements (AREA)
Abstract
The embodiment of the disclosure provides a state estimation method based on combination of a robust strategy and a dynamic enhanced CKF (CKF), which belongs to the technical field of electricity and specifically comprises the following steps: step 1, confirming volume sampling point zeta i And the weight ω of the point i (ii) a Step 2, updating data of the cubature Kalman filter; step 3, measuring and updating the volume Kalman filter; step 4, designing a dynamic enhanced volume Kalman filter, and determining an ith volume point and a corresponding weight coefficient; step 5, estimating sampling points of the dynamic enhanced volume Kalman filter; and 6, performing second-stage measurement updating according to the sampling points of the dynamic enhanced volume Kalman filter. By means of the scheme, estimation efficiency and accuracy are improved.
Description
Technical Field
The embodiment of the disclosure relates to the technical field of electricity, in particular to a state estimation method based on combination of a robust strategy and a dynamic enhanced CKF.
Background
Currently, rate gyroscopes, which rely on superior stability and accurate information for state estimation in Unmanned Autonomous Helicopters (UAHs), typically employ expensive measurement units to achieve the required accuracy, but are not practical for use with UAHs due to size, weight and cost limitations. Inexpensive devices tend to be low performance sensors as well, and navigational measurement errors can also accumulate significantly over time, so using these instruments for long flights remains a challenging task. The traditional kalman filter is used to solve the state estimation problem of linear system, and is not useful in this case, and extended kalman filter, unscented kalman filter, gaussian kalman filter, fuzzy complementary kalman filter, sparse grid orthogonal kalman filter, random integral filter, volumetric kalman filter, etc. are derived for the kalman filter in the nonlinear case. Among these methods, the volumetric kalman filter (CKF) has its outstanding advantages, since CKF does not need to calculate jacobian and hessian matrices, so that the filter is easy to create, it calculates the estimated value quickly with low complexity, and especially meets the system requirements of real-time fast state estimation. However, because the EKF is implemented by linear fitting, taylor series expansion needs to be performed on the nonlinear sensor system to be estimated, and high-order terms need to be omitted in the processing operation process. In the process of obtaining the estimated state of the approximate linear system, if errors caused by the truncated high-order terms cannot be ignored, the EKF or similar filtering algorithm is adopted at the time, and the estimation precision is reduced. Meanwhile, the EKF requires a Jacobian matrix of a calculation model, and the calculation is more complicated.
Therefore, an efficient and accurate state estimation method based on the combination of the robust strategy and the dynamic enhanced CKF is needed.
Disclosure of Invention
In view of this, the embodiments of the present disclosure provide a state estimation method based on a combination of a robust policy and a dynamic enhanced CKF, which at least partially solves the problems of complex algorithm, and poor estimation efficiency and accuracy in the prior art.
The embodiment of the disclosure provides a state estimation method based on combination of a robust strategy and a dynamic enhanced CKF, which comprises the following steps:
step 1, confirming volume sampling point zeta i And the weight omega of the point i ;
Step 2, updating data of the cubature Kalman filter;
step 3, measuring and updating the volume Kalman filter;
step 4, designing a dynamic enhanced type volume Kalman filter, and determining the ith volume point and a corresponding weight coefficient thereof;
step 5, estimating sampling points of the dynamic enhanced volume Kalman filter;
and 6, performing second-stage measurement updating according to the sampling points of the dynamic enhanced volume Kalman filter.
According to a specific implementation of an embodiment of the present disclosure, the volume sampling point ζ i And the weight ω of the point i Is expressed as
Wherein i represents the ith sampling point of the filter, the number of the sampling points is twice of the state dimension nx, [1] i represents the set of the ith sampling point, the set is a complete symmetrical set, and the nx dimension represents the point set generated after the unit vector symbol is transformed and the permutation and conversion are carried out.
According to a specific implementation manner of the embodiment of the present disclosure, the step 2 specifically includes:
from volume points of the non-linear equation of state and the initial state estimate X K-1 And its covariance P K-1 Obtaining the state estimation variable X of the cubature Kalman filter K/K-1 And the predicted variance P K/K-1 And performing data updating according to the expression.
According to a specific implementation of the embodiments of the present disclosure, the state estimation variable X K/K-1 And the predicted variance P K/K-1 Is expressed as
According to a specific implementation manner of the embodiment of the present disclosure, the step 3 specifically includes:
calculating the volume point of the nonlinear equation of state according to a preset formula and based on P K By singular value decomposition to obtain a measured value P at the time K-1 K-1 ;
Updating first-stage cubature Kalman filter measurement estimation valueInnovation variance P zz,k And covariance P xz,k Updating the gain matrix K k Update of stateSum covariance matrix P k 。
According to a specific implementation manner of the embodiment of the present disclosure, the expression of the ith volume point and the corresponding weight coefficient thereof is
According to a specific implementation manner of the embodiment of the present disclosure, the step 5 specifically includes:
defining a forgetting factor of a system to be observed, introducing the forgetting factor to replace a covariance solving expression, and rewriting the forgetting factor into a new covariance matrix P K/K-1
According to a specific implementation manner of the embodiment of the present disclosure, the step 6 specifically includes:
obtaining the system state value X at the moment K i,k/k-1 Measuring the state value Y i,k/k-1 And the estimated value of the measurement stateUpdating the innovation variance matrix P of the second stage xz,k And its covariance P zz,k Update the system state X K And its covariance P K And obtaining the gain value of the new information variance matrix through the ratio of the new information variance matrix and the covariance matrix.
The state estimation scheme based on the combination of the robust strategy and the dynamic enhanced CKF in the embodiment of the disclosure comprises the following steps: step 1, confirming volume sampling point zeta i And the weight ω of the point i (ii) a Step 2, updating data of the cubature Kalman filter; step 3, measuring and updating the volume Kalman filter; step 4, designing a dynamic enhanced type volume Kalman filter, and determining the ith volume point and a corresponding weight coefficient thereof; step 5, estimating sampling points of the dynamic enhanced volume Kalman filter; and 6, performing second-stage measurement updating according to the sampling points of the dynamic enhanced volume Kalman filter.
The beneficial effects of the embodiment of the disclosure are: by the scheme, an enhanced CKF algorithm is provided to help improve the estimation performance of the CKF when the experiment platform runs under the condition of disturbance. A novel computational attenuation factor algorithm is described in detail, eliminating the need for Jacobian and Hessian matrix computations, thereby saving execution time. And then, a dynamic enhancement strategy is provided, the automatic adjustment of the enhancement strategy is realized by detecting the uncertainty state of the system, the effective system state estimation is realized at a proper time, the loss of precision when the uncertainty of the system does not exist is avoided, and the estimation efficiency and precision are improved.
Drawings
In order to more clearly illustrate the technical solutions of the embodiments of the present disclosure, the drawings needed to be used in the embodiments will be briefly described below, and it is apparent that the drawings in the following description are only some embodiments of the present disclosure, and it is obvious for those skilled in the art that other drawings can be obtained according to the drawings without creative efforts.
Fig. 1 is a schematic flowchart of a state estimation method based on a combination of a robust policy and a dynamic enhanced CKF according to an embodiment of the present disclosure;
fig. 2 is a schematic diagram comparing the estimation results of the pitch channel according to the embodiment of the present disclosure.
Detailed Description
The embodiments of the present disclosure are described in detail below with reference to the accompanying drawings.
The embodiments of the present disclosure are described below with specific examples, and other advantages and effects of the present disclosure will be readily apparent to those skilled in the art from the disclosure in the specification. It is to be understood that the described embodiments are merely illustrative of some, and not restrictive, of the embodiments of the disclosure. The disclosure may be carried into practice or applied to various other specific embodiments, and various modifications and changes may be made in the details within the description and the drawings without departing from the spirit of the disclosure. It is to be noted that the features in the following embodiments and examples may be combined with each other without conflict. All other embodiments, which can be derived by a person skilled in the art from the embodiments disclosed herein without making any creative effort, shall fall within the protection scope of the present disclosure.
It is noted that various aspects of the embodiments are described below within the scope of the appended claims. It should be apparent that the aspects described herein may be embodied in a wide variety of forms and that any specific structure and/or function described herein is merely illustrative. Based on the disclosure, one skilled in the art should appreciate that one aspect described herein may be implemented independently of any other aspects and that two or more of these aspects may be combined in various ways. For example, an apparatus may be implemented and/or a method practiced using any number of the aspects set forth herein. Additionally, such an apparatus may be implemented and/or such a method may be practiced using other structure and/or functionality in addition to one or more of the aspects set forth herein.
It should be noted that the drawings provided in the following embodiments are only for illustrating the basic idea of the present disclosure, and the drawings only show the components related to the present disclosure rather than the number, shape and size of the components in actual implementation, and the type, amount and ratio of the components in actual implementation may be changed arbitrarily, and the layout of the components may be more complicated.
In addition, in the following description, specific details are provided to facilitate a thorough understanding of the examples. However, it will be understood by those skilled in the art that the aspects may be practiced without these specific details.
The embodiment of the disclosure provides a state estimation method based on combination of a robust strategy and a dynamic enhanced CKF (CKF), and the method can be applied to the control process of an unmanned autonomous helicopter in a state estimation scene.
Referring to fig. 1, a schematic flowchart of a state estimation method based on a combination of a robust policy and a dynamic enhanced CKF is provided for an embodiment of the present disclosure. As shown in fig. 1, the method mainly comprises the following steps:
step 1, confirming volume sampling point zeta i And the weight ω of the point i ;
Further, the volume sampling point ζ i And the weight ω of the point i Is expressed as
Wherein i represents the ith sampling point of the filter, the number of the sampling points is twice of the state dimension nx, [1] i represents the set of the ith sampling point, the set is a complete symmetrical set, and the nx dimension represents the point set generated after the unit vector symbol is transformed and the permutation and conversion are carried out.
For example, CKF may employ a third order volume sampling rule, and the sampling point and the weight of the point may be expressed as:
in the above formula, i represents the ith sampling point of the filter, the number of the sampling points is twice of the state dimension nx, [1] i represents the set of the ith sampling point, the set is a complete symmetrical set, and the nx dimension represents the point set generated after the unit vector symbol is transformed and is subjected to permutation and conversion.
Step 2, updating data of the cubature Kalman filter;
on the basis of the above embodiment, the step 2 specifically includes:
from volume points of the non-linear equation of state and the initial state estimate X K-1 And its covariance P K-1 Obtaining the state estimation variable X of the cubature Kalman filter K/K-1 And the predicted variance P K/K-1 And expressing and updating data according to the expression.
Further, the state estimation variable X K/K-1 And the predicted variance P K/K-1 Is expressed as
In specific implementation, the square root form of the covariance matrix is as follows:
wherein S represents a diagonally symmetric matrix composed of sn, S = diag [ S ] 1 ,s 2 ,...,s n ]The expression of the above formula PK-1 can be obtained according to a singular value decomposition algorithm:
the volume points of the nonlinear equation of state can be found:
X′ i,k =f(X i,k-1 )
the state estimation variables and the prediction variance of the filter can be expressed as:
step 3, measuring and updating the volume Kalman filter;
on the basis of the above embodiment, the step 3 specifically includes:
calculating the volume point of the nonlinear equation of state according to a preset formula and based on P K Is subjected to singular value decomposition to obtain a measured value P at the time of K-1 K-1 ;
Updating first-stage cubature Kalman filter measurement estimation valueInnovation variance P zz,k And covariance P xz,k Updating the gain matrix K k Update of stateSum covariance matrix P k 。
In the specific implementation, the measured value P at the time k-1 K-1 Based on P K Is obtained by singular value decomposition
The volume point of the nonlinear equation of state is calculated by the following formula:
Z i,k =h(X i,k/k-1 )
the measurement estimate update, innovation variance update, and covariance update for the filter are expressed as:
calculation of gain matrix, state update and covariance matrix update:
comparing with the calculation process of the standard CKF, the filtering of the invention is different from the standard CKF in the state updating equation of the filter.
Step 4, designing a dynamic enhanced volume Kalman filter, and determining an ith volume point and a corresponding weight coefficient;
further, the expression of the ith volume point and the corresponding weight coefficient is
In specific implementation, the CKF filter design adopting a dynamic enhancement strategy is adopted, and for a nonlinear system:
in the above formula, X (K), U (K) and Z (K) respectively represent state variables, control inputs and measurement state quantities of the state variable system at the moment K; f () and h (, respectively) represent the system function and the measurement function; w (K-1) and V (K) represent the system noise and the measurement noise, respectively, with the corresponding covariances Q (K-1) and R (K). The initial state of the system is denoted as X (0), the covariance is denoted as P (0), and the expression:
the constraints on the noise and uncertainty transfer functions are:
then the ith volume point and the corresponding weight coefficient are determined:
step 5, estimating sampling points of the dynamic enhanced volume Kalman filter;
on the basis of the above embodiment, the step 5 specifically includes:
defining a forgetting factor of a system to be observed, introducing the forgetting factor to replace a covariance solving expression, and rewriting the forgetting factor into a new covariance matrix P K/K-1
In specific implementation, the estimation of the CKF point is as follows:
original covariance matrix:
where N is the sampling point, the prediction information at time k:wi in the above equation is the matrix of correlation coefficients at point i of the aircraft state filter.
Here we need to introduce a forgetting factor to replace the above constant covariance solution expression, and the forgetting factor of the system to be observed is defined as:
wherein λ 0 =tr(W k+1 )·tr -1 (M k+1 ) And tr (, denotes the operation of the matrix trace, and the expression is as follows:
and LK +1 and W0, K +1 in the above formula can be represented as:
where WK +1 represents the radius of error. The new covariance matrix at this time can be rewritten as:
and 6, performing second-stage measurement updating according to the sampling points of the dynamic enhanced volume Kalman filter.
On the basis of the above embodiment, the step 6 specifically includes:
obtaining the system state value X at the moment K i,k/k-1 And measuring the state value Y i,k/k-1 And the estimated value of the measurement stateUpdating the innovation variance matrix P of the second stage xz,k And its covariance P zz,k Update the system state X K And its covariance P K And obtaining the gain value of the new information variance matrix through the ratio of the new information variance matrix and the covariance matrix.
In the specific implementation, at time K, the system state is as follows:
X i,k|k-1 =S k-1/k-l γ i +X k/k-l
measurement state value at time K:
Y i,k/k-1 =H(x k/k-1 )
measurement state estimation at time K:
innovation variance matrix update and covariance matrix update:
updating system state and covariance matrix:
gain value in equation: k is k =P xz,k /P zz,k 。
Through comparative simulation experiments, as shown in fig. 2, it can be seen from the comparison of state estimates of the pitch channel, the roll channel and the yaw channel that the filtering effect of the filter of the invention is far superior to the filtering accuracy of the conventional CKF and is also closest to the measured value. The performance of the filter of the invention is obviously better than the state estimation result of the traditional filter from the enlarged region; on biasIn the state estimation comparison graph of the navigation channel, the accumulated error of the estimation error of the classical CKF gradually increases along with the time, and obvious observation error is generated with an actually measured value in the simulation process. Meanwhile, by observing the estimation curves of the pitching channel state and the rolling channel state, the following conditions can be seen: strong coupling exists between the two channels, so that the state estimation of the pitch channel and the roll channel is more susceptible to interference; and in the velocity response graph, from V X 、V Y And V Z The performance difference of the filter in the aspect of speed estimation can be seen from the channel speed estimation curve, and the filter provided by the invention has the best speed estimation performance, is superior to the filtering result of the traditional CKF, and is closest to the measured curve. The curves of the amplification region clearly distinguish the results of the individual filters. Meanwhile, the vertical speed is much smaller than the speed estimation errors on the x axis and the y axis, and is much more stable, except for the coupling effect reason of the x-y, the ground height measurement radar and the ultrasonic height measurement radar which are arranged on the platform perform differential calculation on the measurement height information, so that high-precision non-deviation real-time vertical speed measurement is obtained, and the result after the measurement is fused with the result after z-axis data filtering, and more accurate speed measurement can be realized.
The state estimation method based on the combination of the robust strategy and the dynamic enhancement CKF provided by the embodiment helps to improve the estimation performance of the CKF when the experimental platform runs in the presence of disturbance by providing an enhanced CKF algorithm. A novel computational attenuation factor algorithm is described in detail, eliminating the need for Jacobian and Hessian matrix computations, thereby saving execution time. Then, a dynamic enhancement strategy is provided, automatic adjustment of the enhancement strategy is realized by detecting the uncertainty state of the system, effective system state estimation is realized at a proper time, and loss of precision when the system uncertainty does not exist is avoided; and a dynamic enhancement strategy is provided in the second stage, so that the estimation efficiency and accuracy are improved.
The units described in the embodiments of the present disclosure may be implemented by software or hardware.
It should be understood that portions of the present disclosure may be implemented in hardware, software, firmware, or a combination thereof.
The above description is only for the specific embodiments of the present disclosure, but the scope of the present disclosure is not limited thereto, and any changes or substitutions that can be easily conceived by those skilled in the art within the technical scope of the present disclosure should be covered within the scope of the present disclosure. Therefore, the protection scope of the present disclosure shall be subject to the protection scope of the claims.
Claims (8)
1. A state estimation method based on combination of a robust strategy and a dynamic enhanced CKF (CKF) is characterized by comprising the following steps:
step 1, confirming volume sampling point zeta i And the weight ω of the point i ;
Step 2, updating data of the cubature Kalman filter;
step 3, measuring and updating the volume Kalman filter;
step 4, designing a dynamic enhanced type volume Kalman filter, and determining the ith volume point and a corresponding weight coefficient thereof;
step 5, estimating sampling points of the dynamic enhanced volume Kalman filter;
and 6, performing second-stage measurement updating according to the sampling points of the dynamic enhanced volume Kalman filter.
2. According to the claimThe method of claim 1, wherein the volume sampling points ζ i And the weight ω of the point i Is expressed as
Wherein i represents the ith sampling point of the filter, the number of the sampling points is twice of the state dimension nx, [1] i represents the set of the ith sampling point, the set is a complete symmetrical set, and the nx dimension represents the point set generated after the unit vector symbol is transformed and the permutation and conversion are carried out.
3. The method according to claim 2, wherein the step 2 specifically comprises:
from the volume points of the non-linear equation of state and the initial state estimate X K-1 And its covariance P K-1 Obtaining the state estimation variable X of the cubature Kalman filter K/K-1 And the predicted variance P K/K-1 And expressing and updating data according to the expression.
5. The method according to claim 4, wherein the step 3 specifically comprises:
calculating the volume point of the nonlinear equation of state according to a preset formula and based on P K By singular value decomposition to obtain a measured value P at the time K-1 K-1 ;
8. The method according to claim 7, wherein the step 6 specifically comprises:
obtaining the system state value X at the moment K i,k/k-1 And measuring the state value Y i,k/k-1 And the estimated value of the measurement stateUpdating the innovation variance matrix P of the second stage xz,k And its covariance P zz,k Update, updateSystem State X K And its covariance P K And obtaining the gain value of the innovation variance matrix through the ratio of the innovation variance matrix and the covariance matrix.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202211246725.XA CN115580261A (en) | 2022-10-12 | 2022-10-12 | State estimation method based on combination of robust strategy and dynamic enhanced CKF |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202211246725.XA CN115580261A (en) | 2022-10-12 | 2022-10-12 | State estimation method based on combination of robust strategy and dynamic enhanced CKF |
Publications (1)
Publication Number | Publication Date |
---|---|
CN115580261A true CN115580261A (en) | 2023-01-06 |
Family
ID=84585918
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202211246725.XA Pending CN115580261A (en) | 2022-10-12 | 2022-10-12 | State estimation method based on combination of robust strategy and dynamic enhanced CKF |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN115580261A (en) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN117074962A (en) * | 2023-08-22 | 2023-11-17 | 江南大学 | Lithium ion battery state joint estimation method and system |
-
2022
- 2022-10-12 CN CN202211246725.XA patent/CN115580261A/en active Pending
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN117074962A (en) * | 2023-08-22 | 2023-11-17 | 江南大学 | Lithium ion battery state joint estimation method and system |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN108545081B (en) | Centroid slip angle estimation method and system based on robust unscented Kalman filtering | |
CN109211276B (en) | SINS initial alignment method based on GPR and improved SRCKF | |
CN109459019B (en) | Vehicle navigation calculation method based on cascade adaptive robust federal filtering | |
CN109974714B (en) | Sage-Husa self-adaptive unscented Kalman filtering attitude data fusion method | |
CN111156987B (en) | Inertia/astronomy combined navigation method based on residual compensation multi-rate CKF | |
CN101982732B (en) | Micro-satellite attitude determination method based on ESOQPF (estimar of quaternion particle filter ) and UKF (unscented kalman filter) master-slave filtering | |
CN105136145A (en) | Kalman filtering based quadrotor unmanned aerial vehicle attitude data fusion method | |
CN106840093B (en) | Unmanned aerial vehicle flight height detection method and device and unmanned aerial vehicle | |
CN103033186A (en) | High-precision integrated navigation positioning method for underwater glider | |
CN110346821B (en) | SINS/GPS combined attitude-determining and positioning method and system for solving long-time GPS unlocking problem | |
CN104112079A (en) | Fuzzy adaptive variational Bayesian unscented Kalman filter method | |
CN106772524A (en) | A kind of agricultural robot integrated navigation information fusion method based on order filtering | |
CN106406092B (en) | A kind of robust identification method suitable for helicopter adaptive flight control system | |
CN110132287B (en) | Satellite high-precision joint attitude determination method based on extreme learning machine network compensation | |
CN115580261A (en) | State estimation method based on combination of robust strategy and dynamic enhanced CKF | |
CN111649747A (en) | IMU-based adaptive EKF attitude measurement improvement method | |
CN113432612B (en) | Navigation method, device and system for flying object | |
CN114440895A (en) | Atmospheric pressure assisted Wi-Fi/PDR indoor positioning method based on factor graph | |
CN111190207A (en) | Unmanned aerial vehicle INS BDS integrated navigation method based on PSTCSDREF algorithm | |
CN110595434A (en) | Quaternion fusion attitude estimation method based on MEMS sensor | |
CN111310110B (en) | Hybrid state estimation method for high-dimensional coupling uncertain system | |
CN107340026B (en) | Unstable state level gauging value filtering method | |
CN110912535B (en) | Novel non-pilot Kalman filtering method | |
CN113587926A (en) | Spacecraft space autonomous rendezvous and docking relative navigation method | |
CN113075717A (en) | Wavelet self-adaptive neural network subtraction clustering fuzzy inference method and system, positioning equipment and storage medium |
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 |