添加卡尔曼滤波
This commit is contained in:
parent
796dbb2c16
commit
6d73319b54
11
.mxproject
11
.mxproject
File diff suppressed because one or more lines are too long
@ -64,6 +64,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
|
|||||||
User/component/freertos_cli.c
|
User/component/freertos_cli.c
|
||||||
User/component/limiter.c
|
User/component/limiter.c
|
||||||
User/component/lqr.c
|
User/component/lqr.c
|
||||||
|
User/component/kalman_filter.c
|
||||||
User/component/pid.c
|
User/component/pid.c
|
||||||
User/component/user_math.c
|
User/component/user_math.c
|
||||||
User/component/vmc.c
|
User/component/vmc.c
|
||||||
|
|||||||
@ -280,11 +280,13 @@ Mcu.Pin61=VP_FREERTOS_VS_CMSIS_V2
|
|||||||
Mcu.Pin62=VP_OCTOSPI1_VS_octo
|
Mcu.Pin62=VP_OCTOSPI1_VS_octo
|
||||||
Mcu.Pin63=VP_SYS_VS_tim23
|
Mcu.Pin63=VP_SYS_VS_tim23
|
||||||
Mcu.Pin64=VP_MEMORYMAP_VS_MEMORYMAP
|
Mcu.Pin64=VP_MEMORYMAP_VS_MEMORYMAP
|
||||||
|
Mcu.Pin65=VP_STMicroelectronics.X-CUBE-ALGOBUILD_VS_DSPOoLibraryJjLibrary_1.4.0_1.4.0
|
||||||
Mcu.Pin7=PC0
|
Mcu.Pin7=PC0
|
||||||
Mcu.Pin8=PC1
|
Mcu.Pin8=PC1
|
||||||
Mcu.Pin9=PC2_C
|
Mcu.Pin9=PC2_C
|
||||||
Mcu.PinsNb=65
|
Mcu.PinsNb=66
|
||||||
Mcu.ThirdPartyNb=0
|
Mcu.ThirdParty0=STMicroelectronics.X-CUBE-ALGOBUILD.1.4.0
|
||||||
|
Mcu.ThirdPartyNb=1
|
||||||
Mcu.UserConstants=
|
Mcu.UserConstants=
|
||||||
Mcu.UserName=STM32H723VGTx
|
Mcu.UserName=STM32H723VGTx
|
||||||
MxCube.Version=6.15.0
|
MxCube.Version=6.15.0
|
||||||
@ -549,7 +551,7 @@ ProjectManager.ProjectName=CtrBoard-H7_ALL
|
|||||||
ProjectManager.ProjectStructure=
|
ProjectManager.ProjectStructure=
|
||||||
ProjectManager.RegisterCallBack=
|
ProjectManager.RegisterCallBack=
|
||||||
ProjectManager.StackSize=0x2000
|
ProjectManager.StackSize=0x2000
|
||||||
ProjectManager.TargetToolchain=MDK-ARM V5.32
|
ProjectManager.TargetToolchain=CMake
|
||||||
ProjectManager.ToolChainLocation=
|
ProjectManager.ToolChainLocation=
|
||||||
ProjectManager.UAScriptAfterPath=
|
ProjectManager.UAScriptAfterPath=
|
||||||
ProjectManager.UAScriptBeforePath=
|
ProjectManager.UAScriptBeforePath=
|
||||||
@ -681,6 +683,10 @@ SPI2.Direction=SPI_DIRECTION_2LINES
|
|||||||
SPI2.IPParameters=VirtualType,Mode,Direction,CalculateBaudRate,DataSize,BaudRatePrescaler,CLKPolarity,CLKPhase
|
SPI2.IPParameters=VirtualType,Mode,Direction,CalculateBaudRate,DataSize,BaudRatePrescaler,CLKPolarity,CLKPhase
|
||||||
SPI2.Mode=SPI_MODE_MASTER
|
SPI2.Mode=SPI_MODE_MASTER
|
||||||
SPI2.VirtualType=VM_MASTER
|
SPI2.VirtualType=VM_MASTER
|
||||||
|
STMicroelectronics.X-CUBE-ALGOBUILD.1.4.0.DSPOoLibraryJjLibrary_Checked=true
|
||||||
|
STMicroelectronics.X-CUBE-ALGOBUILD.1.4.0.IPParameters=LibraryCcDSPOoLibraryJjDSPOoLibrary
|
||||||
|
STMicroelectronics.X-CUBE-ALGOBUILD.1.4.0.LibraryCcDSPOoLibraryJjDSPOoLibrary=true
|
||||||
|
STMicroelectronics.X-CUBE-ALGOBUILD.1.4.0_SwParameter=LibraryCcDSPOoLibraryJjDSPOoLibrary\:true;
|
||||||
TIM1.Channel-PWM\ Generation1\ CH1=TIM_CHANNEL_1
|
TIM1.Channel-PWM\ Generation1\ CH1=TIM_CHANNEL_1
|
||||||
TIM1.Channel-PWM\ Generation3\ CH3=TIM_CHANNEL_3
|
TIM1.Channel-PWM\ Generation3\ CH3=TIM_CHANNEL_3
|
||||||
TIM1.IPParameters=Channel-PWM Generation3 CH3,Channel-PWM Generation1 CH1,Period,Pulse-PWM Generation1 CH1,Pulse-PWM Generation3 CH3,Prescaler
|
TIM1.IPParameters=Channel-PWM Generation3 CH3,Channel-PWM Generation1 CH1,Period,Pulse-PWM Generation1 CH1,Pulse-PWM Generation3 CH3,Prescaler
|
||||||
@ -734,6 +740,8 @@ VP_MEMORYMAP_VS_MEMORYMAP.Mode=CurAppReg
|
|||||||
VP_MEMORYMAP_VS_MEMORYMAP.Signal=MEMORYMAP_VS_MEMORYMAP
|
VP_MEMORYMAP_VS_MEMORYMAP.Signal=MEMORYMAP_VS_MEMORYMAP
|
||||||
VP_OCTOSPI1_VS_octo.Mode=octo_mode
|
VP_OCTOSPI1_VS_octo.Mode=octo_mode
|
||||||
VP_OCTOSPI1_VS_octo.Signal=OCTOSPI1_VS_octo
|
VP_OCTOSPI1_VS_octo.Signal=OCTOSPI1_VS_octo
|
||||||
|
VP_STMicroelectronics.X-CUBE-ALGOBUILD_VS_DSPOoLibraryJjLibrary_1.4.0_1.4.0.Mode=DSPOoLibraryJjLibrary
|
||||||
|
VP_STMicroelectronics.X-CUBE-ALGOBUILD_VS_DSPOoLibraryJjLibrary_1.4.0_1.4.0.Signal=STMicroelectronics.X-CUBE-ALGOBUILD_VS_DSPOoLibraryJjLibrary_1.4.0_1.4.0
|
||||||
VP_SYS_VS_tim23.Mode=TIM23
|
VP_SYS_VS_tim23.Mode=TIM23
|
||||||
VP_SYS_VS_tim23.Signal=SYS_VS_tim23
|
VP_SYS_VS_tim23.Signal=SYS_VS_tim23
|
||||||
board=custom
|
board=custom
|
||||||
|
|||||||
Binary file not shown.
@ -7236,7 +7236,7 @@
|
|||||||
:10C410009DF93F0010B080BD0000000000C0DA456B
|
:10C410009DF93F0010B080BD0000000000C0DA456B
|
||||||
:10C4200080B588B0069005918DED040A069858B144
|
:10C4200080B588B0069005918DED040A069858B144
|
||||||
:10C43000FFE7059840B1FFE79DED040AB5EE400A1D
|
:10C43000FFE7059840B1FFE79DED040AB5EE400A1D
|
||||||
:10C44000F1EE10FA04DCFFE7FF208DF81F0093E007
|
:10C44000F1EE10FA04DCFFE7FF208DF81F0092E008
|
||||||
:10C45000059806994861E7F7B7FA00200390FFE7CF
|
:10C45000059806994861E7F7B7FA00200390FFE7CF
|
||||||
:10C460000398012845DCFFE70598039900EBC1001C
|
:10C460000398012845DCFFE70598039900EBC1001C
|
||||||
:10C470000A30FDF769FB06980399C1EB011100EB47
|
:10C470000A30FDF769FB06980399C1EB011100EB47
|
||||||
@ -7254,8 +7254,8 @@
|
|||||||
:10C530004969D1ED2B0AFBF73FFC069901F52C70F8
|
:10C530004969D1ED2B0AFBF73FFC069901F52C70F8
|
||||||
:10C540009DED040A4969D1ED2C0AFBF735FC0699EB
|
:10C540009DED040A4969D1ED2C0AFBF735FC0699EB
|
||||||
:10C55000002001900867C866069874302C21E4F723
|
:10C55000002001900867C866069874302C21E4F723
|
||||||
:10C5600015F90198069A48F20001C4F2BB51C2F8CD
|
:10C5600015F90198069A0021C4F2FA41C2F8A01008
|
||||||
:10C57000A0108DF81F00FFE79DF91F0008B080BDD7
|
:10C570008DF81F00FFE79DF91F0008B080BD000087
|
||||||
:10C5800080B586B00490049820B9FFE7FF208DF8AD
|
:10C5800080B586B00490049820B9FFE7FF208DF8AD
|
||||||
:10C59000170052E000200390FFE7039801282EDCEB
|
:10C59000170052E000200390FFE7039801282EDCEB
|
||||||
:10C5A000FFE704980399C1EB011100EB8100A8306B
|
:10C5A000FFE704980399C1EB011100EB8100A8306B
|
||||||
@ -7676,8 +7676,8 @@
|
|||||||
:10DF900090F8390001460691012808D0FFE706985D
|
:10DF900090F8390001460691012808D0FFE706985D
|
||||||
:10DFA000022812D0FFE70698032807D014E041F2B8
|
:10DFA000022812D0FFE70698032807D014E041F2B8
|
||||||
:10DFB0009C31C2F200410020087014E041F29C3113
|
:10DFB0009C31C2F200410020087014E041F29C3113
|
||||||
:10DFC000C2F20041022008700DE041F29C31C2F221
|
:10DFC000C2F20041012008700DE041F29C31C2F222
|
||||||
:10DFD00000410320087006E041F29C31C2F200418A
|
:10DFD00000410220087006E041F29C31C2F200418B
|
||||||
:10DFE00000200870FFE741F2C830C2F20040816AA9
|
:10DFE00000200870FFE741F2C830C2F20040816AA9
|
||||||
:10DFF00041F29C32C2F200425160416A9160C16AB2
|
:10DFF00041F29C32C2F200425160416A9160C16AB2
|
||||||
:10E00000D160406B0C9000200B909DED0C0A9DEDB3
|
:10E00000D160406B0C9000200B909DED0C0A9DEDB3
|
||||||
|
|||||||
8970
Middlewares/ST/ARM/DSP/Inc/arm_math.h
Normal file
8970
Middlewares/ST/ARM/DSP/Inc/arm_math.h
Normal file
File diff suppressed because it is too large
Load Diff
201
Middlewares/Third_Party/ARM/DSP/LICENSE.txt
vendored
Normal file
201
Middlewares/Third_Party/ARM/DSP/LICENSE.txt
vendored
Normal file
@ -0,0 +1,201 @@
|
|||||||
|
Apache License
|
||||||
|
Version 2.0, January 2004
|
||||||
|
http://www.apache.org/licenses/
|
||||||
|
|
||||||
|
TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
|
||||||
|
|
||||||
|
1. Definitions.
|
||||||
|
|
||||||
|
"License" shall mean the terms and conditions for use, reproduction,
|
||||||
|
and distribution as defined by Sections 1 through 9 of this document.
|
||||||
|
|
||||||
|
"Licensor" shall mean the copyright owner or entity authorized by
|
||||||
|
the copyright owner that is granting the License.
|
||||||
|
|
||||||
|
"Legal Entity" shall mean the union of the acting entity and all
|
||||||
|
other entities that control, are controlled by, or are under common
|
||||||
|
control with that entity. For the purposes of this definition,
|
||||||
|
"control" means (i) the power, direct or indirect, to cause the
|
||||||
|
direction or management of such entity, whether by contract or
|
||||||
|
otherwise, or (ii) ownership of fifty percent (50%) or more of the
|
||||||
|
outstanding shares, or (iii) beneficial ownership of such entity.
|
||||||
|
|
||||||
|
"You" (or "Your") shall mean an individual or Legal Entity
|
||||||
|
exercising permissions granted by this License.
|
||||||
|
|
||||||
|
"Source" form shall mean the preferred form for making modifications,
|
||||||
|
including but not limited to software source code, documentation
|
||||||
|
source, and configuration files.
|
||||||
|
|
||||||
|
"Object" form shall mean any form resulting from mechanical
|
||||||
|
transformation or translation of a Source form, including but
|
||||||
|
not limited to compiled object code, generated documentation,
|
||||||
|
and conversions to other media types.
|
||||||
|
|
||||||
|
"Work" shall mean the work of authorship, whether in Source or
|
||||||
|
Object form, made available under the License, as indicated by a
|
||||||
|
copyright notice that is included in or attached to the work
|
||||||
|
(an example is provided in the Appendix below).
|
||||||
|
|
||||||
|
"Derivative Works" shall mean any work, whether in Source or Object
|
||||||
|
form, that is based on (or derived from) the Work and for which the
|
||||||
|
editorial revisions, annotations, elaborations, or other modifications
|
||||||
|
represent, as a whole, an original work of authorship. For the purposes
|
||||||
|
of this License, Derivative Works shall not include works that remain
|
||||||
|
separable from, or merely link (or bind by name) to the interfaces of,
|
||||||
|
the Work and Derivative Works thereof.
|
||||||
|
|
||||||
|
"Contribution" shall mean any work of authorship, including
|
||||||
|
the original version of the Work and any modifications or additions
|
||||||
|
to that Work or Derivative Works thereof, that is intentionally
|
||||||
|
submitted to Licensor for inclusion in the Work by the copyright owner
|
||||||
|
or by an individual or Legal Entity authorized to submit on behalf of
|
||||||
|
the copyright owner. For the purposes of this definition, "submitted"
|
||||||
|
means any form of electronic, verbal, or written communication sent
|
||||||
|
to the Licensor or its representatives, including but not limited to
|
||||||
|
communication on electronic mailing lists, source code control systems,
|
||||||
|
and issue tracking systems that are managed by, or on behalf of, the
|
||||||
|
Licensor for the purpose of discussing and improving the Work, but
|
||||||
|
excluding communication that is conspicuously marked or otherwise
|
||||||
|
designated in writing by the copyright owner as "Not a Contribution."
|
||||||
|
|
||||||
|
"Contributor" shall mean Licensor and any individual or Legal Entity
|
||||||
|
on behalf of whom a Contribution has been received by Licensor and
|
||||||
|
subsequently incorporated within the Work.
|
||||||
|
|
||||||
|
2. Grant of Copyright License. Subject to the terms and conditions of
|
||||||
|
this License, each Contributor hereby grants to You a perpetual,
|
||||||
|
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
|
||||||
|
copyright license to reproduce, prepare Derivative Works of,
|
||||||
|
publicly display, publicly perform, sublicense, and distribute the
|
||||||
|
Work and such Derivative Works in Source or Object form.
|
||||||
|
|
||||||
|
3. Grant of Patent License. Subject to the terms and conditions of
|
||||||
|
this License, each Contributor hereby grants to You a perpetual,
|
||||||
|
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
|
||||||
|
(except as stated in this section) patent license to make, have made,
|
||||||
|
use, offer to sell, sell, import, and otherwise transfer the Work,
|
||||||
|
where such license applies only to those patent claims licensable
|
||||||
|
by such Contributor that are necessarily infringed by their
|
||||||
|
Contribution(s) alone or by combination of their Contribution(s)
|
||||||
|
with the Work to which such Contribution(s) was submitted. If You
|
||||||
|
institute patent litigation against any entity (including a
|
||||||
|
cross-claim or counterclaim in a lawsuit) alleging that the Work
|
||||||
|
or a Contribution incorporated within the Work constitutes direct
|
||||||
|
or contributory patent infringement, then any patent licenses
|
||||||
|
granted to You under this License for that Work shall terminate
|
||||||
|
as of the date such litigation is filed.
|
||||||
|
|
||||||
|
4. Redistribution. You may reproduce and distribute copies of the
|
||||||
|
Work or Derivative Works thereof in any medium, with or without
|
||||||
|
modifications, and in Source or Object form, provided that You
|
||||||
|
meet the following conditions:
|
||||||
|
|
||||||
|
(a) You must give any other recipients of the Work or
|
||||||
|
Derivative Works a copy of this License; and
|
||||||
|
|
||||||
|
(b) You must cause any modified files to carry prominent notices
|
||||||
|
stating that You changed the files; and
|
||||||
|
|
||||||
|
(c) You must retain, in the Source form of any Derivative Works
|
||||||
|
that You distribute, all copyright, patent, trademark, and
|
||||||
|
attribution notices from the Source form of the Work,
|
||||||
|
excluding those notices that do not pertain to any part of
|
||||||
|
the Derivative Works; and
|
||||||
|
|
||||||
|
(d) If the Work includes a "NOTICE" text file as part of its
|
||||||
|
distribution, then any Derivative Works that You distribute must
|
||||||
|
include a readable copy of the attribution notices contained
|
||||||
|
within such NOTICE file, excluding those notices that do not
|
||||||
|
pertain to any part of the Derivative Works, in at least one
|
||||||
|
of the following places: within a NOTICE text file distributed
|
||||||
|
as part of the Derivative Works; within the Source form or
|
||||||
|
documentation, if provided along with the Derivative Works; or,
|
||||||
|
within a display generated by the Derivative Works, if and
|
||||||
|
wherever such third-party notices normally appear. The contents
|
||||||
|
of the NOTICE file are for informational purposes only and
|
||||||
|
do not modify the License. You may add Your own attribution
|
||||||
|
notices within Derivative Works that You distribute, alongside
|
||||||
|
or as an addendum to the NOTICE text from the Work, provided
|
||||||
|
that such additional attribution notices cannot be construed
|
||||||
|
as modifying the License.
|
||||||
|
|
||||||
|
You may add Your own copyright statement to Your modifications and
|
||||||
|
may provide additional or different license terms and conditions
|
||||||
|
for use, reproduction, or distribution of Your modifications, or
|
||||||
|
for any such Derivative Works as a whole, provided Your use,
|
||||||
|
reproduction, and distribution of the Work otherwise complies with
|
||||||
|
the conditions stated in this License.
|
||||||
|
|
||||||
|
5. Submission of Contributions. Unless You explicitly state otherwise,
|
||||||
|
any Contribution intentionally submitted for inclusion in the Work
|
||||||
|
by You to the Licensor shall be under the terms and conditions of
|
||||||
|
this License, without any additional terms or conditions.
|
||||||
|
Notwithstanding the above, nothing herein shall supersede or modify
|
||||||
|
the terms of any separate license agreement you may have executed
|
||||||
|
with Licensor regarding such Contributions.
|
||||||
|
|
||||||
|
6. Trademarks. This License does not grant permission to use the trade
|
||||||
|
names, trademarks, service marks, or product names of the Licensor,
|
||||||
|
except as required for reasonable and customary use in describing the
|
||||||
|
origin of the Work and reproducing the content of the NOTICE file.
|
||||||
|
|
||||||
|
7. Disclaimer of Warranty. Unless required by applicable law or
|
||||||
|
agreed to in writing, Licensor provides the Work (and each
|
||||||
|
Contributor provides its Contributions) on an "AS IS" BASIS,
|
||||||
|
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
|
||||||
|
implied, including, without limitation, any warranties or conditions
|
||||||
|
of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
|
||||||
|
PARTICULAR PURPOSE. You are solely responsible for determining the
|
||||||
|
appropriateness of using or redistributing the Work and assume any
|
||||||
|
risks associated with Your exercise of permissions under this License.
|
||||||
|
|
||||||
|
8. Limitation of Liability. In no event and under no legal theory,
|
||||||
|
whether in tort (including negligence), contract, or otherwise,
|
||||||
|
unless required by applicable law (such as deliberate and grossly
|
||||||
|
negligent acts) or agreed to in writing, shall any Contributor be
|
||||||
|
liable to You for damages, including any direct, indirect, special,
|
||||||
|
incidental, or consequential damages of any character arising as a
|
||||||
|
result of this License or out of the use or inability to use the
|
||||||
|
Work (including but not limited to damages for loss of goodwill,
|
||||||
|
work stoppage, computer failure or malfunction, or any and all
|
||||||
|
other commercial damages or losses), even if such Contributor
|
||||||
|
has been advised of the possibility of such damages.
|
||||||
|
|
||||||
|
9. Accepting Warranty or Additional Liability. While redistributing
|
||||||
|
the Work or Derivative Works thereof, You may choose to offer,
|
||||||
|
and charge a fee for, acceptance of support, warranty, indemnity,
|
||||||
|
or other liability obligations and/or rights consistent with this
|
||||||
|
License. However, in accepting such obligations, You may act only
|
||||||
|
on Your own behalf and on Your sole responsibility, not on behalf
|
||||||
|
of any other Contributor, and only if You agree to indemnify,
|
||||||
|
defend, and hold each Contributor harmless for any liability
|
||||||
|
incurred by, or claims asserted against, such Contributor by reason
|
||||||
|
of your accepting any such warranty or additional liability.
|
||||||
|
|
||||||
|
END OF TERMS AND CONDITIONS
|
||||||
|
|
||||||
|
APPENDIX: How to apply the Apache License to your work.
|
||||||
|
|
||||||
|
To apply the Apache License to your work, attach the following
|
||||||
|
boilerplate notice, with the fields enclosed by brackets "{}"
|
||||||
|
replaced with your own identifying information. (Don't include
|
||||||
|
the brackets!) The text should be enclosed in the appropriate
|
||||||
|
comment syntax for the file format. We also recommend that a
|
||||||
|
file or class name and description of purpose be included on the
|
||||||
|
same "printed page" as the copyright notice for easier
|
||||||
|
identification within third-party archives.
|
||||||
|
|
||||||
|
Copyright {yyyy} {name of copyright owner}
|
||||||
|
|
||||||
|
Licensed under the Apache License, Version 2.0 (the "License");
|
||||||
|
you may not use this file except in compliance with the License.
|
||||||
|
You may obtain a copy of the License at
|
||||||
|
|
||||||
|
http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
|
||||||
|
Unless required by applicable law or agreed to in writing, software
|
||||||
|
distributed under the License is distributed on an "AS IS" BASIS,
|
||||||
|
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||||
|
See the License for the specific language governing permissions and
|
||||||
|
limitations under the License.
|
||||||
488
User/component/kalman_filter.c
Normal file
488
User/component/kalman_filter.c
Normal file
@ -0,0 +1,488 @@
|
|||||||
|
/**
|
||||||
|
******************************************************************************
|
||||||
|
* @file kalman filter.c
|
||||||
|
* @author Wang Hongxi
|
||||||
|
* @version V1.2.2
|
||||||
|
* @date 2022/1/8
|
||||||
|
* @brief C implementation of kalman filter
|
||||||
|
******************************************************************************
|
||||||
|
* @attention
|
||||||
|
* 该卡尔曼滤波器可以在传感器采样频率不同的情况下，动态调整矩阵H R和K的维数与数值。
|
||||||
|
* This implementation of kalman filter can dynamically adjust dimension and
|
||||||
|
* value of matrix H R and K according to the measurement validity under any
|
||||||
|
* circumstance that the sampling rate of component sensors are different.
|
||||||
|
*
|
||||||
|
* 因此矩阵H和R的初始化会与矩阵P A和Q有所不同。另外的，在初始化量测向量z时需要额外写
|
||||||
|
* 入传感器量测所对应的状态与这个量测的方式，详情请见例程
|
||||||
|
* Therefore, the initialization of matrix P, F, and Q is sometimes different
|
||||||
|
* from that of matrices H R. when initialization. Additionally, the corresponding
|
||||||
|
* state and the method of the measurement should be provided when initializing
|
||||||
|
* measurement vector z. For more details, please see the example.
|
||||||
|
*
|
||||||
|
* 若不需要动态调整量测向量z，可简单将结构体中的Use_Auto_Adjustment初始化为0，并像初
|
||||||
|
* 始化矩阵P那样用常规方式初始化z H R即可。
|
||||||
|
* If automatic adjustment is not required, assign zero to the UseAutoAdjustment
|
||||||
|
* and initialize z H R in the normal way as matrix P.
|
||||||
|
*
|
||||||
|
* 要求量测向量z与控制向量u在传感器回调函数中更新。整数0意味着量测无效，即自上次卡尔曼
|
||||||
|
* 滤波更新后无传感器数据更新。因此量测向量z与控制向量u会在卡尔曼滤波更新过程中被清零
|
||||||
|
* MeasuredVector and ControlVector are required to be updated in the sensor
|
||||||
|
* callback function. Integer 0 in measurement vector z indicates the invalidity
|
||||||
|
* of current measurement, so MeasuredVector and ControlVector will be reset
|
||||||
|
* (to 0) during each update.
|
||||||
|
*
|
||||||
|
* 此外，矩阵P过度收敛后滤波器将难以再适应状态的缓慢变化，从而产生滤波估计偏差。该算法
|
||||||
|
* 通过限制矩阵P最小值的方法，可有效抑制滤波器的过度收敛，详情请见例程。
|
||||||
|
* Additionally, the excessive convergence of matrix P will make filter incapable
|
||||||
|
* of adopting the slowly changing state. This implementation can effectively
|
||||||
|
* suppress filter excessive convergence through boundary limiting for matrix P.
|
||||||
|
* For more details, please see the example.
|
||||||
|
*
|
||||||
|
* @example:
|
||||||
|
* x =
|
||||||
|
* | height |
|
||||||
|
* | velocity |
|
||||||
|
* |acceleration|
|
||||||
|
*
|
||||||
|
* KalmanFilter_t Height_KF;
|
||||||
|
*
|
||||||
|
* void INS_Task_Init(void)
|
||||||
|
* {
|
||||||
|
* static float P_Init[9] =
|
||||||
|
* {
|
||||||
|
* 10, 0, 0,
|
||||||
|
* 0, 30, 0,
|
||||||
|
* 0, 0, 10,
|
||||||
|
* };
|
||||||
|
* static float F_Init[9] =
|
||||||
|
* {
|
||||||
|
* 1, dt, 0.5*dt*dt,
|
||||||
|
* 0, 1, dt,
|
||||||
|
* 0, 0, 1,
|
||||||
|
* };
|
||||||
|
* static float Q_Init[9] =
|
||||||
|
* {
|
||||||
|
* 0.25*dt*dt*dt*dt, 0.5*dt*dt*dt, 0.5*dt*dt,
|
||||||
|
* 0.5*dt*dt*dt, dt*dt, dt,
|
||||||
|
* 0.5*dt*dt, dt, 1,
|
||||||
|
* };
|
||||||
|
*
|
||||||
|
* // 设置最小方差
|
||||||
|
* static float state_min_variance[3] = {0.03, 0.005, 0.1};
|
||||||
|
*
|
||||||
|
* // 开启自动调整
|
||||||
|
* Height_KF.UseAutoAdjustment = 1;
|
||||||
|
*
|
||||||
|
* // 气压测得高度 GPS测得高度 加速度计测得z轴运动加速度
|
||||||
|
* static uint8_t measurement_reference[3] = {1, 1, 3}
|
||||||
|
*
|
||||||
|
* static float measurement_degree[3] = {1, 1, 1}
|
||||||
|
* // 根据measurement_reference与measurement_degree生成H矩阵如下（在当前周期全部测量数据有效情况下）
|
||||||
|
* |1 0 0|
|
||||||
|
* |1 0 0|
|
||||||
|
* |0 0 1|
|
||||||
|
*
|
||||||
|
* static float mat_R_diagonal_elements = {30, 25, 35}
|
||||||
|
* //根据mat_R_diagonal_elements生成R矩阵如下（在当前周期全部测量数据有效情况下）
|
||||||
|
* |30 0 0|
|
||||||
|
* | 0 25 0|
|
||||||
|
* | 0 0 35|
|
||||||
|
*
|
||||||
|
* Kalman_Filter_Init(&Height_KF, 3, 0, 3);
|
||||||
|
*
|
||||||
|
* // 设置矩阵值
|
||||||
|
* memcpy(Height_KF.P_data, P_Init, sizeof(P_Init));
|
||||||
|
* memcpy(Height_KF.F_data, F_Init, sizeof(F_Init));
|
||||||
|
* memcpy(Height_KF.Q_data, Q_Init, sizeof(Q_Init));
|
||||||
|
* memcpy(Height_KF.MeasurementMap, measurement_reference, sizeof(measurement_reference));
|
||||||
|
* memcpy(Height_KF.MeasurementDegree, measurement_degree, sizeof(measurement_degree));
|
||||||
|
* memcpy(Height_KF.MatR_DiagonalElements, mat_R_diagonal_elements, sizeof(mat_R_diagonal_elements));
|
||||||
|
* memcpy(Height_KF.StateMinVariance, state_min_variance, sizeof(state_min_variance));
|
||||||
|
* }
|
||||||
|
*
|
||||||
|
* void INS_Task(void const *pvParameters)
|
||||||
|
* {
|
||||||
|
* // 循环更新
|
||||||
|
* Kalman_Filter_Update(&Height_KF);
|
||||||
|
* vTaskDelay(ts);
|
||||||
|
* }
|
||||||
|
*
|
||||||
|
* // 测量数据更新应按照以下形式 即向MeasuredVector赋值
|
||||||
|
* void Barometer_Read_Over(void)
|
||||||
|
* {
|
||||||
|
* ......
|
||||||
|
* INS_KF.MeasuredVector[0] = baro_height;
|
||||||
|
* }
|
||||||
|
* void GPS_Read_Over(void)
|
||||||
|
* {
|
||||||
|
* ......
|
||||||
|
* INS_KF.MeasuredVector[1] = GPS_height;
|
||||||
|
* }
|
||||||
|
* void Acc_Data_Process(void)
|
||||||
|
* {
|
||||||
|
* ......
|
||||||
|
* INS_KF.MeasuredVector[2] = acc.z;
|
||||||
|
* }
|
||||||
|
******************************************************************************
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "kalman_filter.h"
|
||||||
|
|
||||||
|
uint16_t sizeof_float, sizeof_double;
|
||||||
|
|
||||||
|
static void H_K_R_Adjustment(KalmanFilter_t *kf);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 初始化矩阵维度信息并为矩阵分配空间
|
||||||
|
*
|
||||||
|
* @param kf kf类型定义
|
||||||
|
* @param xhatSize 状态变量维度
|
||||||
|
* @param uSize 控制变量维度
|
||||||
|
* @param zSize 观测量维度
|
||||||
|
*/
|
||||||
|
void Kalman_Filter_Init(KalmanFilter_t *kf, uint8_t xhatSize, uint8_t uSize, uint8_t zSize)
|
||||||
|
{
|
||||||
|
sizeof_float = sizeof(float);
|
||||||
|
sizeof_double = sizeof(double);
|
||||||
|
|
||||||
|
kf->xhatSize = xhatSize;
|
||||||
|
kf->uSize = uSize;
|
||||||
|
kf->zSize = zSize;
|
||||||
|
|
||||||
|
kf->MeasurementValidNum = 0;
|
||||||
|
|
||||||
|
// measurement flags
|
||||||
|
kf->MeasurementMap = (uint8_t *)user_malloc(sizeof(uint8_t) * zSize);
|
||||||
|
memset(kf->MeasurementMap, 0, sizeof(uint8_t) * zSize);
|
||||||
|
kf->MeasurementDegree = (float *)user_malloc(sizeof_float * zSize);
|
||||||
|
memset(kf->MeasurementDegree, 0, sizeof_float * zSize);
|
||||||
|
kf->MatR_DiagonalElements = (float *)user_malloc(sizeof_float * zSize);
|
||||||
|
memset(kf->MatR_DiagonalElements, 0, sizeof_float * zSize);
|
||||||
|
kf->StateMinVariance = (float *)user_malloc(sizeof_float * xhatSize);
|
||||||
|
memset(kf->StateMinVariance, 0, sizeof_float * xhatSize);
|
||||||
|
kf->temp = (uint8_t *)user_malloc(sizeof(uint8_t) * zSize);
|
||||||
|
memset(kf->temp, 0, sizeof(uint8_t) * zSize);
|
||||||
|
|
||||||
|
// filter data
|
||||||
|
kf->FilteredValue = (float *)user_malloc(sizeof_float * xhatSize);
|
||||||
|
memset(kf->FilteredValue, 0, sizeof_float * xhatSize);
|
||||||
|
kf->MeasuredVector = (float *)user_malloc(sizeof_float * zSize);
|
||||||
|
memset(kf->MeasuredVector, 0, sizeof_float * zSize);
|
||||||
|
kf->ControlVector = (float *)user_malloc(sizeof_float * uSize);
|
||||||
|
memset(kf->ControlVector, 0, sizeof_float * uSize);
|
||||||
|
|
||||||
|
// xhat x(k|k)
|
||||||
|
kf->xhat_data = (float *)user_malloc(sizeof_float * xhatSize);
|
||||||
|
memset(kf->xhat_data, 0, sizeof_float * xhatSize);
|
||||||
|
Matrix_Init(&kf->xhat, kf->xhatSize, 1, (float *)kf->xhat_data);
|
||||||
|
|
||||||
|
// xhatminus x(k|k-1)
|
||||||
|
kf->xhatminus_data = (float *)user_malloc(sizeof_float * xhatSize);
|
||||||
|
memset(kf->xhatminus_data, 0, sizeof_float * xhatSize);
|
||||||
|
Matrix_Init(&kf->xhatminus, kf->xhatSize, 1, (float *)kf->xhatminus_data);
|
||||||
|
|
||||||
|
if (uSize != 0)
|
||||||
|
{
|
||||||
|
// control vector u
|
||||||
|
kf->u_data = (float *)user_malloc(sizeof_float * uSize);
|
||||||
|
memset(kf->u_data, 0, sizeof_float * uSize);
|
||||||
|
Matrix_Init(&kf->u, kf->uSize, 1, (float *)kf->u_data);
|
||||||
|
}
|
||||||
|
|
||||||
|
// measurement vector z
|
||||||
|
kf->z_data = (float *)user_malloc(sizeof_float * zSize);
|
||||||
|
memset(kf->z_data, 0, sizeof_float * zSize);
|
||||||
|
Matrix_Init(&kf->z, kf->zSize, 1, (float *)kf->z_data);
|
||||||
|
|
||||||
|
// covariance matrix P(k|k)
|
||||||
|
kf->P_data = (float *)user_malloc(sizeof_float * xhatSize * xhatSize);
|
||||||
|
memset(kf->P_data, 0, sizeof_float * xhatSize * xhatSize);
|
||||||
|
Matrix_Init(&kf->P, kf->xhatSize, kf->xhatSize, (float *)kf->P_data);
|
||||||
|
|
||||||
|
// create covariance matrix P(k|k-1)
|
||||||
|
kf->Pminus_data = (float *)user_malloc(sizeof_float * xhatSize * xhatSize);
|
||||||
|
memset(kf->Pminus_data, 0, sizeof_float * xhatSize * xhatSize);
|
||||||
|
Matrix_Init(&kf->Pminus, kf->xhatSize, kf->xhatSize, (float *)kf->Pminus_data);
|
||||||
|
|
||||||
|
// state transition matrix F FT
|
||||||
|
kf->F_data = (float *)user_malloc(sizeof_float * xhatSize * xhatSize);
|
||||||
|
kf->FT_data = (float *)user_malloc(sizeof_float * xhatSize * xhatSize);
|
||||||
|
memset(kf->F_data, 0, sizeof_float * xhatSize * xhatSize);
|
||||||
|
memset(kf->FT_data, 0, sizeof_float * xhatSize * xhatSize);
|
||||||
|
Matrix_Init(&kf->F, kf->xhatSize, kf->xhatSize, (float *)kf->F_data);
|
||||||
|
Matrix_Init(&kf->FT, kf->xhatSize, kf->xhatSize, (float *)kf->FT_data);
|
||||||
|
|
||||||
|
if (uSize != 0)
|
||||||
|
{
|
||||||
|
// control matrix B
|
||||||
|
kf->B_data = (float *)user_malloc(sizeof_float * xhatSize * uSize);
|
||||||
|
memset(kf->B_data, 0, sizeof_float * xhatSize * uSize);
|
||||||
|
Matrix_Init(&kf->B, kf->xhatSize, kf->uSize, (float *)kf->B_data);
|
||||||
|
}
|
||||||
|
|
||||||
|
// measurement matrix H
|
||||||
|
kf->H_data = (float *)user_malloc(sizeof_float * zSize * xhatSize);
|
||||||
|
kf->HT_data = (float *)user_malloc(sizeof_float * xhatSize * zSize);
|
||||||
|
memset(kf->H_data, 0, sizeof_float * zSize * xhatSize);
|
||||||
|
memset(kf->HT_data, 0, sizeof_float * xhatSize * zSize);
|
||||||
|
Matrix_Init(&kf->H, kf->zSize, kf->xhatSize, (float *)kf->H_data);
|
||||||
|
Matrix_Init(&kf->HT, kf->xhatSize, kf->zSize, (float *)kf->HT_data);
|
||||||
|
|
||||||
|
// process noise covariance matrix Q
|
||||||
|
kf->Q_data = (float *)user_malloc(sizeof_float * xhatSize * xhatSize);
|
||||||
|
memset(kf->Q_data, 0, sizeof_float * xhatSize * xhatSize);
|
||||||
|
Matrix_Init(&kf->Q, kf->xhatSize, kf->xhatSize, (float *)kf->Q_data);
|
||||||
|
|
||||||
|
// measurement noise covariance matrix R
|
||||||
|
kf->R_data = (float *)user_malloc(sizeof_float * zSize * zSize);
|
||||||
|
memset(kf->R_data, 0, sizeof_float * zSize * zSize);
|
||||||
|
Matrix_Init(&kf->R, kf->zSize, kf->zSize, (float *)kf->R_data);
|
||||||
|
|
||||||
|
// kalman gain K
|
||||||
|
kf->K_data = (float *)user_malloc(sizeof_float * xhatSize * zSize);
|
||||||
|
memset(kf->K_data, 0, sizeof_float * xhatSize * zSize);
|
||||||
|
Matrix_Init(&kf->K, kf->xhatSize, kf->zSize, (float *)kf->K_data);
|
||||||
|
|
||||||
|
kf->S_data = (float *)user_malloc(sizeof_float * kf->xhatSize * kf->xhatSize);
|
||||||
|
kf->temp_matrix_data = (float *)user_malloc(sizeof_float * kf->xhatSize * kf->xhatSize);
|
||||||
|
kf->temp_matrix_data1 = (float *)user_malloc(sizeof_float * kf->xhatSize * kf->xhatSize);
|
||||||
|
kf->temp_vector_data = (float *)user_malloc(sizeof_float * kf->xhatSize);
|
||||||
|
kf->temp_vector_data1 = (float *)user_malloc(sizeof_float * kf->xhatSize);
|
||||||
|
Matrix_Init(&kf->S, kf->xhatSize, kf->xhatSize, (float *)kf->S_data);
|
||||||
|
Matrix_Init(&kf->temp_matrix, kf->xhatSize, kf->xhatSize, (float *)kf->temp_matrix_data);
|
||||||
|
Matrix_Init(&kf->temp_matrix1, kf->xhatSize, kf->xhatSize, (float *)kf->temp_matrix_data1);
|
||||||
|
Matrix_Init(&kf->temp_vector, kf->xhatSize, 1, (float *)kf->temp_vector_data);
|
||||||
|
Matrix_Init(&kf->temp_vector1, kf->xhatSize, 1, (float *)kf->temp_vector_data1);
|
||||||
|
|
||||||
|
kf->SkipEq1 = 0;
|
||||||
|
kf->SkipEq2 = 0;
|
||||||
|
kf->SkipEq3 = 0;
|
||||||
|
kf->SkipEq4 = 0;
|
||||||
|
kf->SkipEq5 = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
void Kalman_Filter_Measure(KalmanFilter_t *kf)
|
||||||
|
{
|
||||||
|
// 矩阵H K R根据量测情况自动调整
|
||||||
|
// matrix H K R auto adjustment
|
||||||
|
if (kf->UseAutoAdjustment != 0)
|
||||||
|
H_K_R_Adjustment(kf);
|
||||||
|
else
|
||||||
|
{
|
||||||
|
memcpy(kf->z_data, kf->MeasuredVector, sizeof_float * kf->zSize);
|
||||||
|
memset(kf->MeasuredVector, 0, sizeof_float * kf->zSize);
|
||||||
|
}
|
||||||
|
|
||||||
|
memcpy(kf->u_data, kf->ControlVector, sizeof_float * kf->uSize);
|
||||||
|
}
|
||||||
|
|
||||||
|
extern int stop_time;
|
||||||
|
void Kalman_Filter_xhatMinusUpdate(KalmanFilter_t *kf)
|
||||||
|
{
|
||||||
|
if (!kf->SkipEq1)
|
||||||
|
{
|
||||||
|
if (kf->uSize > 0)
|
||||||
|
{
|
||||||
|
kf->temp_vector.numRows = kf->xhatSize;
|
||||||
|
kf->temp_vector.numCols = 1;
|
||||||
|
// if(stop_time==0)
|
||||||
|
// {
|
||||||
|
// kf->MatStatus = Matrix_Multiply(&kf->temp_F, &kf->xhat, &kf->temp_vector);
|
||||||
|
// }
|
||||||
|
// else
|
||||||
|
// {
|
||||||
|
kf->MatStatus = Matrix_Multiply(&kf->F, &kf->xhat, &kf->temp_vector);
|
||||||
|
// }
|
||||||
|
kf->temp_vector1.numRows = kf->xhatSize;
|
||||||
|
kf->temp_vector1.numCols = 1;
|
||||||
|
kf->MatStatus = Matrix_Multiply(&kf->B, &kf->u, &kf->temp_vector1);
|
||||||
|
kf->MatStatus = Matrix_Add(&kf->temp_vector, &kf->temp_vector1, &kf->xhatminus);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
kf->MatStatus = Matrix_Multiply(&kf->F, &kf->xhat, &kf->xhatminus);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void Kalman_Filter_PminusUpdate(KalmanFilter_t *kf)
|
||||||
|
{
|
||||||
|
if (!kf->SkipEq2)
|
||||||
|
{
|
||||||
|
kf->MatStatus = Matrix_Transpose(&kf->F, &kf->FT);
|
||||||
|
kf->MatStatus = Matrix_Multiply(&kf->F, &kf->P, &kf->Pminus);
|
||||||
|
kf->temp_matrix.numRows = kf->Pminus.numRows;
|
||||||
|
kf->temp_matrix.numCols = kf->FT.numCols;
|
||||||
|
kf->MatStatus = Matrix_Multiply(&kf->Pminus, &kf->FT, &kf->temp_matrix); // temp_matrix = F P(k-1) FT
|
||||||
|
kf->MatStatus = Matrix_Add(&kf->temp_matrix, &kf->Q, &kf->Pminus);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
void Kalman_Filter_SetK(KalmanFilter_t *kf)
|
||||||
|
{
|
||||||
|
if (!kf->SkipEq3)
|
||||||
|
{
|
||||||
|
kf->MatStatus = Matrix_Transpose(&kf->H, &kf->HT); // z|x => x|z
|
||||||
|
kf->temp_matrix.numRows = kf->H.numRows;
|
||||||
|
kf->temp_matrix.numCols = kf->Pminus.numCols;
|
||||||
|
kf->MatStatus = Matrix_Multiply(&kf->H, &kf->Pminus, &kf->temp_matrix); // temp_matrix = H·P'(k)
|
||||||
|
kf->temp_matrix1.numRows = kf->temp_matrix.numRows;
|
||||||
|
kf->temp_matrix1.numCols = kf->HT.numCols;
|
||||||
|
kf->MatStatus = Matrix_Multiply(&kf->temp_matrix, &kf->HT, &kf->temp_matrix1); // temp_matrix1 = H·P'(k)·HT
|
||||||
|
kf->S.numRows = kf->R.numRows;
|
||||||
|
kf->S.numCols = kf->R.numCols;
|
||||||
|
kf->MatStatus = Matrix_Add(&kf->temp_matrix1, &kf->R, &kf->S); // S = H P'(k) HT + R
|
||||||
|
kf->MatStatus = Matrix_Inverse(&kf->S, &kf->temp_matrix1); // temp_matrix1 = inv(H·P'(k)·HT + R)
|
||||||
|
kf->temp_matrix.numRows = kf->Pminus.numRows;
|
||||||
|
kf->temp_matrix.numCols = kf->HT.numCols;
|
||||||
|
kf->MatStatus = Matrix_Multiply(&kf->Pminus, &kf->HT, &kf->temp_matrix); // temp_matrix = P'(k)·HT
|
||||||
|
kf->MatStatus = Matrix_Multiply(&kf->temp_matrix, &kf->temp_matrix1, &kf->K);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
void Kalman_Filter_xhatUpdate(KalmanFilter_t *kf)
|
||||||
|
{
|
||||||
|
if (!kf->SkipEq4)
|
||||||
|
{
|
||||||
|
kf->temp_vector.numRows = kf->H.numRows;
|
||||||
|
kf->temp_vector.numCols = 1;
|
||||||
|
kf->MatStatus = Matrix_Multiply(&kf->H, &kf->xhatminus, &kf->temp_vector); // temp_vector = H xhat'(k)
|
||||||
|
kf->temp_vector1.numRows = kf->z.numRows;
|
||||||
|
kf->temp_vector1.numCols = 1;
|
||||||
|
kf->MatStatus = Matrix_Subtract(&kf->z, &kf->temp_vector, &kf->temp_vector1); // temp_vector1 = z(k) - H·xhat'(k)
|
||||||
|
kf->temp_vector.numRows = kf->K.numRows;
|
||||||
|
kf->temp_vector.numCols = 1;
|
||||||
|
kf->MatStatus = Matrix_Multiply(&kf->K, &kf->temp_vector1, &kf->temp_vector); // temp_vector = K(k)·(z(k) - H·xhat'(k))
|
||||||
|
kf->MatStatus = Matrix_Add(&kf->xhatminus, &kf->temp_vector, &kf->xhat);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
void Kalman_Filter_P_Update(KalmanFilter_t *kf)
|
||||||
|
{
|
||||||
|
if (!kf->SkipEq5)
|
||||||
|
{
|
||||||
|
kf->temp_matrix.numRows = kf->K.numRows;
|
||||||
|
kf->temp_matrix.numCols = kf->H.numCols;
|
||||||
|
kf->temp_matrix1.numRows = kf->temp_matrix.numRows;
|
||||||
|
kf->temp_matrix1.numCols = kf->Pminus.numCols;
|
||||||
|
kf->MatStatus = Matrix_Multiply(&kf->K, &kf->H, &kf->temp_matrix); // temp_matrix = K(k)·H
|
||||||
|
kf->MatStatus = Matrix_Multiply(&kf->temp_matrix, &kf->Pminus, &kf->temp_matrix1); // temp_matrix1 = K(k)·H·P'(k)
|
||||||
|
kf->MatStatus = Matrix_Subtract(&kf->Pminus, &kf->temp_matrix1, &kf->P);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 执行卡尔曼滤波黄金五式,提供了用户定义函数,可以替代五个中的任意一个环节,方便自行扩展为EKF/UKF/ESKF/AUKF等
|
||||||
|
*
|
||||||
|
* @param kf kf类型定义
|
||||||
|
* @return float* 返回滤波值
|
||||||
|
*/
|
||||||
|
float *Kalman_Filter_Update(KalmanFilter_t *kf)
|
||||||
|
{
|
||||||
|
// 0. 获取量测信息
|
||||||
|
Kalman_Filter_Measure(kf);
|
||||||
|
if (kf->User_Func0_f != NULL)
|
||||||
|
kf->User_Func0_f(kf);
|
||||||
|
|
||||||
|
// 先验估计
|
||||||
|
// 1. xhat'(k)= A·xhat(k-1) + B·u
|
||||||
|
Kalman_Filter_xhatMinusUpdate(kf);
|
||||||
|
if (kf->User_Func1_f != NULL)
|
||||||
|
kf->User_Func1_f(kf);
|
||||||
|
|
||||||
|
// 预测更新
|
||||||
|
// 2. P'(k) = A·P(k-1)·AT + Q
|
||||||
|
Kalman_Filter_PminusUpdate(kf);
|
||||||
|
if (kf->User_Func2_f != NULL)
|
||||||
|
kf->User_Func2_f(kf);
|
||||||
|
|
||||||
|
if (kf->MeasurementValidNum != 0 || kf->UseAutoAdjustment == 0)
|
||||||
|
{
|
||||||
|
// 量测更新
|
||||||
|
// 3. K(k) = P'(k)·HT / (H·P'(k)·HT + R)
|
||||||
|
Kalman_Filter_SetK(kf);
|
||||||
|
|
||||||
|
if (kf->User_Func3_f != NULL)
|
||||||
|
kf->User_Func3_f(kf);
|
||||||
|
|
||||||
|
// 融合
|
||||||
|
// 4. xhat(k) = xhat'(k) + K(k)·(z(k) - H·xhat'(k))
|
||||||
|
Kalman_Filter_xhatUpdate(kf);
|
||||||
|
|
||||||
|
if (kf->User_Func4_f != NULL)
|
||||||
|
kf->User_Func4_f(kf);
|
||||||
|
|
||||||
|
// 修正方差
|
||||||
|
// 5. P(k) = (1-K(k)·H)·P'(k) ==> P(k) = P'(k)-K(k)·H·P'(k)
|
||||||
|
Kalman_Filter_P_Update(kf);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
// 无有效量测,仅预测
|
||||||
|
// xhat(k) = xhat'(k)
|
||||||
|
// P(k) = P'(k)
|
||||||
|
memcpy(kf->xhat_data, kf->xhatminus_data, sizeof_float * kf->xhatSize);
|
||||||
|
memcpy(kf->P_data, kf->Pminus_data, sizeof_float * kf->xhatSize * kf->xhatSize);
|
||||||
|
}
|
||||||
|
|
||||||
|
// 自定义函数,可以提供后处理等
|
||||||
|
if (kf->User_Func5_f != NULL)
|
||||||
|
kf->User_Func5_f(kf);
|
||||||
|
|
||||||
|
// 避免滤波器过度收敛
|
||||||
|
// suppress filter excessive convergence
|
||||||
|
for (uint8_t i = 0; i < kf->xhatSize; i++)
|
||||||
|
{
|
||||||
|
if (kf->P_data[i * kf->xhatSize + i] < kf->StateMinVariance[i])
|
||||||
|
kf->P_data[i * kf->xhatSize + i] = kf->StateMinVariance[i];
|
||||||
|
}
|
||||||
|
|
||||||
|
memcpy(kf->FilteredValue, kf->xhat_data, sizeof_float * kf->xhatSize);
|
||||||
|
|
||||||
|
if (kf->User_Func6_f != NULL)
|
||||||
|
kf->User_Func6_f(kf);
|
||||||
|
|
||||||
|
return kf->FilteredValue;
|
||||||
|
}
|
||||||
|
|
||||||
|
static void H_K_R_Adjustment(KalmanFilter_t *kf)
|
||||||
|
{
|
||||||
|
kf->MeasurementValidNum = 0;
|
||||||
|
|
||||||
|
memcpy(kf->z_data, kf->MeasuredVector, sizeof_float * kf->zSize);
|
||||||
|
memset(kf->MeasuredVector, 0, sizeof_float * kf->zSize);
|
||||||
|
|
||||||
|
// 识别量测数据有效性并调整矩阵H R K
|
||||||
|
// recognize measurement validity and adjust matrices H R K
|
||||||
|
memset(kf->R_data, 0, sizeof_float * kf->zSize * kf->zSize);
|
||||||
|
memset(kf->H_data, 0, sizeof_float * kf->xhatSize * kf->zSize);
|
||||||
|
for (uint8_t i = 0; i < kf->zSize; i++)
|
||||||
|
{
|
||||||
|
if (kf->z_data[i] != 0)
|
||||||
|
{
|
||||||
|
// 重构向量z
|
||||||
|
// rebuild vector z
|
||||||
|
kf->z_data[kf->MeasurementValidNum] = kf->z_data[i];
|
||||||
|
kf->temp[kf->MeasurementValidNum] = i;
|
||||||
|
// 重构矩阵H
|
||||||
|
// rebuild matrix H
|
||||||
|
kf->H_data[kf->xhatSize * kf->MeasurementValidNum + kf->MeasurementMap[i] - 1] = kf->MeasurementDegree[i];
|
||||||
|
kf->MeasurementValidNum++;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
for (uint8_t i = 0; i < kf->MeasurementValidNum; i++)
|
||||||
|
{
|
||||||
|
// 重构矩阵R
|
||||||
|
// rebuild matrix R
|
||||||
|
kf->R_data[i * kf->MeasurementValidNum + i] = kf->MatR_DiagonalElements[kf->temp[i]];
|
||||||
|
}
|
||||||
|
|
||||||
|
// 调整矩阵维数
|
||||||
|
// adjust the dimensions of system matrices
|
||||||
|
kf->H.numRows = kf->MeasurementValidNum;
|
||||||
|
kf->H.numCols = kf->xhatSize;
|
||||||
|
kf->HT.numRows = kf->xhatSize;
|
||||||
|
kf->HT.numCols = kf->MeasurementValidNum;
|
||||||
|
kf->R.numRows = kf->MeasurementValidNum;
|
||||||
|
kf->R.numCols = kf->MeasurementValidNum;
|
||||||
|
kf->K.numRows = kf->xhatSize;
|
||||||
|
kf->K.numCols = kf->MeasurementValidNum;
|
||||||
|
kf->z.numRows = kf->MeasurementValidNum;
|
||||||
|
}
|
||||||
112
User/component/kalman_filter.h
Normal file
112
User/component/kalman_filter.h
Normal file
@ -0,0 +1,112 @@
|
|||||||
|
/**
|
||||||
|
******************************************************************************
|
||||||
|
* @file kalman filter.h
|
||||||
|
* @author Wang Hongxi
|
||||||
|
* @version V1.2.2
|
||||||
|
* @date 2022/1/8
|
||||||
|
* @brief
|
||||||
|
******************************************************************************
|
||||||
|
* @attention
|
||||||
|
*
|
||||||
|
******************************************************************************
|
||||||
|
*/
|
||||||
|
#ifndef __KALMAN_FILTER_H
|
||||||
|
#define __KALMAN_FILTER_H
|
||||||
|
|
||||||
|
|
||||||
|
#include "arm_math.h"
|
||||||
|
|
||||||
|
#include "math.h"
|
||||||
|
#include "stdint.h"
|
||||||
|
#include "stdlib.h"
|
||||||
|
|
||||||
|
#ifndef user_malloc
|
||||||
|
#ifdef _CMSIS_OS_H
|
||||||
|
#define user_malloc pvPortMalloc
|
||||||
|
#else
|
||||||
|
#define user_malloc malloc
|
||||||
|
#endif
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#define mat arm_matrix_instance_f32
|
||||||
|
#define Matrix_Init arm_mat_init_f32
|
||||||
|
#define Matrix_Add arm_mat_add_f32
|
||||||
|
#define Matrix_Subtract arm_mat_sub_f32
|
||||||
|
#define Matrix_Multiply arm_mat_mult_f32
|
||||||
|
#define Matrix_Transpose arm_mat_trans_f32
|
||||||
|
#define Matrix_Inverse arm_mat_inverse_f32
|
||||||
|
|
||||||
|
typedef struct kf_t
|
||||||
|
{
|
||||||
|
float *FilteredValue;
|
||||||
|
float *MeasuredVector;
|
||||||
|
float *ControlVector;
|
||||||
|
|
||||||
|
uint8_t xhatSize;
|
||||||
|
uint8_t uSize;
|
||||||
|
uint8_t zSize;
|
||||||
|
|
||||||
|
uint8_t UseAutoAdjustment;
|
||||||
|
uint8_t MeasurementValidNum;
|
||||||
|
|
||||||
|
uint8_t *MeasurementMap; // 量测与状态的关系 how measurement relates to the state
|
||||||
|
float *MeasurementDegree; // 测量值对应H矩阵元素值 elements of each measurement in H
|
||||||
|
float *MatR_DiagonalElements; // 量测方差 variance for each measurement
|
||||||
|
float *StateMinVariance; // 最小方差 避免方差过度收敛 suppress filter excessive convergence
|
||||||
|
uint8_t *temp;
|
||||||
|
|
||||||
|
// 配合用户定义函数使用,作为标志位用于判断是否要跳过标准KF中五个环节中的任意一个
|
||||||
|
uint8_t SkipEq1, SkipEq2, SkipEq3, SkipEq4, SkipEq5;
|
||||||
|
|
||||||
|
// definiion of struct mat: rows & cols & pointer to vars
|
||||||
|
mat xhat; // x(k|k)
|
||||||
|
mat xhatminus; // x(k|k-1)
|
||||||
|
mat u; // control vector u
|
||||||
|
mat z; // measurement vector z
|
||||||
|
mat P; // covariance matrix P(k|k)
|
||||||
|
mat Pminus; // covariance matrix P(k|k-1)
|
||||||
|
mat F, FT,temp_F; // state transition matrix F FT
|
||||||
|
mat B; // control matrix B
|
||||||
|
mat H, HT; // measurement matrix H
|
||||||
|
mat Q; // process noise covariance matrix Q
|
||||||
|
mat R; // measurement noise covariance matrix R
|
||||||
|
mat K; // kalman gain K
|
||||||
|
mat S, temp_matrix, temp_matrix1, temp_vector, temp_vector1;
|
||||||
|
|
||||||
|
int8_t MatStatus;
|
||||||
|
|
||||||
|
// 用户定义函数,可以替换或扩展基准KF的功能
|
||||||
|
void (*User_Func0_f)(struct kf_t *kf);
|
||||||
|
void (*User_Func1_f)(struct kf_t *kf);
|
||||||
|
void (*User_Func2_f)(struct kf_t *kf);
|
||||||
|
void (*User_Func3_f)(struct kf_t *kf);
|
||||||
|
void (*User_Func4_f)(struct kf_t *kf);
|
||||||
|
void (*User_Func5_f)(struct kf_t *kf);
|
||||||
|
void (*User_Func6_f)(struct kf_t *kf);
|
||||||
|
|
||||||
|
// 矩阵存储空间指针
|
||||||
|
float *xhat_data, *xhatminus_data;
|
||||||
|
float *u_data;
|
||||||
|
float *z_data;
|
||||||
|
float *P_data, *Pminus_data;
|
||||||
|
float *F_data, *FT_data,*temp_F_data;
|
||||||
|
float *B_data;
|
||||||
|
float *H_data, *HT_data;
|
||||||
|
float *Q_data;
|
||||||
|
float *R_data;
|
||||||
|
float *K_data;
|
||||||
|
float *S_data, *temp_matrix_data, *temp_matrix_data1, *temp_vector_data, *temp_vector_data1;
|
||||||
|
} KalmanFilter_t;
|
||||||
|
|
||||||
|
extern uint16_t sizeof_float, sizeof_double;
|
||||||
|
|
||||||
|
void Kalman_Filter_Init(KalmanFilter_t *kf, uint8_t xhatSize, uint8_t uSize, uint8_t zSize);
|
||||||
|
void Kalman_Filter_Measure(KalmanFilter_t *kf);
|
||||||
|
void Kalman_Filter_xhatMinusUpdate(KalmanFilter_t *kf);
|
||||||
|
void Kalman_Filter_PminusUpdate(KalmanFilter_t *kf);
|
||||||
|
void Kalman_Filter_SetK(KalmanFilter_t *kf);
|
||||||
|
void Kalman_Filter_xhatUpdate(KalmanFilter_t *kf);
|
||||||
|
void Kalman_Filter_P_Update(KalmanFilter_t *kf);
|
||||||
|
float *Kalman_Filter_Update(KalmanFilter_t *kf);
|
||||||
|
|
||||||
|
#endif //__KALMAN_FILTER_H
|
||||||
@ -134,7 +134,7 @@ int8_t Shoot_Init(Shoot_t *s, Shoot_Params_t *param, float target_freq)
|
|||||||
|
|
||||||
memset(&s->shoot_Anglecalu,0,sizeof(s->shoot_Anglecalu));
|
memset(&s->shoot_Anglecalu,0,sizeof(s->shoot_Anglecalu));
|
||||||
memset(&s->output,0,sizeof(s->output));
|
memset(&s->output,0,sizeof(s->output));
|
||||||
s->target_variable.target_rpm=6000.0f; /* 归一化目标转速 */
|
s->target_variable.target_rpm=2000.0f; /* 归一化目标转速 */
|
||||||
|
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|||||||
@ -132,13 +132,13 @@ switch (dr16.data.sw_l) {
|
|||||||
cmd_for_chassis.mode = CHASSIS_MODE_RELAX;
|
cmd_for_chassis.mode = CHASSIS_MODE_RELAX;
|
||||||
break;
|
break;
|
||||||
case DR16_SW_MID:
|
case DR16_SW_MID:
|
||||||
// cmd_for_chassis.mode = CHASSIS_MODE_RECOVER;
|
cmd_for_chassis.mode = CHASSIS_MODE_RECOVER;
|
||||||
cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
|
// cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
|
||||||
break;
|
break;
|
||||||
case DR16_SW_DOWN:
|
case DR16_SW_DOWN:
|
||||||
// cmd_for_chassis.mode = CHASSIS_MODE_RECOVER;
|
// cmd_for_chassis.mode = CHASSIS_MODE_RECOVER;
|
||||||
cmd_for_chassis.mode = CHASSIS_MODE_BALANCE_ROTOR;
|
// cmd_for_chassis.mode = CHASSIS_MODE_BALANCE_ROTOR;
|
||||||
// cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
|
cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
|
||||||
// cmd_for_chassis.mode = CHASSIS_MODE_CALIBRATE;
|
// cmd_for_chassis.mode = CHASSIS_MODE_CALIBRATE;
|
||||||
break;
|
break;
|
||||||
default:
|
default:
|
||||||
|
|||||||
@ -19,6 +19,7 @@ set(MX_Include_Dirs
|
|||||||
${CMAKE_CURRENT_SOURCE_DIR}/../../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F
|
${CMAKE_CURRENT_SOURCE_DIR}/../../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F
|
||||||
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/CMSIS/Device/ST/STM32H7xx/Include
|
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/CMSIS/Device/ST/STM32H7xx/Include
|
||||||
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/CMSIS/Include
|
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/CMSIS/Include
|
||||||
|
${CMAKE_CURRENT_SOURCE_DIR}/../../Middlewares/ST/ARM/DSP/Inc
|
||||||
)
|
)
|
||||||
|
|
||||||
# STM32CubeMX generated application sources
|
# STM32CubeMX generated application sources
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user