update.
parent
d20b742a36
commit
f77aeb5ab8
|
|
@ -0,0 +1,378 @@
|
||||||
|
##
|
||||||
|
## PROJECT: Mouri Internal Library Essentials
|
||||||
|
## FILE: .gitignore
|
||||||
|
## PURPOSE: The root .gitignore file for Mile.Project Project
|
||||||
|
##
|
||||||
|
## LICENSE: The MIT License
|
||||||
|
##
|
||||||
|
## DEVELOPER: Mouri_Naruto (Mouri_Naruto AT Outlook.com)
|
||||||
|
##
|
||||||
|
|
||||||
|
##
|
||||||
|
## Ignore Mile.Project specific temporary files, build results,
|
||||||
|
## and files generated by popular Visual Studio add-ons.
|
||||||
|
##
|
||||||
|
Output/
|
||||||
|
|
||||||
|
##
|
||||||
|
## Ignore Visual Studio temporary files, build results, and
|
||||||
|
## files generated by popular Visual Studio add-ons.
|
||||||
|
##
|
||||||
|
|
||||||
|
# User-specific files
|
||||||
|
*.rsuser
|
||||||
|
*.suo
|
||||||
|
*.user
|
||||||
|
*.userosscache
|
||||||
|
*.sln.docstates
|
||||||
|
|
||||||
|
# User-specific files (MonoDevelop/Xamarin Studio)
|
||||||
|
*.userprefs
|
||||||
|
|
||||||
|
# Mono auto generated files
|
||||||
|
mono_crash.*
|
||||||
|
|
||||||
|
# Build results
|
||||||
|
[Dd]ebug/
|
||||||
|
[Dd]ebugPublic/
|
||||||
|
[Rr]elease/
|
||||||
|
[Rr]eleases/
|
||||||
|
x64/
|
||||||
|
x86/
|
||||||
|
[Ww][Ii][Nn]32/
|
||||||
|
[Aa][Rr][Mm]/
|
||||||
|
[Aa][Rr][Mm]64/
|
||||||
|
bld/
|
||||||
|
[Bb]in/
|
||||||
|
[Oo]bj/
|
||||||
|
[Ll]og/
|
||||||
|
[Ll]ogs/
|
||||||
|
|
||||||
|
# Visual Studio 2015/2017 cache/options directory
|
||||||
|
.vs/
|
||||||
|
# Uncomment if you have tasks that create the project's static files in wwwroot
|
||||||
|
#wwwroot/
|
||||||
|
|
||||||
|
# Visual Studio 2017 auto generated files
|
||||||
|
Generated\ Files/
|
||||||
|
|
||||||
|
# MSTest test Results
|
||||||
|
[Tt]est[Rr]esult*/
|
||||||
|
[Bb]uild[Ll]og.*
|
||||||
|
|
||||||
|
# NUnit
|
||||||
|
*.VisualState.xml
|
||||||
|
TestResult.xml
|
||||||
|
nunit-*.xml
|
||||||
|
|
||||||
|
# Build Results of an ATL Project
|
||||||
|
[Dd]ebugPS/
|
||||||
|
[Rr]eleasePS/
|
||||||
|
dlldata.c
|
||||||
|
|
||||||
|
# Benchmark Results
|
||||||
|
BenchmarkDotNet.Artifacts/
|
||||||
|
|
||||||
|
# .NET Core
|
||||||
|
project.lock.json
|
||||||
|
project.fragment.lock.json
|
||||||
|
artifacts/
|
||||||
|
|
||||||
|
# ASP.NET Scaffolding
|
||||||
|
ScaffoldingReadMe.txt
|
||||||
|
|
||||||
|
# StyleCop
|
||||||
|
StyleCopReport.xml
|
||||||
|
|
||||||
|
# Files built by Visual Studio
|
||||||
|
*_i.c
|
||||||
|
*_p.c
|
||||||
|
*_h.h
|
||||||
|
*.ilk
|
||||||
|
*.meta
|
||||||
|
*.obj
|
||||||
|
*.iobj
|
||||||
|
*.pch
|
||||||
|
*.pdb
|
||||||
|
*.ipdb
|
||||||
|
*.pgc
|
||||||
|
*.pgd
|
||||||
|
*.rsp
|
||||||
|
*.sbr
|
||||||
|
*.tlb
|
||||||
|
*.tli
|
||||||
|
*.tlh
|
||||||
|
*.tmp
|
||||||
|
*.tmp_proj
|
||||||
|
*_wpftmp.csproj
|
||||||
|
*.log
|
||||||
|
*.vspscc
|
||||||
|
*.vssscc
|
||||||
|
.builds
|
||||||
|
*.pidb
|
||||||
|
*.svclog
|
||||||
|
*.scc
|
||||||
|
|
||||||
|
# Chutzpah Test files
|
||||||
|
_Chutzpah*
|
||||||
|
|
||||||
|
# Visual C++ cache files
|
||||||
|
ipch/
|
||||||
|
*.aps
|
||||||
|
*.ncb
|
||||||
|
*.opendb
|
||||||
|
*.opensdf
|
||||||
|
*.sdf
|
||||||
|
*.cachefile
|
||||||
|
*.VC.db
|
||||||
|
*.VC.VC.opendb
|
||||||
|
|
||||||
|
# Visual Studio profiler
|
||||||
|
*.psess
|
||||||
|
*.vsp
|
||||||
|
*.vspx
|
||||||
|
*.sap
|
||||||
|
|
||||||
|
# Visual Studio Trace Files
|
||||||
|
*.e2e
|
||||||
|
|
||||||
|
# TFS 2012 Local Workspace
|
||||||
|
$tf/
|
||||||
|
|
||||||
|
# Guidance Automation Toolkit
|
||||||
|
*.gpState
|
||||||
|
|
||||||
|
# ReSharper is a .NET coding add-in
|
||||||
|
_ReSharper*/
|
||||||
|
*.[Rr]e[Ss]harper
|
||||||
|
*.DotSettings.user
|
||||||
|
|
||||||
|
# TeamCity is a build add-in
|
||||||
|
_TeamCity*
|
||||||
|
|
||||||
|
# DotCover is a Code Coverage Tool
|
||||||
|
*.dotCover
|
||||||
|
|
||||||
|
# AxoCover is a Code Coverage Tool
|
||||||
|
.axoCover/*
|
||||||
|
!.axoCover/settings.json
|
||||||
|
|
||||||
|
# Coverlet is a free, cross platform Code Coverage Tool
|
||||||
|
coverage*.json
|
||||||
|
coverage*.xml
|
||||||
|
coverage*.info
|
||||||
|
|
||||||
|
# Visual Studio code coverage results
|
||||||
|
*.coverage
|
||||||
|
*.coveragexml
|
||||||
|
|
||||||
|
# NCrunch
|
||||||
|
_NCrunch_*
|
||||||
|
.*crunch*.local.xml
|
||||||
|
nCrunchTemp_*
|
||||||
|
|
||||||
|
# MightyMoose
|
||||||
|
*.mm.*
|
||||||
|
AutoTest.Net/
|
||||||
|
|
||||||
|
# Web workbench (sass)
|
||||||
|
.sass-cache/
|
||||||
|
|
||||||
|
# Installshield output folder
|
||||||
|
[Ee]xpress/
|
||||||
|
|
||||||
|
# DocProject is a documentation generator add-in
|
||||||
|
DocProject/buildhelp/
|
||||||
|
DocProject/Help/*.HxT
|
||||||
|
DocProject/Help/*.HxC
|
||||||
|
DocProject/Help/*.hhc
|
||||||
|
DocProject/Help/*.hhk
|
||||||
|
DocProject/Help/*.hhp
|
||||||
|
DocProject/Help/Html2
|
||||||
|
DocProject/Help/html
|
||||||
|
|
||||||
|
# Click-Once directory
|
||||||
|
publish/
|
||||||
|
|
||||||
|
# Publish Web Output
|
||||||
|
*.[Pp]ublish.xml
|
||||||
|
*.azurePubxml
|
||||||
|
# Note: Comment the next line if you want to checkin your web deploy settings,
|
||||||
|
# but database connection strings (with potential passwords) will be unencrypted
|
||||||
|
*.pubxml
|
||||||
|
*.publishproj
|
||||||
|
|
||||||
|
# Microsoft Azure Web App publish settings. Comment the next line if you want to
|
||||||
|
# checkin your Azure Web App publish settings, but sensitive information contained
|
||||||
|
# in these scripts will be unencrypted
|
||||||
|
PublishScripts/
|
||||||
|
|
||||||
|
# NuGet Packages
|
||||||
|
*.nupkg
|
||||||
|
# NuGet Symbol Packages
|
||||||
|
*.snupkg
|
||||||
|
# The packages folder can be ignored because of Package Restore
|
||||||
|
**/[Pp]ackages/*
|
||||||
|
# except build/, which is used as an MSBuild target.
|
||||||
|
!**/[Pp]ackages/build/
|
||||||
|
# Uncomment if necessary however generally it will be regenerated when needed
|
||||||
|
#!**/[Pp]ackages/repositories.config
|
||||||
|
# NuGet v3's project.json files produces more ignorable files
|
||||||
|
*.nuget.props
|
||||||
|
*.nuget.targets
|
||||||
|
|
||||||
|
# Microsoft Azure Build Output
|
||||||
|
csx/
|
||||||
|
*.build.csdef
|
||||||
|
|
||||||
|
# Microsoft Azure Emulator
|
||||||
|
ecf/
|
||||||
|
rcf/
|
||||||
|
|
||||||
|
# Windows Store app package directories and files
|
||||||
|
AppPackages/
|
||||||
|
BundleArtifacts/
|
||||||
|
Package.StoreAssociation.xml
|
||||||
|
_pkginfo.txt
|
||||||
|
*.appx
|
||||||
|
*.appxbundle
|
||||||
|
*.appxupload
|
||||||
|
|
||||||
|
# Visual Studio cache files
|
||||||
|
# files ending in .cache can be ignored
|
||||||
|
*.[Cc]ache
|
||||||
|
# but keep track of directories ending in .cache
|
||||||
|
!?*.[Cc]ache/
|
||||||
|
|
||||||
|
# Others
|
||||||
|
ClientBin/
|
||||||
|
~$*
|
||||||
|
*~
|
||||||
|
*.dbmdl
|
||||||
|
*.dbproj.schemaview
|
||||||
|
*.jfm
|
||||||
|
*.pfx
|
||||||
|
*.publishsettings
|
||||||
|
orleans.codegen.cs
|
||||||
|
|
||||||
|
# Including strong name files can present a security risk
|
||||||
|
# (https://github.com/github/gitignore/pull/2483#issue-259490424)
|
||||||
|
#*.snk
|
||||||
|
|
||||||
|
# Since there are multiple workflows, uncomment next line to ignore bower_components
|
||||||
|
# (https://github.com/github/gitignore/pull/1529#issuecomment-104372622)
|
||||||
|
#bower_components/
|
||||||
|
|
||||||
|
# RIA/Silverlight projects
|
||||||
|
Generated_Code/
|
||||||
|
|
||||||
|
# Backup & report files from converting an old project file
|
||||||
|
# to a newer Visual Studio version. Backup files are not needed,
|
||||||
|
# because we have git ;-)
|
||||||
|
_UpgradeReport_Files/
|
||||||
|
Backup*/
|
||||||
|
UpgradeLog*.XML
|
||||||
|
UpgradeLog*.htm
|
||||||
|
ServiceFabricBackup/
|
||||||
|
*.rptproj.bak
|
||||||
|
|
||||||
|
# SQL Server files
|
||||||
|
*.mdf
|
||||||
|
*.ldf
|
||||||
|
*.ndf
|
||||||
|
|
||||||
|
# Business Intelligence projects
|
||||||
|
*.rdl.data
|
||||||
|
*.bim.layout
|
||||||
|
*.bim_*.settings
|
||||||
|
*.rptproj.rsuser
|
||||||
|
*- [Bb]ackup.rdl
|
||||||
|
*- [Bb]ackup ([0-9]).rdl
|
||||||
|
*- [Bb]ackup ([0-9][0-9]).rdl
|
||||||
|
|
||||||
|
# Microsoft Fakes
|
||||||
|
FakesAssemblies/
|
||||||
|
|
||||||
|
# GhostDoc plugin setting file
|
||||||
|
*.GhostDoc.xml
|
||||||
|
|
||||||
|
# Node.js Tools for Visual Studio
|
||||||
|
.ntvs_analysis.dat
|
||||||
|
node_modules/
|
||||||
|
|
||||||
|
# Visual Studio 6 build log
|
||||||
|
*.plg
|
||||||
|
|
||||||
|
# Visual Studio 6 workspace options file
|
||||||
|
*.opt
|
||||||
|
|
||||||
|
# Visual Studio 6 auto-generated workspace file (contains which files were open etc.)
|
||||||
|
*.vbw
|
||||||
|
|
||||||
|
# Visual Studio LightSwitch build output
|
||||||
|
**/*.HTMLClient/GeneratedArtifacts
|
||||||
|
**/*.DesktopClient/GeneratedArtifacts
|
||||||
|
**/*.DesktopClient/ModelManifest.xml
|
||||||
|
**/*.Server/GeneratedArtifacts
|
||||||
|
**/*.Server/ModelManifest.xml
|
||||||
|
_Pvt_Extensions
|
||||||
|
|
||||||
|
# Paket dependency manager
|
||||||
|
.paket/paket.exe
|
||||||
|
paket-files/
|
||||||
|
|
||||||
|
# FAKE - F# Make
|
||||||
|
.fake/
|
||||||
|
|
||||||
|
# CodeRush personal settings
|
||||||
|
.cr/personal
|
||||||
|
|
||||||
|
# Python Tools for Visual Studio (PTVS)
|
||||||
|
__pycache__/
|
||||||
|
*.pyc
|
||||||
|
|
||||||
|
# Cake - Uncomment if you are using it
|
||||||
|
# tools/**
|
||||||
|
# !tools/packages.config
|
||||||
|
|
||||||
|
# Tabs Studio
|
||||||
|
*.tss
|
||||||
|
|
||||||
|
# Telerik's JustMock configuration file
|
||||||
|
*.jmconfig
|
||||||
|
|
||||||
|
# BizTalk build output
|
||||||
|
*.btp.cs
|
||||||
|
*.btm.cs
|
||||||
|
*.odx.cs
|
||||||
|
*.xsd.cs
|
||||||
|
|
||||||
|
# OpenCover UI analysis results
|
||||||
|
OpenCover/
|
||||||
|
|
||||||
|
# Azure Stream Analytics local run output
|
||||||
|
ASALocalRun/
|
||||||
|
|
||||||
|
# MSBuild Binary and Structured Log
|
||||||
|
*.binlog
|
||||||
|
|
||||||
|
# NVidia Nsight GPU debugger configuration file
|
||||||
|
*.nvuser
|
||||||
|
|
||||||
|
# MFractors (Xamarin productivity tool) working folder
|
||||||
|
.mfractor/
|
||||||
|
|
||||||
|
# Local History for Visual Studio
|
||||||
|
.localhistory/
|
||||||
|
|
||||||
|
# BeatPulse healthcheck temp database
|
||||||
|
healthchecksdb
|
||||||
|
|
||||||
|
# Backup folder for Package Reference Convert tool in Visual Studio 2017
|
||||||
|
MigrationBackup/
|
||||||
|
|
||||||
|
# Ionide (cross platform F# VS Code tools) working folder
|
||||||
|
.ionide/
|
||||||
|
|
||||||
|
# Fody - auto-generated XML schema
|
||||||
|
FodyWeavers.xsd
|
||||||
|
|
@ -0,0 +1,2 @@
|
||||||
|
rd /s /Q .\.vs
|
||||||
|
rd /s /Q .\Output
|
||||||
|
|
@ -0,0 +1,31 @@
|
||||||
|
|
||||||
|
Microsoft Visual Studio Solution File, Format Version 12.00
|
||||||
|
# Visual Studio Version 16
|
||||||
|
VisualStudioVersion = 16.0.32106.194
|
||||||
|
MinimumVisualStudioVersion = 10.0.40219.1
|
||||||
|
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "GnssIns-PDR", "Project\GnssIns-PDR.vcxproj", "{7613DB61-9DAE-4700-9EFB-4596CC6E527E}"
|
||||||
|
EndProject
|
||||||
|
Global
|
||||||
|
GlobalSection(SolutionConfigurationPlatforms) = preSolution
|
||||||
|
Debug|x64 = Debug|x64
|
||||||
|
Debug|x86 = Debug|x86
|
||||||
|
Release|x64 = Release|x64
|
||||||
|
Release|x86 = Release|x86
|
||||||
|
EndGlobalSection
|
||||||
|
GlobalSection(ProjectConfigurationPlatforms) = postSolution
|
||||||
|
{7613DB61-9DAE-4700-9EFB-4596CC6E527E}.Debug|x64.ActiveCfg = Debug|x64
|
||||||
|
{7613DB61-9DAE-4700-9EFB-4596CC6E527E}.Debug|x64.Build.0 = Debug|x64
|
||||||
|
{7613DB61-9DAE-4700-9EFB-4596CC6E527E}.Debug|x86.ActiveCfg = Debug|Win32
|
||||||
|
{7613DB61-9DAE-4700-9EFB-4596CC6E527E}.Debug|x86.Build.0 = Debug|Win32
|
||||||
|
{7613DB61-9DAE-4700-9EFB-4596CC6E527E}.Release|x64.ActiveCfg = Release|x64
|
||||||
|
{7613DB61-9DAE-4700-9EFB-4596CC6E527E}.Release|x64.Build.0 = Release|x64
|
||||||
|
{7613DB61-9DAE-4700-9EFB-4596CC6E527E}.Release|x86.ActiveCfg = Release|Win32
|
||||||
|
{7613DB61-9DAE-4700-9EFB-4596CC6E527E}.Release|x86.Build.0 = Release|Win32
|
||||||
|
EndGlobalSection
|
||||||
|
GlobalSection(SolutionProperties) = preSolution
|
||||||
|
HideSolutionNode = FALSE
|
||||||
|
EndGlobalSection
|
||||||
|
GlobalSection(ExtensibilityGlobals) = postSolution
|
||||||
|
SolutionGuid = {030E4BF1-F6DC-452C-97C2-79F995A59FD0}
|
||||||
|
EndGlobalSection
|
||||||
|
EndGlobal
|
||||||
|
|
@ -0,0 +1,176 @@
|
||||||
|
<?xml version="1.0" encoding="utf-8"?>
|
||||||
|
<Project DefaultTargets="Build" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
|
||||||
|
<ItemGroup Label="ProjectConfigurations">
|
||||||
|
<ProjectConfiguration Include="Debug|Win32">
|
||||||
|
<Configuration>Debug</Configuration>
|
||||||
|
<Platform>Win32</Platform>
|
||||||
|
</ProjectConfiguration>
|
||||||
|
<ProjectConfiguration Include="Release|Win32">
|
||||||
|
<Configuration>Release</Configuration>
|
||||||
|
<Platform>Win32</Platform>
|
||||||
|
</ProjectConfiguration>
|
||||||
|
<ProjectConfiguration Include="Debug|x64">
|
||||||
|
<Configuration>Debug</Configuration>
|
||||||
|
<Platform>x64</Platform>
|
||||||
|
</ProjectConfiguration>
|
||||||
|
<ProjectConfiguration Include="Release|x64">
|
||||||
|
<Configuration>Release</Configuration>
|
||||||
|
<Platform>x64</Platform>
|
||||||
|
</ProjectConfiguration>
|
||||||
|
</ItemGroup>
|
||||||
|
<PropertyGroup Label="Globals">
|
||||||
|
<VCProjectVersion>16.0</VCProjectVersion>
|
||||||
|
<Keyword>Win32Proj</Keyword>
|
||||||
|
<ProjectGuid>{7613db61-9dae-4700-9efb-4596cc6e527e}</ProjectGuid>
|
||||||
|
<RootNamespace>GnssInsPDR</RootNamespace>
|
||||||
|
<WindowsTargetPlatformVersion>10.0</WindowsTargetPlatformVersion>
|
||||||
|
</PropertyGroup>
|
||||||
|
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.Default.props" />
|
||||||
|
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'" Label="Configuration">
|
||||||
|
<ConfigurationType>Application</ConfigurationType>
|
||||||
|
<UseDebugLibraries>true</UseDebugLibraries>
|
||||||
|
<PlatformToolset>v142</PlatformToolset>
|
||||||
|
<CharacterSet>Unicode</CharacterSet>
|
||||||
|
</PropertyGroup>
|
||||||
|
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'" Label="Configuration">
|
||||||
|
<ConfigurationType>Application</ConfigurationType>
|
||||||
|
<UseDebugLibraries>false</UseDebugLibraries>
|
||||||
|
<PlatformToolset>v142</PlatformToolset>
|
||||||
|
<WholeProgramOptimization>true</WholeProgramOptimization>
|
||||||
|
<CharacterSet>Unicode</CharacterSet>
|
||||||
|
</PropertyGroup>
|
||||||
|
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'" Label="Configuration">
|
||||||
|
<ConfigurationType>Application</ConfigurationType>
|
||||||
|
<UseDebugLibraries>true</UseDebugLibraries>
|
||||||
|
<PlatformToolset>v142</PlatformToolset>
|
||||||
|
<CharacterSet>Unicode</CharacterSet>
|
||||||
|
</PropertyGroup>
|
||||||
|
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'" Label="Configuration">
|
||||||
|
<ConfigurationType>Application</ConfigurationType>
|
||||||
|
<UseDebugLibraries>false</UseDebugLibraries>
|
||||||
|
<PlatformToolset>v142</PlatformToolset>
|
||||||
|
<WholeProgramOptimization>true</WholeProgramOptimization>
|
||||||
|
<CharacterSet>Unicode</CharacterSet>
|
||||||
|
</PropertyGroup>
|
||||||
|
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.props" />
|
||||||
|
<ImportGroup Label="ExtensionSettings">
|
||||||
|
</ImportGroup>
|
||||||
|
<ImportGroup Label="Shared">
|
||||||
|
</ImportGroup>
|
||||||
|
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
|
||||||
|
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||||
|
</ImportGroup>
|
||||||
|
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
|
||||||
|
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||||
|
</ImportGroup>
|
||||||
|
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
|
||||||
|
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||||
|
</ImportGroup>
|
||||||
|
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
|
||||||
|
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||||
|
</ImportGroup>
|
||||||
|
<PropertyGroup Label="UserMacros" />
|
||||||
|
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
|
||||||
|
<LinkIncremental>true</LinkIncremental>
|
||||||
|
</PropertyGroup>
|
||||||
|
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
|
||||||
|
<LinkIncremental>false</LinkIncremental>
|
||||||
|
</PropertyGroup>
|
||||||
|
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
|
||||||
|
<LinkIncremental>true</LinkIncremental>
|
||||||
|
</PropertyGroup>
|
||||||
|
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
|
||||||
|
<LinkIncremental>false</LinkIncremental>
|
||||||
|
</PropertyGroup>
|
||||||
|
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
|
||||||
|
<ClCompile>
|
||||||
|
<WarningLevel>Level3</WarningLevel>
|
||||||
|
<SDLCheck>true</SDLCheck>
|
||||||
|
<PreprocessorDefinitions>WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||||
|
<ConformanceMode>true</ConformanceMode>
|
||||||
|
<AdditionalIncludeDirectories>..\Src\</AdditionalIncludeDirectories>
|
||||||
|
</ClCompile>
|
||||||
|
<Link>
|
||||||
|
<SubSystem>Console</SubSystem>
|
||||||
|
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||||
|
</Link>
|
||||||
|
</ItemDefinitionGroup>
|
||||||
|
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
|
||||||
|
<ClCompile>
|
||||||
|
<WarningLevel>Level3</WarningLevel>
|
||||||
|
<FunctionLevelLinking>true</FunctionLevelLinking>
|
||||||
|
<IntrinsicFunctions>true</IntrinsicFunctions>
|
||||||
|
<SDLCheck>true</SDLCheck>
|
||||||
|
<PreprocessorDefinitions>WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||||
|
<ConformanceMode>true</ConformanceMode>
|
||||||
|
<AdditionalIncludeDirectories>..\Src\</AdditionalIncludeDirectories>
|
||||||
|
</ClCompile>
|
||||||
|
<Link>
|
||||||
|
<SubSystem>Console</SubSystem>
|
||||||
|
<EnableCOMDATFolding>true</EnableCOMDATFolding>
|
||||||
|
<OptimizeReferences>true</OptimizeReferences>
|
||||||
|
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||||
|
</Link>
|
||||||
|
</ItemDefinitionGroup>
|
||||||
|
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
|
||||||
|
<ClCompile>
|
||||||
|
<WarningLevel>Level3</WarningLevel>
|
||||||
|
<SDLCheck>true</SDLCheck>
|
||||||
|
<PreprocessorDefinitions>_DEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||||
|
<ConformanceMode>true</ConformanceMode>
|
||||||
|
<AdditionalIncludeDirectories>..\Src\;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories>
|
||||||
|
</ClCompile>
|
||||||
|
<Link>
|
||||||
|
<SubSystem>Console</SubSystem>
|
||||||
|
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||||
|
</Link>
|
||||||
|
</ItemDefinitionGroup>
|
||||||
|
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
|
||||||
|
<ClCompile>
|
||||||
|
<WarningLevel>Level3</WarningLevel>
|
||||||
|
<FunctionLevelLinking>true</FunctionLevelLinking>
|
||||||
|
<IntrinsicFunctions>true</IntrinsicFunctions>
|
||||||
|
<SDLCheck>true</SDLCheck>
|
||||||
|
<PreprocessorDefinitions>NDEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||||
|
<ConformanceMode>true</ConformanceMode>
|
||||||
|
<AdditionalIncludeDirectories>..\Src\</AdditionalIncludeDirectories>
|
||||||
|
</ClCompile>
|
||||||
|
<Link>
|
||||||
|
<SubSystem>Console</SubSystem>
|
||||||
|
<EnableCOMDATFolding>true</EnableCOMDATFolding>
|
||||||
|
<OptimizeReferences>true</OptimizeReferences>
|
||||||
|
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||||
|
</Link>
|
||||||
|
</ItemDefinitionGroup>
|
||||||
|
<ItemGroup>
|
||||||
|
<ClCompile Include="..\Src\AHRS.cpp" />
|
||||||
|
<ClCompile Include="..\Src\CoordTrans.cpp" />
|
||||||
|
<ClCompile Include="..\Src\Detector.cpp" />
|
||||||
|
<ClCompile Include="..\Src\DirectionEstimator.cpp" />
|
||||||
|
<ClCompile Include="..\Src\Interface.cpp" />
|
||||||
|
<ClCompile Include="..\Src\Kalman.cpp" />
|
||||||
|
<ClCompile Include="..\Src\KmlSupport.cpp" />
|
||||||
|
<ClCompile Include="..\Src\Location.cpp" />
|
||||||
|
<ClCompile Include="..\Src\Matrix.cpp" />
|
||||||
|
<ClCompile Include="..\Src\Pedometer.cpp" />
|
||||||
|
<ClCompile Include="..\Src\Quaternion.cpp" />
|
||||||
|
<ClCompile Include="main.cpp" />
|
||||||
|
</ItemGroup>
|
||||||
|
<ItemGroup>
|
||||||
|
<ClInclude Include="..\Src\AHRS.h" />
|
||||||
|
<ClInclude Include="..\Src\CoordTrans.h" />
|
||||||
|
<ClInclude Include="..\Src\Detector.h" />
|
||||||
|
<ClInclude Include="..\Src\DirectionEstimator.h" />
|
||||||
|
<ClInclude Include="..\Src\Interface.h" />
|
||||||
|
<ClInclude Include="..\Src\Kalman.h" />
|
||||||
|
<ClInclude Include="..\Src\KmlSupport.h" />
|
||||||
|
<ClInclude Include="..\Src\Location.h" />
|
||||||
|
<ClInclude Include="..\Src\PDRBase.h" />
|
||||||
|
<ClInclude Include="..\Src\Matrix.h" />
|
||||||
|
<ClInclude Include="..\Src\Pedometer.h" />
|
||||||
|
<ClInclude Include="..\Src\Quaternion.h" />
|
||||||
|
</ItemGroup>
|
||||||
|
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
|
||||||
|
<ImportGroup Label="ExtensionTargets">
|
||||||
|
</ImportGroup>
|
||||||
|
</Project>
|
||||||
|
|
@ -0,0 +1,105 @@
|
||||||
|
<?xml version="1.0" encoding="utf-8"?>
|
||||||
|
<Project ToolsVersion="4.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
|
||||||
|
<ItemGroup>
|
||||||
|
<Filter Include="源文件">
|
||||||
|
<UniqueIdentifier>{4FC737F1-C7A5-4376-A066-2A32D752A2FF}</UniqueIdentifier>
|
||||||
|
<Extensions>cpp;c;cc;cxx;c++;cppm;ixx;def;odl;idl;hpj;bat;asm;asmx</Extensions>
|
||||||
|
</Filter>
|
||||||
|
<Filter Include="头文件">
|
||||||
|
<UniqueIdentifier>{93995380-89BD-4b04-88EB-625FBE52EBFB}</UniqueIdentifier>
|
||||||
|
<Extensions>h;hh;hpp;hxx;h++;hm;inl;inc;ipp;xsd</Extensions>
|
||||||
|
</Filter>
|
||||||
|
<Filter Include="资源文件">
|
||||||
|
<UniqueIdentifier>{67DA6AB6-F800-4c08-8B7A-83BB121AAD01}</UniqueIdentifier>
|
||||||
|
<Extensions>rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms</Extensions>
|
||||||
|
</Filter>
|
||||||
|
<Filter Include="源文件\Utils">
|
||||||
|
<UniqueIdentifier>{63104727-b67d-48da-a323-d8807e093166}</UniqueIdentifier>
|
||||||
|
</Filter>
|
||||||
|
<Filter Include="源文件\Location">
|
||||||
|
<UniqueIdentifier>{36420699-adfa-418b-b9e0-0cdf1e967fcc}</UniqueIdentifier>
|
||||||
|
</Filter>
|
||||||
|
<Filter Include="源文件\Ins">
|
||||||
|
<UniqueIdentifier>{f0d1d596-0324-4c0d-b101-9a73a7695bd7}</UniqueIdentifier>
|
||||||
|
</Filter>
|
||||||
|
<Filter Include="源文件\Math">
|
||||||
|
<UniqueIdentifier>{19c0ab01-53bc-44ae-95f4-5eec5b71dbc0}</UniqueIdentifier>
|
||||||
|
</Filter>
|
||||||
|
</ItemGroup>
|
||||||
|
<ItemGroup>
|
||||||
|
<ClCompile Include="main.cpp">
|
||||||
|
<Filter>源文件</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\Src\CoordTrans.cpp">
|
||||||
|
<Filter>源文件\Utils</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\Src\Location.cpp">
|
||||||
|
<Filter>源文件\Location</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\Src\Kalman.cpp">
|
||||||
|
<Filter>源文件\Location</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\Src\AHRS.cpp">
|
||||||
|
<Filter>源文件\Ins</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\Src\Quaternion.cpp">
|
||||||
|
<Filter>源文件\Math</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\Src\Matrix.cpp">
|
||||||
|
<Filter>源文件\Math</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\Src\KmlSupport.cpp">
|
||||||
|
<Filter>源文件\Utils</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\Src\Detector.cpp">
|
||||||
|
<Filter>源文件\Ins</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\Src\Interface.cpp">
|
||||||
|
<Filter>源文件</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\Src\Pedometer.cpp">
|
||||||
|
<Filter>源文件\Ins</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\Src\DirectionEstimator.cpp">
|
||||||
|
<Filter>源文件\Ins</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
</ItemGroup>
|
||||||
|
<ItemGroup>
|
||||||
|
<ClInclude Include="..\Src\CoordTrans.h">
|
||||||
|
<Filter>源文件\Utils</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\Src\Location.h">
|
||||||
|
<Filter>源文件\Location</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\Src\Kalman.h">
|
||||||
|
<Filter>源文件\Location</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\Src\PDRBase.h">
|
||||||
|
<Filter>头文件</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\Src\AHRS.h">
|
||||||
|
<Filter>源文件\Ins</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\Src\Quaternion.h">
|
||||||
|
<Filter>源文件\Math</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\Src\Matrix.h">
|
||||||
|
<Filter>源文件\Math</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\Src\KmlSupport.h">
|
||||||
|
<Filter>源文件\Utils</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\Src\Detector.h">
|
||||||
|
<Filter>源文件\Ins</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\Src\Interface.h">
|
||||||
|
<Filter>源文件</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\Src\Pedometer.h">
|
||||||
|
<Filter>源文件\Ins</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\Src\DirectionEstimator.h">
|
||||||
|
<Filter>源文件\Ins</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
</ItemGroup>
|
||||||
|
</Project>
|
||||||
|
|
@ -0,0 +1,23 @@
|
||||||
|
#include <iostream>
|
||||||
|
#include <stdlib.h>
|
||||||
|
#include <Matrix.h>
|
||||||
|
|
||||||
|
|
||||||
|
void TestMatrix() {
|
||||||
|
double a[4] = { 1,2,3,4 };
|
||||||
|
double b[4] = { 2,3,4,5 };
|
||||||
|
double r[4] = { 0,0,0,0 };
|
||||||
|
|
||||||
|
VectorAdd(a, b, r);
|
||||||
|
|
||||||
|
for (size_t i = 0; i < 4; i++) {
|
||||||
|
std::cout << "a[" << i << "] = " << r[i] << std::endl;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
int main()
|
||||||
|
{
|
||||||
|
TestMatrix();
|
||||||
|
system("pause");
|
||||||
|
}
|
||||||
|
|
||||||
|
|
@ -0,0 +1,123 @@
|
||||||
|
/******************** (C) COPYRIGHT 2022 Geek************************************
|
||||||
|
* File Name : AHRS.cpp
|
||||||
|
* Current Version : V1.0
|
||||||
|
* Author : logzhan
|
||||||
|
* Date of Issued : 2022.09.24
|
||||||
|
* Comments : PDR姿态解算
|
||||||
|
********************************************************************************/
|
||||||
|
/* Header File Including -----------------------------------------------------*/
|
||||||
|
#include <string.h>
|
||||||
|
#include <math.h>
|
||||||
|
#include "AHRS.h"
|
||||||
|
#include "Quaternion.h"
|
||||||
|
/* Macro Definition ----------------------------------------------------------*/
|
||||||
|
#define FLT_THRES 0.000001f // 浮点数最小阈值
|
||||||
|
#define ENABLE_MAG 0
|
||||||
|
/* Global Variable Definition ------------------------------------------------*/
|
||||||
|
AHRS_t Ahrs;
|
||||||
|
|
||||||
|
/**----------------------------------------------------------------------
|
||||||
|
* Function : AHRS_Init
|
||||||
|
* Description : AHRS初始化
|
||||||
|
* Date : 2022/09/21 logzhan
|
||||||
|
*---------------------------------------------------------------------**/
|
||||||
|
void AHRS_Init()
|
||||||
|
{
|
||||||
|
memset(&Ahrs, 0, sizeof(Ahrs));
|
||||||
|
Ahrs.Kp = AHRS_KP;
|
||||||
|
Ahrs.Dt = 1.0f / AHRS_SAMPLE_FREQ;
|
||||||
|
Ahrs.q[0] = 1.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**---------------------------------------------------------------------
|
||||||
|
* Function : Vec3Norm
|
||||||
|
* Description : 三维浮点数向量归一化
|
||||||
|
* Date : 2022/11/1 logzhan
|
||||||
|
*---------------------------------------------------------------------**/
|
||||||
|
void Vec3Norm(float* vx, float* vy, float* vz)
|
||||||
|
{
|
||||||
|
float norm = sqrtf(((*vx) * (*vx) + (*vy) * (*vy) +
|
||||||
|
(*vz) * (*vz)));
|
||||||
|
// 防止出现模为0的情况
|
||||||
|
if (norm > FLT_THRES) {
|
||||||
|
norm = 1 / norm;
|
||||||
|
(*vx) *= norm; (*vy) *= norm; (*vz) *= norm;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/**----------------------------------------------------------------------
|
||||||
|
* Function : MahonyUpdateAHRS
|
||||||
|
* Description : Mahony姿态更新算法
|
||||||
|
* Date : 2022/09/21 logzhan
|
||||||
|
*---------------------------------------------------------------------**/
|
||||||
|
void MahonyUpdateAHRS(float gx, float gy, float gz, float ax, float ay, float az,
|
||||||
|
float mx, float my, float mz)
|
||||||
|
{
|
||||||
|
float* q = Ahrs.q;
|
||||||
|
// 归一化磁力计和加速度计
|
||||||
|
Vec3Norm(&ax, &ay, &az);
|
||||||
|
|
||||||
|
float wx = 0.0f, wy = 0.0f, wz = 0.0f;
|
||||||
|
|
||||||
|
#if ENABLE_MAG
|
||||||
|
if (!((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f))) {
|
||||||
|
|
||||||
|
pdr_v3Norm(&mx, &my, &mz);
|
||||||
|
|
||||||
|
// 把磁场从载体系转换到地理坐标系 h = R^-1 * (mx,my,mz)
|
||||||
|
float hx = 2.0f * (mx * (0.5f - q2 * q2 - q[3] * q[3]) + my * (q[1] * q2 - q[0] * q[3]) + mz * (q[1] * q[3] + q[0] * q2));
|
||||||
|
float hy = 2.0f * (mx * (q[1] * q2 + q[0] * q[3]) + my * (0.5f - q[1] * q[1] - q[3] * q[3]) + mz * (q2 * q[3] - q[0] * q[1]));
|
||||||
|
float hz = 2.0f * (mx * (q[1] * q[3] - q[0] * q2) + my * (q2 * q[3] + q[0] * q[1]) + mz * (0.5f - q[1] * q[1] - q2 * q2));
|
||||||
|
|
||||||
|
// 理论上bx = 0, by 指向北向，因为手机IMU数据使用的是东北天(x,y,z)坐标系
|
||||||
|
float by = (float)sqrt(hx * hx + hy * hy);
|
||||||
|
float bz = hz;
|
||||||
|
|
||||||
|
wx = by * (q[0] * q[3] + q[1] * q2) + bz * (q[1] * q[3] - q[0] * q2);
|
||||||
|
wy = by * (0.5f - q[1] * q[1] - q[3] * q[3]) + bz * (q[0] * q[1] + q2 * q[3]);
|
||||||
|
wz = by * (q2 * q[3] - q[0] * q[1]) + bz * (0.5f - q[1] * q[1] - q2 * q2);
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
// 把重力转换到地理坐标系 v = R * (0,0,1)
|
||||||
|
float vx = q[1] * q[3] - q[0] * q[2];
|
||||||
|
float vy = q[0] * q[1] + q[2] * q[3];
|
||||||
|
float vz = q[0] * q[0] - 0.5f + q[3] * q[3];
|
||||||
|
|
||||||
|
// 计算矢量叉乘误差
|
||||||
|
float ex = ay * vz - az * vy + my * wz - mz * wy;
|
||||||
|
float ey = az * vx - ax * vz + mz * wx - mx * wz;
|
||||||
|
float ez = ax * vy - ay * vx + mx * wy - my * wx;
|
||||||
|
|
||||||
|
// Apply proportional feedback
|
||||||
|
gx += Ahrs.Kp * ex;
|
||||||
|
gy += Ahrs.Kp * ey;
|
||||||
|
gz += Ahrs.Kp * ez;
|
||||||
|
|
||||||
|
// 龙格库塔法更新四元数
|
||||||
|
float qa = q[0]; float qb = q[1]; float qc = q[2];
|
||||||
|
q[0] += (-qb * gx - qc * gy - q[3] * gz) * 0.5f * (Ahrs.Dt);
|
||||||
|
q[1] += (qa * gx + qc * gz - q[3] * gy) * 0.5f * (Ahrs.Dt);
|
||||||
|
q[2] += (qa * gy - qb * gz + q[3] * gx) * 0.5f * (Ahrs.Dt);
|
||||||
|
q[3] += (qa * gz + qb * gy - qc * gx) * 0.5f * (Ahrs.Dt);
|
||||||
|
|
||||||
|
// 四元数归一化
|
||||||
|
QuaternionNorm(&q[0], &q[1], &q[2], &q[3]);
|
||||||
|
|
||||||
|
// 四元数转欧拉角
|
||||||
|
Ahrs.Pitch = (float)asin(-2.0f * (q[3] * q[1] - q[0] * q[2])) * (180.0f / 3.141592f);
|
||||||
|
Ahrs.Yaw = (float)atan2(q[2] * q[1] + q[0] * q[3], 0.5f - q[2] * q[2] - q[3] * q[3]) * (180.0f / 3.141592f);
|
||||||
|
Ahrs.Roll = (float)atan2(q[2] * q[3] + q[0] * q[1], 0.5f - q[2] * q[2] - q[1] * q[1]) * (180.0f / 3.141592f);
|
||||||
|
}
|
||||||
|
|
||||||
|
/**----------------------------------------------------------------------
|
||||||
|
* Function : UpdateAHRS
|
||||||
|
* Description : AHRS融合解算
|
||||||
|
* Date : 2022/09/23 logzhan
|
||||||
|
*---------------------------------------------------------------------**/
|
||||||
|
int UpdateAHRS(IMU_t* imu)
|
||||||
|
{
|
||||||
|
MahonyUpdateAHRS(imu->gyr.s[0], imu->gyr.s[1], imu->gyr.s[2],
|
||||||
|
imu->acc.s[0], imu->acc.s[1], imu->acc.s[2],
|
||||||
|
imu->mag.s[0], imu->mag.s[1], imu->mag.s[2]);
|
||||||
|
return PDR_TRUE;
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,31 @@
|
||||||
|
#ifndef _PDR_AHRS_H_
|
||||||
|
#define _PDR_AHRS_H_
|
||||||
|
|
||||||
|
#include "PDRBase.h"
|
||||||
|
|
||||||
|
#define AHRS_SAMPLE_FREQ 100 // ARHS算法的传感器采样频率
|
||||||
|
#define AHRS_KP 0.500
|
||||||
|
|
||||||
|
/**----------------------------------------------------------------------
|
||||||
|
* Function : AHRS_Init
|
||||||
|
* Description : AHRS初始化
|
||||||
|
* Date : 2022/09/21 logzhan
|
||||||
|
*---------------------------------------------------------------------**/
|
||||||
|
void AHRS_Init();
|
||||||
|
|
||||||
|
/**----------------------------------------------------------------------
|
||||||
|
* Function : MahonyUpdateAHRS
|
||||||
|
* Description : Mahony姿态更新算法
|
||||||
|
* Date : 2022/09/21 logzhan
|
||||||
|
*---------------------------------------------------------------------**/
|
||||||
|
void MahonyUpdateAHRS(float gx, float gy, float gz, float ax, float ay, float az,
|
||||||
|
float mx, float my, float mz);
|
||||||
|
|
||||||
|
/**----------------------------------------------------------------------
|
||||||
|
* Function : UpdateAHRS
|
||||||
|
* Description : AHRS融合解算
|
||||||
|
* Date : 2022/09/23 logzhan
|
||||||
|
*---------------------------------------------------------------------**/
|
||||||
|
int UpdateAHRS(IMU_t* IMU);
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,74 @@
|
||||||
|
/******************** (C) COPYRIGHT 2022 Geek************************************
|
||||||
|
* File Name : CoordTrans.cpp
|
||||||
|
* Current Version : V1.0
|
||||||
|
* Author : logzhan
|
||||||
|
* Date of Issued : 2022.09.20
|
||||||
|
* Comments : PDR 坐标系转换相关函数支持
|
||||||
|
********************************************************************************/
|
||||||
|
#include <math.h>
|
||||||
|
#include "CoordTrans.h"
|
||||||
|
|
||||||
|
void WGS842ECEF(double* plla, double* ecef) {
|
||||||
|
double f = 1 / 298.257223563;
|
||||||
|
double a = 6378136.49;
|
||||||
|
double b, e, roc;
|
||||||
|
|
||||||
|
b = a * (1 - f);
|
||||||
|
e = sqrt(pow(a, 2) - pow(b, 2)) / a;
|
||||||
|
roc = a / sqrt(1 - pow(e * sin(plla[0]), 2));
|
||||||
|
ecef[0] = (roc + plla[2]) * cos(plla[0]) * cos(plla[1]);
|
||||||
|
ecef[1] = (roc + plla[2]) * cos(plla[0]) * sin(plla[1]);
|
||||||
|
ecef[2] = (roc * (1 - e * e) + plla[2]) * sin(plla[0]);
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
void ECEF2WGS84(double* ecef, double* plla)
|
||||||
|
{
|
||||||
|
double a = 6378137;
|
||||||
|
double b = 6356752.314245179;
|
||||||
|
double e, ep, p, th, roc;
|
||||||
|
|
||||||
|
e = sqrt(pow(a, 2) - pow(b, 2)) / a;
|
||||||
|
b = sqrt(pow(a, 2) * (1 - pow(e, 2)));
|
||||||
|
ep = sqrt((pow(a, 2) - pow(b, 2)) / pow(b, 2));
|
||||||
|
p = sqrt(ecef[0] * ecef[0] + ecef[1] * ecef[1]);
|
||||||
|
th = atan2(a * ecef[2], b * p);
|
||||||
|
|
||||||
|
plla[1] = atan2(ecef[1], ecef[0]);
|
||||||
|
plla[0] = atan2((ecef[2] + pow(ep, 2) * b * pow(sin(th), 3)), (p - pow(e, 2) * a * pow(cos(th), 3)));
|
||||||
|
roc = a / sqrt(1 - pow(e * sin(plla[0]), 2));
|
||||||
|
plla[2] = p / cos(plla[0]) - roc;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
void ECEF2NED(double* ecef, double* plla, double* ned)
|
||||||
|
{
|
||||||
|
ned[0] = -sin(plla[0]) * cos(plla[1]) * ecef[0] - sin(plla[0]) * sin(plla[1]) * ecef[1] + cos(plla[0]) * ecef[2];
|
||||||
|
ned[1] = -sin(plla[1]) * ecef[0] + cos(plla[1]) * ecef[1];
|
||||||
|
ned[2] = -cos(plla[0]) * cos(plla[1]) * ecef[0] - cos(plla[0]) * sin(plla[1]) * ecef[1] - sin(plla[0]) * ecef[2];
|
||||||
|
}
|
||||||
|
|
||||||
|
void NED2ECEF(double* plla, double* ned, double* ecef0, double* ecef)
|
||||||
|
{
|
||||||
|
ecef[0] = -sin(plla[0]) * cos(plla[1]) * ned[0] - sin(plla[1]) * ned[1] - cos(plla[0]) * cos(plla[1]) * ned[2] + ecef0[0];
|
||||||
|
ecef[1] = -sin(plla[0]) * sin(plla[1]) * ned[0] + cos(plla[1]) * ned[1] - cos(plla[0]) * sin(plla[1]) * ned[2] + ecef0[1];
|
||||||
|
ecef[2] = cos(plla[0]) * ned[0] - sin(plla[0]) * ned[2] + ecef0[2];;
|
||||||
|
}
|
||||||
|
|
||||||
|
void WGS842NED(double* plla, double* ref_lla, double* ned)
|
||||||
|
{
|
||||||
|
double ecef[3] = { 0 };
|
||||||
|
|
||||||
|
WGS842ECEF(plla, ecef);
|
||||||
|
ECEF2NED(ecef, ref_lla, ned);
|
||||||
|
}
|
||||||
|
|
||||||
|
void NED2WGS84(double* ref_plla, double* ned, double* plla)
|
||||||
|
{
|
||||||
|
double ecef[3] = { 0 };
|
||||||
|
double ref_ecef[3] = { 0 };
|
||||||
|
WGS842ECEF(ref_plla, ref_ecef);
|
||||||
|
|
||||||
|
NED2ECEF(ref_plla, ned, ref_ecef, ecef);
|
||||||
|
ECEF2WGS84(ecef, plla);
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,22 @@
|
||||||
|
#ifndef _PDR_COORD_SUPPORT_H
|
||||||
|
#define _PDR_COORD_SUPPORT_H
|
||||||
|
|
||||||
|
#include <string>
|
||||||
|
using namespace std;
|
||||||
|
|
||||||
|
namespace PDR {
|
||||||
|
|
||||||
|
void WGS842ECEF(double* plla, double* ecef);
|
||||||
|
|
||||||
|
void ECEF2WGS84(double* ecef, double* plla);
|
||||||
|
|
||||||
|
void ECEF2NED(double* ecef, double* plla, double* ned);
|
||||||
|
|
||||||
|
void NED2ECEF(double* plla, double* ned, double* ecef0, double* ecef);
|
||||||
|
|
||||||
|
void WGS842NED(double* plla, double* ref_lla, double* ned);
|
||||||
|
|
||||||
|
void NED2WGS84(double* ref_plla, double* ned, double* plla);
|
||||||
|
|
||||||
|
}
|
||||||
|
#endif // _PDR_KML_SUPPORT_H
|
||||||
|
|
@ -0,0 +1,49 @@
|
||||||
|
/******************** (C) COPYRIGHT 2022 Geek************************************
|
||||||
|
* File Name : Detector.cpp
|
||||||
|
* Current Version : V1.0
|
||||||
|
* Author : logzhan
|
||||||
|
* Date of Issued : 2022.09.24
|
||||||
|
* Comments : PDR运动检测器
|
||||||
|
********************************************************************************/
|
||||||
|
/* Header File Including -----------------------------------------------------*/
|
||||||
|
#include <iostream>
|
||||||
|
#include "Detector.h"
|
||||||
|
|
||||||
|
/**---------------------------------------------------------------------
|
||||||
|
* Function : pdr_initDetector
|
||||||
|
* Description : 初始化运动分类器，识别用户处于哪一种运动模式
|
||||||
|
* Date : 2020/02/16 logzhan & logzhan
|
||||||
|
*---------------------------------------------------------------------**/
|
||||||
|
void Detector_Init() {
|
||||||
|
std::cout << "Detector_Init" << std::endl;
|
||||||
|
DetectorReset();
|
||||||
|
}
|
||||||
|
|
||||||
|
/**---------------------------------------------------------------------
|
||||||
|
* Function : DetectorReset
|
||||||
|
* Description : 重置PDR运动检测器
|
||||||
|
* Date : 2022/09/23 logzhan
|
||||||
|
*---------------------------------------------------------------------**/
|
||||||
|
void DetectorReset(void) {
|
||||||
|
std::cout << "DetectorReset" << std::endl;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**---------------------------------------------------------------------
|
||||||
|
* Function : DetectorUpdateIMU
|
||||||
|
* Description : 更新运动类型检测器的imu信息，如果到达一定的时间间隔，则会检测
|
||||||
|
* 用户的运动类型
|
||||||
|
* Date : 2022/09/23
|
||||||
|
*---------------------------------------------------------------------**/
|
||||||
|
void DetectorUpdateIMU(IMU_t* imu) {
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
/**---------------------------------------------------------------------
|
||||||
|
* Function : DetectMotionType
|
||||||
|
* Description : pdr运动类型检测
|
||||||
|
* Date : 2020/7/20
|
||||||
|
*---------------------------------------------------------------------**/
|
||||||
|
int DetectMotionType() {
|
||||||
|
// 目前仅支持手持平放类型
|
||||||
|
return DETECTOR_TYPE_HANDHELD;
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,35 @@
|
||||||
|
#ifndef _PDR_DETECTOR_H_
|
||||||
|
#define _PDR_DETECTOR_H_
|
||||||
|
|
||||||
|
#include "PDRBase.h"
|
||||||
|
|
||||||
|
/**---------------------------------------------------------------------
|
||||||
|
* Function : pdr_initDetector
|
||||||
|
* Description : 初始化运动分类器，识别用户处于哪一种运动模式
|
||||||
|
* Date : 2020/02/16 logzhan & logzhan
|
||||||
|
*---------------------------------------------------------------------**/
|
||||||
|
void Detector_Init(void);
|
||||||
|
|
||||||
|
/**---------------------------------------------------------------------
|
||||||
|
* Function : DetectorReset
|
||||||
|
* Description : 重置PDR运动检测器
|
||||||
|
* Date : 2022/09/23 logzhan
|
||||||
|
*---------------------------------------------------------------------**/
|
||||||
|
void DetectorReset(void);
|
||||||
|
|
||||||
|
/**---------------------------------------------------------------------
|
||||||
|
* Function : DetectorUpdateIMU
|
||||||
|
* Description : 更新运动类型检测器的imu信息，如果到达一定的时间间隔，则会检测
|
||||||
|
* 用户的运动类型
|
||||||
|
* Date : 2022/09/23
|
||||||
|
*---------------------------------------------------------------------**/
|
||||||
|
void DetectorUpdateIMU(IMU_t* imu);
|
||||||
|
|
||||||
|
/**---------------------------------------------------------------------
|
||||||
|
* Function : DetectMotionType
|
||||||
|
* Description : pdr运动类型检测
|
||||||
|
* Date : 2020/7/20
|
||||||
|
*---------------------------------------------------------------------**/
|
||||||
|
int DetectMotionType(void);
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,12 @@
|
||||||
|
#include <iostream>
|
||||||
|
#include "DirectionEstimator.h"
|
||||||
|
|
||||||
|
|
||||||
|
void DirectionEstimator_Init() {
|
||||||
|
std::cout << "DirectionEstimator_Init" << std::endl;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
double DirectionPredict(double* directioin) {
|
||||||
|
return 0.0;
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,8 @@
|
||||||
|
#ifndef _DIRECTION_ESTIMATOR_H_
|
||||||
|
#define _DIRECTION_ESTIMATOR_H_
|
||||||
|
|
||||||
|
void DirectionEstimator_Init(void);
|
||||||
|
|
||||||
|
double DirectionPredict(double* directioin);
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,78 @@
|
||||||
|
/******************** (C) COPYRIGHT 2022 Geek************************************
|
||||||
|
* File Name : Interface.c
|
||||||
|
* Current Version : V2.0
|
||||||
|
* Author : logzhan
|
||||||
|
* Date of Issued : 2022.10.15
|
||||||
|
* Comments : PDR导航算法对外部接口
|
||||||
|
********************************************************************************/
|
||||||
|
#include "Interface.h"
|
||||||
|
#include "Location.h"
|
||||||
|
|
||||||
|
/* Global Variable Definition ------------------------------------------------*/
|
||||||
|
const char* PDR_Version = "2.0";
|
||||||
|
Nmea_t Nmea;
|
||||||
|
IMU_t Imu;
|
||||||
|
/* Extern Variable Definition ------------------------------------------------*/
|
||||||
|
extern EKFPara_t EkfPara;
|
||||||
|
|
||||||
|
/**---------------------------------------------------------------------
|
||||||
|
* Function : ParseDataAndUpdate
|
||||||
|
* Description : 解析字符串信息并更新
|
||||||
|
* Date : 2022/10/16 logzhan
|
||||||
|
*---------------------------------------------------------------------**/
|
||||||
|
int ParseDataAndUpdate(char* line, LctFs_t* LocFusion)
|
||||||
|
{
|
||||||
|
// 解析Gnss和Imu数据用于PDR导航
|
||||||
|
//ParseGnssInsData(line, &Imu, &Nmea);
|
||||||
|
// 给定Imu和Nmea结构体，返回融合位置
|
||||||
|
return PDRLocationMainLoop(&Imu, &Nmea, LocFusion);
|
||||||
|
}
|
||||||
|
|
||||||
|
/**---------------------------------------------------------------------
|
||||||
|
* Function : PDRLocationMainLoop
|
||||||
|
* Description : PDR定位主循环
|
||||||
|
* Date : 2022/11/1 logzhan
|
||||||
|
*---------------------------------------------------------------------**/
|
||||||
|
int PDRLocationMainLoop(IMU_t* imu, Nmea_t* nmea, LctFs_t* LocFusion) {
|
||||||
|
int type = 0;
|
||||||
|
|
||||||
|
if (imu->gyr.update) {
|
||||||
|
// 如果陀螺仪更新则采用惯性传感器计算
|
||||||
|
InsLocationUpdate(imu, &EkfPara);
|
||||||
|
imu->gyr.update = NO_UPDATE;
|
||||||
|
}
|
||||||
|
// 如果GPS不更新，返回
|
||||||
|
if (!nmea->Update)return TYPE_FIX_NONE;
|
||||||
|
|
||||||
|
// 写GPS相关LOG信息
|
||||||
|
//SaveGnssInfo(nmea, LocFusion, NULL);
|
||||||
|
|
||||||
|
// 有GPS则采用GPS融合定位
|
||||||
|
//int flag = GnssInsFusionLocation(nmea, &g_kfPara, LocFusion);
|
||||||
|
|
||||||
|
//if (flag != TYPE_FIX_NONE) {
|
||||||
|
// // 如果是开车这种，直接输出GPS，不进行平滑处理
|
||||||
|
// LocFusion->latitude = R2D(LocFusion->latitude);
|
||||||
|
// LocFusion->longitudinal = R2D(LocFusion->longitudinal);
|
||||||
|
|
||||||
|
// LocFusion->last_lat = LocFusion->latitude;
|
||||||
|
// LocFusion->last_lon = LocFusion->longitudinal;
|
||||||
|
// type = 1;
|
||||||
|
//}
|
||||||
|
//else if (LocFusion->last_lat != 0.0 && LocFusion->last_lon != 0.0) {
|
||||||
|
// LocFusion->latitude = LocFusion->last_lat;
|
||||||
|
// LocFusion->longitudinal = LocFusion->last_lon;
|
||||||
|
// type = 1;
|
||||||
|
//}
|
||||||
|
//ClearNmeaFlg(nmea);
|
||||||
|
return type;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**----------------------------------------------------------------------
|
||||||
|
* Function : GetPDRVersion
|
||||||
|
* Description : 获取pdr版本号
|
||||||
|
* Date : 2022/10/15 logzhan
|
||||||
|
*---------------------------------------------------------------------**/
|
||||||
|
const char* GetPDRVersion(void) {
|
||||||
|
return PDR_Version;
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,20 @@
|
||||||
|
#ifndef _PDR_INTERFACE_H
|
||||||
|
#define _PDR_INTERFACE_H
|
||||||
|
#include "PDRBase.h"
|
||||||
|
|
||||||
|
/**---------------------------------------------------------------------
|
||||||
|
* Function : PDRLocationMainLoop
|
||||||
|
* Description : PDR¶¨Î»Ö÷Ñ»·
|
||||||
|
* Date : 2022/11/1 logzhan
|
||||||
|
*---------------------------------------------------------------------**/
|
||||||
|
int PDRLocationMainLoop(IMU_t* imu, Nmea_t* nmea, LctFs_t* LocFusion);
|
||||||
|
|
||||||
|
|
||||||
|
/**----------------------------------------------------------------------
|
||||||
|
* Function : GetPDRVersion
|
||||||
|
* Description : »ñÈ¡pdr°æ±¾ºÅ
|
||||||
|
* Date : 2022/10/15 logzhan
|
||||||
|
*---------------------------------------------------------------------**/
|
||||||
|
const char* GetPDRVersion(void);
|
||||||
|
|
||||||
|
#endif // _PDR_INTERFACE_H
|
||||||
|
|
@ -0,0 +1,130 @@
|
||||||
|
#include <string.h>
|
||||||
|
#include "Kalman.h"
|
||||||
|
#include "Matrix.h"
|
||||||
|
|
||||||
|
// 扩展卡尔曼向量缓存
|
||||||
|
static double EkfVecBuf[5][N] = { {0.0} };
|
||||||
|
// 扩展卡尔曼矩阵缓存
|
||||||
|
static double EkfMatBuf[7][N][N] = { { {0.0} } };
|
||||||
|
|
||||||
|
/**----------------------------------------------------------------------
|
||||||
|
* Function : EKF_Init
|
||||||
|
* Description : 初始化卡尔曼滤波器相关
|
||||||
|
* Date : 2022/09/21 logzhan
|
||||||
|
*---------------------------------------------------------------------**/
|
||||||
|
void EKF_Init(void) {
|
||||||
|
memset(EkfMatBuf, 0, sizeof(double) * 7 * N * N);
|
||||||
|
memset(EkfVecBuf, 0, sizeof(double) * 5 * N);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
/**----------------------------------------------------------------------
|
||||||
|
* Function : EKFCalQRMatrix
|
||||||
|
* Description : 根据GPS信号特征调整卡尔曼滤波噪声矩阵，q矩阵是过程噪声矩阵，
|
||||||
|
* 需要跟惯导预测位置精度相关。r矩阵为GNSS观测噪声，跟GPS输出的
|
||||||
|
* 信息精度有关。
|
||||||
|
* Date : 2022/09/21 logzhan
|
||||||
|
*---------------------------------------------------------------------**/
|
||||||
|
void EKFCalQRMatrix() {
|
||||||
|
memset(EkfMatBuf, 0, sizeof(double) * 7 * N * N);
|
||||||
|
memset(EkfVecBuf, 0, sizeof(double) * 5 * N);
|
||||||
|
}
|
||||||
|
|
||||||
|
/**----------------------------------------------------------------------
|
||||||
|
* Function : EKFStateUpdate
|
||||||
|
* Description : 扩展卡尔曼滤波器的状态更新方程
|
||||||
|
* Date : 2020/7/22 logzhan
|
||||||
|
*---------------------------------------------------------------------**/
|
||||||
|
void EKFStateUpdate(EKFPara_t* kf) {
|
||||||
|
double lambda = 0;
|
||||||
|
|
||||||
|
// NxN矩阵定义
|
||||||
|
double(*H)[N] = (double(*)[N]) & (EkfMatBuf[0][0][0]);
|
||||||
|
double(*I)[N] = (double(*)[N]) & (EkfMatBuf[1][0][0]);
|
||||||
|
double(*pvec1)[N] = (double(*)[N]) & (EkfMatBuf[2][0][0]);
|
||||||
|
double(*Ht)[N] = (double(*)[N]) & (EkfMatBuf[3][0][0]);
|
||||||
|
double(*pvec3)[N] = (double(*)[N]) & (EkfMatBuf[4][0][0]);
|
||||||
|
double(*pvec4)[N] = (double(*)[N]) & (EkfMatBuf[5][0][0]);
|
||||||
|
double(*pv3)[N] = (double(*)[N]) & (EkfMatBuf[6][0][0]);
|
||||||
|
|
||||||
|
memset(EkfMatBuf, 0, sizeof(double) * 7 * N * N);
|
||||||
|
memset(EkfVecBuf, 0, sizeof(double) * 5 * N);
|
||||||
|
|
||||||
|
// Nx1向量定义
|
||||||
|
double* z = &(EkfVecBuf[0][0]);
|
||||||
|
double(*pvec5) = &(EkfVecBuf[1][0]);
|
||||||
|
double(*pvec6) = &(EkfVecBuf[2][0]);
|
||||||
|
double(*pv1) = &(EkfVecBuf[3][0]);
|
||||||
|
double(*pv2) = &(EkfVecBuf[4][0]);
|
||||||
|
|
||||||
|
// 创建单位矩阵
|
||||||
|
for (char i = 0; i < 4; i++) {
|
||||||
|
H[i][i] = 1;
|
||||||
|
I[i][i] = 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
//z[0] = pgnss->xNed; // 北向位置
|
||||||
|
//z[1] = pgnss->yNed; // 东向位置
|
||||||
|
//z[2] = 0.5; // 步长默认0.5
|
||||||
|
//z[3] = pgnss->yaw * PI / 180;
|
||||||
|
|
||||||
|
// 计算 K = K = PHt / (R + H*P*H^T)
|
||||||
|
MatrixTrans(H, Ht); // 计算Ht
|
||||||
|
MatrixMultiply(H, kf->pPk, pvec1); // 计算HP
|
||||||
|
MatrixMultiply(pvec1, Ht, pvec3); // pvec3 = H*P*H^T
|
||||||
|
MatrixAdd(pvec3, kf->R, pvec4); // pvec4 = R + H*P*H^T
|
||||||
|
MatrixInverse(pvec4, pvec1); // 计算 1 / (R + H*P*H^T)
|
||||||
|
|
||||||
|
for (char i = 0; i < N; i++) {
|
||||||
|
for (char j = 0; j < N; j++) {
|
||||||
|
pv3[i][j] = pvec3[i][j];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
MatrixMultiply(kf->pPk, Ht, pvec3); // 计算PHt
|
||||||
|
MatrixMultiply(pvec3, pvec1, kf->Kk); // 计算增益K = PHt / (R + H*P*H^T)
|
||||||
|
|
||||||
|
|
||||||
|
VecMatMultiply(kf->pXk, H, pvec5); // pvec5 = Hx'
|
||||||
|
VectorSub(z, pvec5, pvec6); // pvec6 = Z - Hx'
|
||||||
|
//modAngle(&pvec6[3], -PI, PI);
|
||||||
|
for (char i = 0; i < N; i++) {
|
||||||
|
pv1[i] = pvec6[i];
|
||||||
|
}
|
||||||
|
|
||||||
|
VecMatMultiply(pvec6, kf->Kk, pvec5); // pvec5 = K*( z - Hx')
|
||||||
|
// Get the optimally updated state estimation
|
||||||
|
VectorAdd(kf->pXk, pvec5, kf->Xk);
|
||||||
|
|
||||||
|
// pvec1 = K*H
|
||||||
|
MatrixMultiply(kf->Kk, H, pvec1);
|
||||||
|
// pvec2 = (I - K*H)
|
||||||
|
MatrixSub(I, pvec1, Ht);
|
||||||
|
// pvec3 = (I - K*H)*P
|
||||||
|
MatrixMultiply(Ht, kf->pPk, pvec3);
|
||||||
|
// pvec4 = (I- K*H)^T
|
||||||
|
MatrixTrans(Ht, pvec4);
|
||||||
|
// pvec1 = (I - K*H)*P*(I- K*H)^T
|
||||||
|
MatrixMultiply(pvec3, pvec4, pvec1);
|
||||||
|
|
||||||
|
// pvec2 = K*R
|
||||||
|
MatrixMultiply(kf->Kk, kf->R, Ht);
|
||||||
|
// pvec3 = K^T
|
||||||
|
MatrixTrans(kf->Kk, pvec3);
|
||||||
|
// pvec4 = K*R*K^T
|
||||||
|
MatrixMultiply(Ht, pvec3, pvec4);
|
||||||
|
// Get the updated estimate covariance: P' = (I - K*H)*P*(I- K*H)^T + K*R*K^T
|
||||||
|
MatrixAdd(pvec1, pvec4, kf->Pk);
|
||||||
|
|
||||||
|
// pv2 = (z - Hx')*( H*P*H^T )
|
||||||
|
VecMatMultiply(pv1, pv3, pv2);
|
||||||
|
}
|
||||||
|
|
||||||
|
/**----------------------------------------------------------------------
|
||||||
|
* Function : EKFUpdateInsState
|
||||||
|
* Description : 扩展卡尔曼滤波器更新INS状态
|
||||||
|
* Date : 2022/09/23 logzhan
|
||||||
|
*---------------------------------------------------------------------**/
|
||||||
|
void EKFUpdateInsState() {
|
||||||
|
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,36 @@
|
||||||
|
#ifndef _PDR_KALMAN_H_
|
||||||
|
#define _PDR_KALMAN_H_
|
||||||
|
|
||||||
|
#include "PDRBase.h"
|
||||||
|
|
||||||
|
/**----------------------------------------------------------------------
|
||||||
|
* Function : EKF_Init
|
||||||
|
* Description : 初始化卡尔曼滤波器相关
|
||||||
|
* Date : 2022/09/21 logzhan
|
||||||
|
*---------------------------------------------------------------------**/
|
||||||
|
void EKF_Init(void);
|
||||||
|
|
||||||
|
/**----------------------------------------------------------------------
|
||||||
|
* Function : EKFCalQRMatrix
|
||||||
|
* Description : 根据GPS信号特征调整卡尔曼滤波噪声矩阵，q矩阵是过程噪声矩阵，
|
||||||
|
* 需要跟惯导预测位置精度相关。r矩阵为GNSS观测噪声，跟GPS输出的
|
||||||
|
* 信息精度有关。
|
||||||
|
* Date : 2022/09/21 logzhan
|
||||||
|
*---------------------------------------------------------------------**/
|
||||||
|
void EKFCalQRMatrix();
|
||||||
|
|
||||||
|
/**----------------------------------------------------------------------
|
||||||
|
* Function : EKFStateUpdate
|
||||||
|
* Description : 扩展卡尔曼滤波器的状态更新方程
|
||||||
|
* Date : 2020/7/22 logzhan
|
||||||
|
*---------------------------------------------------------------------**/
|
||||||
|
void EKFStateUpdate(EKFPara_t* kf);
|
||||||
|
|
||||||
|
/**----------------------------------------------------------------------
|
||||||
|
* Function : EKFUpdateInsState
|
||||||
|
* Description : 扩展卡尔曼滤波器更新INS状态
|
||||||
|
* Date : 2022/09/23 logzhan
|
||||||
|
*---------------------------------------------------------------------**/
|
||||||
|
void EKFUpdateInsState(void);
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,157 @@
|
||||||
|
/******************** (C) COPYRIGHT 2022 Geek************************************
|
||||||
|
* File Name : PDR_KmlSupport.cpp
|
||||||
|
* Current Version : V1.0
|
||||||
|
* Author : logzhan
|
||||||
|
* Date of Issued : 2022.09.15
|
||||||
|
* Comments : PDR Google地图KML格式输出
|
||||||
|
********************************************************************************/
|
||||||
|
#include "KmlSupport.h"
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
|
using namespace PDR;
|
||||||
|
|
||||||
|
/**----------------------------------------------------------------------
|
||||||
|
* Function : KmlWrite
|
||||||
|
* Description : 将pdr算法输出的gps和pdr轨迹写为kml形式
|
||||||
|
* path : kml文件的输出文件路径
|
||||||
|
* name : kml文件主体名称
|
||||||
|
* postfix ：在主体名称后面添加的后缀，用于区分类型或者版本
|
||||||
|
* Date : 2020/11/1 logzhan
|
||||||
|
*---------------------------------------------------------------------**/
|
||||||
|
void KmlWrite(string path, string name, string postfix, vector<KmlTracks_t>& gps,
|
||||||
|
vector<KmlTracks_t>& pdr)
|
||||||
|
{
|
||||||
|
string pdrColor = "ff0000ff";
|
||||||
|
string gpsColor = "ff00ffff";
|
||||||
|
|
||||||
|
if (path == "") {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
string kmlPath = path + name + postfix + ".kml";
|
||||||
|
string kmlName = name + postfix;
|
||||||
|
FILE* fid = NULL;
|
||||||
|
|
||||||
|
fopen_s(&fid,kmlPath.c_str(), "w");
|
||||||
|
|
||||||
|
if (fid == NULL) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
fprintf(fid, "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n");
|
||||||
|
fprintf(fid, "<kml xmlns=\"http://www.opengis.net/kml/2.2\">\n");
|
||||||
|
fprintf(fid, "\t<Document>\n");
|
||||||
|
fprintf(fid, "\t\t<name>%s</name>\n", kmlName.c_str());
|
||||||
|
|
||||||
|
fprintf(fid, "\t\t<Folder id=\"ID1\">\n");
|
||||||
|
fprintf(fid, "\t\t<name>GPS Fixs</name>\n");
|
||||||
|
|
||||||
|
// 写gps结果
|
||||||
|
for (int i = 0; i < gps.size(); i++) {
|
||||||
|
fprintf(fid, "\t\t<Placemark>\n");
|
||||||
|
fprintf(fid, "\t\t\t<Style>\n");
|
||||||
|
fprintf(fid, "\t\t\t\t<IconStyle>\n");
|
||||||
|
fprintf(fid, "\t\t\t\t\t<Icon>\n");
|
||||||
|
fprintf(fid, "\t\t\t\t\t\t<href>%s</href>\n",
|
||||||
|
"http://maps.google.com/mapfiles/kml/shapes/arrow.png");
|
||||||
|
fprintf(fid, "\t\t\t\t\t</Icon>\n");
|
||||||
|
fprintf(fid, "\t\t\t\t\t<scale>%.2f</scale>\n", 0.4);
|
||||||
|
|
||||||
|
// 描述轨迹方向信息
|
||||||
|
fprintf(fid, "\t\t\t\t\t<heading>%.7f</heading>\n", gps[i].heading);
|
||||||
|
fprintf(fid, "\t\t\t\t\t<color>%s</color>\n", gpsColor.c_str());
|
||||||
|
fprintf(fid, "\t\t\t\t</IconStyle>\n");
|
||||||
|
fprintf(fid, "\t\t\t</Style>\n");
|
||||||
|
|
||||||
|
// 写入时间信息
|
||||||
|
//double h, m, s;
|
||||||
|
//pdr_utc2hms(gps[i].utcTime, &h, &m, &s);
|
||||||
|
//fprintf(fid, "\t\t\t<TimeStamp>\n");
|
||||||
|
//fprintf(fid, "\t\t\t\t<when>2021-01-22T%s:%s:%s</when>\n", time2str(h).c_str(),
|
||||||
|
// time2str(m).c_str(), time2str(s).c_str());
|
||||||
|
//fprintf(fid, "\t\t\t</TimeStamp>\n");
|
||||||
|
|
||||||
|
// 写入描述信息
|
||||||
|
fprintf(fid, "\t\t\t<description>\n");
|
||||||
|
fprintf(fid, "\t\t\t\t<![CDATA[\n");
|
||||||
|
fprintf(fid, "\t\t\t\t<dl>\n");
|
||||||
|
fprintf(fid, "\t\t\t\t<dd>East : 0.0 (m/s)</dd>\n");
|
||||||
|
fprintf(fid, "\t\t\t\t<dd>HDOP : %.2f (m/s)</dd>\n",
|
||||||
|
gps[i].hdop);
|
||||||
|
fprintf(fid, "\t\t\t\t<dd>accuracy : %.2f (m)</dd>\n",
|
||||||
|
gps[i].accuracy);
|
||||||
|
fprintf(fid, "\t\t\t\t<dd>speed : %.2f (m/s)</dd>\n",
|
||||||
|
gps[i].vel);
|
||||||
|
fprintf(fid, "\t\t\t\t<dd>Heading : %.2f (degrees) </dd>\n",
|
||||||
|
gps[i].heading);
|
||||||
|
fprintf(fid, "\t\t\t\t</dl><hr>]]>\n");
|
||||||
|
fprintf(fid, "\t\t\t</description>\n");
|
||||||
|
// 写入坐标信息
|
||||||
|
fprintf(fid, "\t\t\t<Point>\n");
|
||||||
|
fprintf(fid, "\t\t\t\t<coordinates>%.10f,%.10f</coordinates>\n",
|
||||||
|
gps[i].lon,
|
||||||
|
gps[i].lat);
|
||||||
|
fprintf(fid, "\t\t\t</Point>\n");
|
||||||
|
fprintf(fid, "\t\t</Placemark>\n");
|
||||||
|
}
|
||||||
|
fprintf(fid, "\t\t</Folder>\n");
|
||||||
|
|
||||||
|
fprintf(fid, "\t\t<Folder id=\"ID2\">\n");
|
||||||
|
fprintf(fid, "\t\t<name>PDR Fixs</name>\n");
|
||||||
|
// 写pdr结果
|
||||||
|
for (int i = 0; i < pdr.size(); i++) {
|
||||||
|
fprintf(fid, "\t\t<Placemark>\n");
|
||||||
|
fprintf(fid, "\t\t\t<Style>\n");
|
||||||
|
fprintf(fid, "\t\t\t\t<IconStyle>\n");
|
||||||
|
fprintf(fid, "\t\t\t\t\t<Icon>\n");
|
||||||
|
fprintf(fid, "\t\t\t\t\t\t<href>%s</href>\n",
|
||||||
|
"http://maps.google.com/mapfiles/kml/shapes/arrow.png");
|
||||||
|
fprintf(fid, "\t\t\t\t\t</Icon>\n");
|
||||||
|
fprintf(fid, "\t\t\t\t\t<scale>%.2f</scale>\n", 0.4);
|
||||||
|
fprintf(fid, "\t\t\t\t\t<heading>%.7f</heading>\n",
|
||||||
|
pdr[i].heading);
|
||||||
|
|
||||||
|
fprintf(fid, "\t\t\t\t\t<color>%s</color>\n", pdrColor.c_str());
|
||||||
|
fprintf(fid, "\t\t\t\t</IconStyle>\n");
|
||||||
|
fprintf(fid, "\t\t\t</Style>\n");
|
||||||
|
|
||||||
|
// 写入时间信息
|
||||||
|
//double h, m, s;
|
||||||
|
//pdr_utc2hms(resTracks.pdrTrack[i].time, &h, &m, &s);
|
||||||
|
//fprintf(fid, "\t\t\t<TimeStamp>\n");
|
||||||
|
//fprintf(fid, "\t\t\t\t<when>2021-01-22T%s:%s:%s</when>\n", time2str(h).c_str(),
|
||||||
|
// time2str(m).c_str(), time2str(s).c_str());
|
||||||
|
//fprintf(fid, "\t\t\t</TimeStamp>\n");
|
||||||
|
|
||||||
|
// description
|
||||||
|
fprintf(fid, "\t\t\t<description>\n");
|
||||||
|
fprintf(fid, "\t\t\t\t<![CDATA[\n");
|
||||||
|
fprintf(fid, "\t\t\t\t<dl>\n");
|
||||||
|
fprintf(fid, "\t\t\t\t<dd>East : 0.0 (m/s)</dd>\n");
|
||||||
|
fprintf(fid, "\t\t\t\t<dd>HDOP : %.2f (m/s)</dd>\n",
|
||||||
|
pdr[i].hdop);
|
||||||
|
|
||||||
|
//fprintf(fid, "\t\t\t\t<dd>MOTION TYPE : %s (m/s)</dd>\n",
|
||||||
|
// motionType2Str(resTracks.pdrTrack[i].motionType));
|
||||||
|
|
||||||
|
fprintf(fid, "\t\t\t\t<dd>accuracy : %.2f (m/s)</dd>\n",
|
||||||
|
pdr[i].accuracy);
|
||||||
|
fprintf(fid, "\t\t\t\t<dd>Heading : %.2f (degrees) </dd>\n",
|
||||||
|
pdr[i].heading);
|
||||||
|
fprintf(fid, "\t\t\t\t</dl><hr>]]>\n");
|
||||||
|
fprintf(fid, "\t\t\t</description>\n");
|
||||||
|
|
||||||
|
fprintf(fid, "\t\t\t<Point>\n");
|
||||||
|
fprintf(fid, "\t\t\t\t<coordinates>%.10f,%.10f</coordinates>\n",
|
||||||
|
pdr[i].lon,
|
||||||
|
pdr[i].lat);
|
||||||
|
fprintf(fid, "\t\t\t</Point>\n");
|
||||||
|
fprintf(fid, "\t\t</Placemark>\n");
|
||||||
|
}
|
||||||
|
fprintf(fid, "\t\t</Folder>\n");
|
||||||
|
|
||||||
|
fprintf(fid, "\t</Document>\n");
|
||||||
|
fprintf(fid, "</kml>\n");
|
||||||
|
|
||||||
|
fclose(fid);
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,23 @@
|
||||||
|
#ifndef _PDR_KML_SUPPORT_H
|
||||||
|
#define _PDR_KML_SUPPORT_H
|
||||||
|
|
||||||
|
#include <string>
|
||||||
|
using namespace std;
|
||||||
|
|
||||||
|
namespace PDR {
|
||||||
|
|
||||||
|
typedef struct ResultTracks {
|
||||||
|
double lat;
|
||||||
|
double lon;
|
||||||
|
double heading;
|
||||||
|
double utcTime;
|
||||||
|
double hdop;
|
||||||
|
double accuracy;
|
||||||
|
double vel;
|
||||||
|
double time;
|
||||||
|
string motionType;
|
||||||
|
}KmlTracks_t;
|
||||||
|
|
||||||
|
|
||||||
|
}
|
||||||
|
#endif // _PDR_KML_SUPPORT_H
|
||||||
|
|
@ -0,0 +1,62 @@
|
||||||
|
/******************** (C) COPYRIGHT 2022 Geek************************************
|
||||||
|
* File Name : Location.cpp
|
||||||
|
* Current Version : V1.0
|
||||||
|
* Author : logzhan
|
||||||
|
* Date of Issued : 2022.09.21
|
||||||
|
* Comments : PDR导航算法主流程
|
||||||
|
********************************************************************************/
|
||||||
|
/* Header File Including -----------------------------------------------------*/
|
||||||
|
#include "DirectionEstimator.h"
|
||||||
|
#include "Kalman.h"
|
||||||
|
#include "AHRS.h"
|
||||||
|
#include "Pedometer.h"
|
||||||
|
#include "Detector.h"
|
||||||
|
#include "Location.h"
|
||||||
|
|
||||||
|
/* Global Variable Definition ------------------------------------------------*/
|
||||||
|
EKFPara_t EkfPara;
|
||||||
|
PDR_t PDR;
|
||||||
|
|
||||||
|
/**---------------------------------------------------------------------
|
||||||
|
* Function : PDRNav_Init
|
||||||
|
* Description : PDR导航系统初始化
|
||||||
|
* Date : 2022/09/21 logzhan
|
||||||
|
*---------------------------------------------------------------------**/
|
||||||
|
void PDRNav_Init(void)
|
||||||
|
{
|
||||||
|
Detector_Init();
|
||||||
|
AHRS_Init();
|
||||||
|
EKF_Init();
|
||||||
|
Pedometer_Init();
|
||||||
|
DirectionEstimator_Init();
|
||||||
|
}
|
||||||
|
|
||||||
|
/**----------------------------------------------------------------------
|
||||||
|
* Function : InsLocation
|
||||||
|
* Description : PDR惯导位置更新
|
||||||
|
* Date : 2022-09-21
|
||||||
|
*---------------------------------------------------------------------**/
|
||||||
|
void InsLocationUpdate(IMU_t* ImuData, EKFPara_t* Ekf)
|
||||||
|
{
|
||||||
|
if (UpdateAHRS(ImuData)) {
|
||||||
|
|
||||||
|
}
|
||||||
|
/* Updating imu info in order to detect user moving type. */
|
||||||
|
DetectorUpdateIMU(ImuData);
|
||||||
|
/* calculate user step info update. */
|
||||||
|
PedometerUpdate(ImuData, &PDR.Steps);
|
||||||
|
/* Using multiply sensor info to predict user real moving direction. */
|
||||||
|
DirectionPredict(&PDR.Heading);
|
||||||
|
/* Extend kalman filter update system state when ins data update. */
|
||||||
|
EKFUpdateInsState();
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
/**----------------------------------------------------------------------
|
||||||
|
* Function : GnssInsLocFusion
|
||||||
|
* Description : PDR的GNSS和INS融合定位
|
||||||
|
* Date : 2022/09/21 logzhan
|
||||||
|
*---------------------------------------------------------------------**/
|
||||||
|
void GnssInsLocationUpdate(void) {
|
||||||
|
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,27 @@
|
||||||
|
#ifndef _PDR_LOCATION_H_
|
||||||
|
#define _PDR_LOCATION_H_
|
||||||
|
|
||||||
|
#include "PDRBase.h"
|
||||||
|
|
||||||
|
/**---------------------------------------------------------------------
|
||||||
|
* Function : PDRNav_Init
|
||||||
|
* Description : PDR导航系统初始化
|
||||||
|
* Date : 2022/09/21 logzhan
|
||||||
|
*---------------------------------------------------------------------**/
|
||||||
|
void PDRNav_Init(void);
|
||||||
|
|
||||||
|
/**----------------------------------------------------------------------
|
||||||
|
* Function : InsLocation
|
||||||
|
* Description : PDR惯导位置更新
|
||||||
|
* Date : 2022-09-21 logzhan
|
||||||
|
*---------------------------------------------------------------------**/
|
||||||
|
void InsLocationUpdate(IMU_t* ImuData, EKFPara_t* Ekf);
|
||||||
|
|
||||||
|
/**----------------------------------------------------------------------
|
||||||
|
* Function : GnssInsLocFusion
|
||||||
|
* Description : PDR的GNSS和INS融合定位
|
||||||
|
* Date : 2022/09/21 logzhan
|
||||||
|
*---------------------------------------------------------------------**/
|
||||||
|
void GnssInsLocationUpdate(void);
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,195 @@
|
||||||
|
/******************** (C) COPYRIGHT 2022 Geek************************************
|
||||||
|
* File Name : Matrix.c
|
||||||
|
* Current Version : V1.0
|
||||||
|
* Author : logzhan
|
||||||
|
* Date of Issued : 2022.09.14
|
||||||
|
* Comments : PDR矩阵运算库
|
||||||
|
********************************************************************************/
|
||||||
|
/* Header File Including -----------------------------------------------------*/
|
||||||
|
#include "Matrix.h"
|
||||||
|
|
||||||
|
/**---------------------------------------------------------------------
|
||||||
|
* Function : MatrixTrans
|
||||||
|
* Description : 矩阵转置
|
||||||
|
* Date : 2022/09/14 logzhan
|
||||||
|
*---------------------------------------------------------------------**/
|
||||||
|
void MatrixTrans(double a[N][N], double r[N][N]) {
|
||||||
|
int i, j;
|
||||||
|
for (i = 0; i < N; i++) {
|
||||||
|
for (j = 0; j < N; j++) {
|
||||||
|
r[i][j] = a[j][i];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/**---------------------------------------------------------------------
|
||||||
|
* Function : VecMatMultiply
|
||||||
|
* Description : 向量和矩阵相乘 r = b * a
|
||||||
|
* Date : 2022/09/14 logzhan
|
||||||
|
*---------------------------------------------------------------------**/
|
||||||
|
void VecMatMultiply(double a[N], double b[N][N], double r[N]) {
|
||||||
|
int i, j;
|
||||||
|
double temp[N] = { 0.0 };
|
||||||
|
//for (i = 0; i < N; i++) {
|
||||||
|
// temp[i] = a[i];
|
||||||
|
//}
|
||||||
|
for (i = 0; i < N; i++) {
|
||||||
|
for (j = 0; j < N; j++) {
|
||||||
|
temp[i] += ((b[i][j] * 100) * (a[j] * 100)) / 10000;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
for (i = 0; i < N; i++) {
|
||||||
|
r[i] = temp[i];
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
/**---------------------------------------------------------------------
|
||||||
|
* Function : MatrixMultiply
|
||||||
|
* Description : 矩阵和矩阵相乘 r = a * b
|
||||||
|
* Date : 2022/09/14 logzhan
|
||||||
|
*---------------------------------------------------------------------**/
|
||||||
|
void MatrixMultiply(double a[N][N], double b[N][N], double r[N][N]) {
|
||||||
|
int i, j, m;
|
||||||
|
double temp[N][N] = { { 0.0 } };
|
||||||
|
for (i = 0; i < N; i++) {
|
||||||
|
for (j = 0; j < N; j++) {
|
||||||
|
for (m = 0; m < N; m++) {
|
||||||
|
//temp[i][j] += a[j][m] * b[m][j];
|
||||||
|
temp[i][j] += a[i][m] * b[m][j];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
for (i = 0; i < N; i++) {
|
||||||
|
for (j = 0; j < N; j++) {
|
||||||
|
r[i][j] = temp[i][j];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/**---------------------------------------------------------------------
|
||||||
|
* Function : MatrixAdd
|
||||||
|
* Description : 矩阵和矩阵相加 r = a + b, 注意这个函数支持a = a + b
|
||||||
|
* Date : 2022/09/14 logzhan
|
||||||
|
*---------------------------------------------------------------------**/
|
||||||
|
void MatrixAdd(double a[N][N], double b[N][N], double r[N][N]) {
|
||||||
|
int i, j;
|
||||||
|
for (i = 0; i < N; i++) {
|
||||||
|
for (j = 0; j < N; j++) {
|
||||||
|
r[i][j] = a[i][j] + b[i][j];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
/**---------------------------------------------------------------------
|
||||||
|
* Function : VectorAdd
|
||||||
|
* Description : 向量和向量相加 r = a + b, 注意这个函数支持a = a + b
|
||||||
|
* Date : 2022/09/14 logzhan
|
||||||
|
*---------------------------------------------------------------------**/
|
||||||
|
void VectorAdd(double a[N], double b[N], double r[N]) {
|
||||||
|
int i;
|
||||||
|
for (i = 0; i < N; i++) {
|
||||||
|
r[i] = a[i] + b[i];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/**---------------------------------------------------------------------
|
||||||
|
* Function : MatrixSub
|
||||||
|
* Description : 矩阵和矩阵相减 r = a - b, 注意这个函数支持a = a - b
|
||||||
|
* Date : 2022/09/14 logzhan
|
||||||
|
*---------------------------------------------------------------------**/
|
||||||
|
void MatrixSub(double a[N][N], double b[N][N], double r[N][N]) {
|
||||||
|
int i, j;
|
||||||
|
for (i = 0; i < N; i++) {
|
||||||
|
for (j = 0; j < N; j++) {
|
||||||
|
r[i][j] = a[i][j] - b[i][j];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
/**---------------------------------------------------------------------
|
||||||
|
* Function : VectorSub
|
||||||
|
* Description : 向量和向量相减 r = a - b, 注意这个函数支持a = a - b
|
||||||
|
* Date : 2022/09/14 logzhan
|
||||||
|
*---------------------------------------------------------------------**/
|
||||||
|
void VectorSub(double a[N], double b[N], double r[N]) {
|
||||||
|
int i;
|
||||||
|
for (i = 0; i < N; i++) {
|
||||||
|
r[i] = a[i] - b[i];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/**---------------------------------------------------------------------
|
||||||
|
* Function : MatrixInverse
|
||||||
|
* Description : 求矩阵的逆矩阵
|
||||||
|
* Date : 2022/09/14 logzhan
|
||||||
|
*---------------------------------------------------------------------**/
|
||||||
|
void MatrixInverse(double(*M)[N], double (*MInv)[N]) {
|
||||||
|
|
||||||
|
double l[N][N] = { { 0.0 } };
|
||||||
|
double u[N][N] = { { 0.0 } };
|
||||||
|
double LInv[N][N] = { { 0.0 } };
|
||||||
|
double UInv[N][N] = { { 0.0 } };
|
||||||
|
double Temp[N][N] = { { 0.0 } };
|
||||||
|
|
||||||
|
int i, j, k;
|
||||||
|
double s;
|
||||||
|
|
||||||
|
for (i = 0; i < N; i++)l[i][i] = 1;
|
||||||
|
|
||||||
|
for (i = 0; i < N; i++){
|
||||||
|
for (j = i; j <N; j++){
|
||||||
|
s = 0;
|
||||||
|
for (k = 0; k < i; k++){
|
||||||
|
s += l[i][k] * u[k][j];
|
||||||
|
}
|
||||||
|
u[i][j] = M[i][j] - s;
|
||||||
|
}
|
||||||
|
for (j = i + 1; j < N; j++){
|
||||||
|
s = 0;
|
||||||
|
for (k = 0; k < i; k++){
|
||||||
|
s += l[j][k] * u[k][i];
|
||||||
|
}
|
||||||
|
l[j][i] = (M[j][i] - s) / u[i][i];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
for (i = 0; i < N; i++)LInv[i][i] = 1;
|
||||||
|
|
||||||
|
for (i = 1; i < N; i++){
|
||||||
|
for (j = 0; j < i; j++){
|
||||||
|
s = 0;
|
||||||
|
for (k = 0; k < i; k++){
|
||||||
|
s += l[i][k] * LInv[k][j];
|
||||||
|
}
|
||||||
|
LInv[i][j] = -s;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
for (i = 0; i < N; i++){
|
||||||
|
UInv[i][i] = 1 / u[i][i];
|
||||||
|
}
|
||||||
|
for (i = 1; i < N; i++)
|
||||||
|
{
|
||||||
|
for (j = i - 1; j >= 0; j--)
|
||||||
|
{
|
||||||
|
s = 0;
|
||||||
|
for (k = j + 1; k <= i; k++)
|
||||||
|
{
|
||||||
|
s += u[j][k] * UInv[k][i];
|
||||||
|
}
|
||||||
|
UInv[j][i] = -s / u[j][j];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
for (i = 0; i < N; i++){
|
||||||
|
for (j = 0; j < N; j++){
|
||||||
|
for (k = 0; k < N; k++){
|
||||||
|
Temp[i][j] += UInv[i][k] * LInv[k][j];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
for (i = 0; i < N; i++) {
|
||||||
|
for (j = 0; j < N; j++) {
|
||||||
|
MInv[i][j] = Temp[i][j];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,70 @@
|
||||||
|
#ifdef __cplusplus
|
||||||
|
extern "C" {
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifndef _PDR_MATRIX_H_
|
||||||
|
#define _PDR_MATRIX_H_
|
||||||
|
|
||||||
|
#define N 4
|
||||||
|
|
||||||
|
/**---------------------------------------------------------------------
|
||||||
|
* Function : MatrixTrans
|
||||||
|
* Description : 矩阵转置
|
||||||
|
* Date : 2022/09/14 logzhan
|
||||||
|
*---------------------------------------------------------------------**/
|
||||||
|
void MatrixTrans(double a[N][N], double r[N][N]);
|
||||||
|
|
||||||
|
/**---------------------------------------------------------------------
|
||||||
|
* Function : VecMatMultiply
|
||||||
|
* Description : 向量和矩阵相乘 r = b * a
|
||||||
|
* Date : 2022/09/14 logzhan
|
||||||
|
*---------------------------------------------------------------------**/
|
||||||
|
void VecMatMultiply(double a[N], double b[N][N], double r[N]);
|
||||||
|
|
||||||
|
/**---------------------------------------------------------------------
|
||||||
|
* Function : MatrixMultiply
|
||||||
|
* Description : 矩阵和矩阵相乘 r = a * b
|
||||||
|
* Date : 2022/09/14 logzhan
|
||||||
|
*---------------------------------------------------------------------**/
|
||||||
|
void MatrixMultiply(double a[N][N], double b[N][N], double r[N][N]);
|
||||||
|
|
||||||
|
/**---------------------------------------------------------------------
|
||||||
|
* Function : MatrixAdd
|
||||||
|
* Description : 矩阵和矩阵相加 r = a + b, 注意这个函数支持a = a + b
|
||||||
|
* Date : 2022/09/14 logzhan
|
||||||
|
*---------------------------------------------------------------------**/
|
||||||
|
void MatrixAdd(double a[N][N], double b[N][N], double r[N][N]);
|
||||||
|
|
||||||
|
/**---------------------------------------------------------------------
|
||||||
|
* Function : VectorAdd
|
||||||
|
* Description : 向量和向量相加 r = a + b, 注意这个函数支持a = a + b
|
||||||
|
* Date : 2022/09/14 logzhan
|
||||||
|
*---------------------------------------------------------------------**/
|
||||||
|
void VectorAdd(double a[N], double b[N], double r[N]);
|
||||||
|
|
||||||
|
/**---------------------------------------------------------------------
|
||||||
|
* Function : MatrixSub
|
||||||
|
* Description : 矩阵和矩阵相减 r = a - b, 注意这个函数支持a = a - b
|
||||||
|
* Date : 2022/09/14 logzhan
|
||||||
|
*---------------------------------------------------------------------**/
|
||||||
|
void MatrixSub(double a[N][N], double b[N][N], double r[N][N]);
|
||||||
|
|
||||||
|
/**---------------------------------------------------------------------
|
||||||
|
* Function : VectorSub
|
||||||
|
* Description : 向量和向量相减 r = a - b, 注意这个函数支持a = a - b
|
||||||
|
* Date : 2022/09/14 logzhan
|
||||||
|
*---------------------------------------------------------------------**/
|
||||||
|
void VectorSub(double a[N], double b[N], double r[N]);
|
||||||
|
|
||||||
|
/**---------------------------------------------------------------------
|
||||||
|
* Function : MatrixInverse
|
||||||
|
* Description : 求矩阵的逆矩阵
|
||||||
|
* Date : 2022/09/14 logzhan
|
||||||
|
*---------------------------------------------------------------------**/
|
||||||
|
void MatrixInverse(double(*M)[N], double(*MInv)[N]);
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifdef __cplusplus
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,117 @@
|
||||||
|
#ifndef _PDR_BASE_H_
|
||||||
|
#define _PDR_BASE_H_
|
||||||
|
|
||||||
|
#include <stdint.h>
|
||||||
|
|
||||||
|
#define ACCURACY_ERR_MAX 1000 // GPS的accuracy最大值,一般用于初始化用
|
||||||
|
#define N 4 // 矩阵维数
|
||||||
|
#define MAX_NO_GPS_PREDICT 10 // 无GPS信息状态，最大位置推算数量
|
||||||
|
#define IMU_SENSOR_AXIS 3 // IMU传感器数据轴数量，默认3
|
||||||
|
// 用户运动识别
|
||||||
|
#define DETECTOR_TYPE_STATIC 0 // 用户静止
|
||||||
|
#define DETECTOR_TYPE_IRREGULAR 1 // 无规律运动
|
||||||
|
#define DETECTOR_TYPE_HANDHELD 2 // 手持运动
|
||||||
|
#define DETECTOR_TYPE_SWINGING 3 // 摆手运动
|
||||||
|
#define DETECTOR_NO_ERROR 0
|
||||||
|
|
||||||
|
#define TYPE_FIX_NONE 0
|
||||||
|
|
||||||
|
#define PDR_TRUE 1
|
||||||
|
#define PDR_FALSE 0
|
||||||
|
#define NO_UPDATE 0
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
double Xk[N]; // 系统状态变量 xk[0]: 北向x xk[1]：东向y xk[2]：步长 xk[3] ：航向角
|
||||||
|
double pXk[N]; // 最佳预测变量 xk[0]: 北向x xk[1]：东向y xk[2]：步长 xk[3] ：航向角
|
||||||
|
double Zk[N];
|
||||||
|
double pPk[N][N];
|
||||||
|
double Pk[N][N];
|
||||||
|
double Phi[N][N];
|
||||||
|
double hk[N][N];
|
||||||
|
double Q[N][N]; // 卡尔曼滤波的Q矩阵(过程噪声)
|
||||||
|
double R[N][N]; // 卡尔曼滤波R矩阵(观测噪声)
|
||||||
|
double Kk[N][N];
|
||||||
|
double Lambda;
|
||||||
|
double pLat;
|
||||||
|
double pLon;
|
||||||
|
double initHeading; // 初始化航向角
|
||||||
|
}EKFPara_t;
|
||||||
|
|
||||||
|
typedef struct Sensor {
|
||||||
|
uint8_t update;
|
||||||
|
int type;
|
||||||
|
double time;
|
||||||
|
float s[IMU_SENSOR_AXIS];
|
||||||
|
}Sensor_t;
|
||||||
|
|
||||||
|
typedef struct IMU {
|
||||||
|
Sensor_t acc;
|
||||||
|
Sensor_t gyr;
|
||||||
|
Sensor_t mag;
|
||||||
|
}IMU_t;
|
||||||
|
|
||||||
|
// 用户运动类型分类器
|
||||||
|
typedef struct DETECTOR {
|
||||||
|
uint32_t type; // 用户运动类别 ： 0:静止运动 1：无规律运动 2：手持运动 3：摆手运动
|
||||||
|
uint32_t lastType;
|
||||||
|
uint64_t tick; // 次数统计，用于调整检测器工作频率
|
||||||
|
}Detector_t;
|
||||||
|
|
||||||
|
|
||||||
|
typedef struct PDR {
|
||||||
|
uint32_t Status; // PDR当前状态
|
||||||
|
uint32_t MotionType; // 用户运动类型
|
||||||
|
// 速度相关
|
||||||
|
double GnssSpeed; // GNSS速度
|
||||||
|
double Heading; // 航向
|
||||||
|
// 步数相关
|
||||||
|
uint64_t Steps; // 当前步数信息
|
||||||
|
uint64_t LastSteps; // 上一次的步数
|
||||||
|
} PDR_t;
|
||||||
|
|
||||||
|
typedef struct AHRS {
|
||||||
|
uint8_t status;
|
||||||
|
uint8_t stable; // 当前AHRS算法收敛稳定性
|
||||||
|
float error;
|
||||||
|
float q[4];
|
||||||
|
float gravity[3];
|
||||||
|
float x_axis[3];
|
||||||
|
float y_axis[3];
|
||||||
|
float z_axis[3];
|
||||||
|
float Dt;
|
||||||
|
float Kp; // mahony kp比例调节参数
|
||||||
|
float Yaw;
|
||||||
|
float Pitch;
|
||||||
|
float Roll;
|
||||||
|
float insHeading;
|
||||||
|
}AHRS_t;
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
uint8_t Update;
|
||||||
|
double MinTime;
|
||||||
|
double MaxTime;
|
||||||
|
}Nmea_t;
|
||||||
|
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
//LL_NSEW latitude_ns;
|
||||||
|
//LL_NSEW longitudinal_ew;
|
||||||
|
double latitude;
|
||||||
|
double longitudinal;
|
||||||
|
double gpsLat;
|
||||||
|
double gpsLon;
|
||||||
|
double gpsHeading;
|
||||||
|
double pdrHeading;
|
||||||
|
double hdop;
|
||||||
|
double gpsSpeed;
|
||||||
|
double accuracy;
|
||||||
|
double time;
|
||||||
|
double yaw;
|
||||||
|
double lambda;
|
||||||
|
double last_lat;
|
||||||
|
double last_lon;
|
||||||
|
unsigned long step;
|
||||||
|
uint8_t motionType;
|
||||||
|
}LctFs_t;
|
||||||
|
|
||||||
|
#endif // ! _PDR_BASE_H
|
||||||
|
|
@ -0,0 +1,11 @@
|
||||||
|
#include "Pedometer.h"
|
||||||
|
|
||||||
|
|
||||||
|
void Pedometer_Init() {
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void PedometerUpdate(IMU_t* imu, uint64_t* step) {
|
||||||
|
*step = 0;
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,10 @@
|
||||||
|
#ifndef _PDR_PEDOMETER_H_
|
||||||
|
#define _PDR_PEDOMETER_H_
|
||||||
|
|
||||||
|
#include "PDRBase.h"
|
||||||
|
|
||||||
|
void Pedometer_Init(void);
|
||||||
|
|
||||||
|
void PedometerUpdate(IMU_t* imu, uint64_t* step);
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,44 @@
|
||||||
|
#include "math.h"
|
||||||
|
#include "Quaternion.h"
|
||||||
|
|
||||||
|
const float FLT_THRES = 0.000001f; // 浮点数最小阈值
|
||||||
|
|
||||||
|
/**---------------------------------------------------------------------
|
||||||
|
* Function : QuaternionNorm
|
||||||
|
* Description : 四元数归一化
|
||||||
|
* Date : 2022/09/21 logzhan
|
||||||
|
*---------------------------------------------------------------------**/
|
||||||
|
void QuaternionNorm(float* q0, float* q1, float* q2, float* q3) {
|
||||||
|
|
||||||
|
float norm = sqrtf(((*q0) * (*q0) + (*q1) * (*q1) +
|
||||||
|
(*q2) * (*q2) + (*q3) * (*q3)));
|
||||||
|
if (norm > FLT_THRES) {
|
||||||
|
norm = 1 / norm;
|
||||||
|
(*q0) *= norm; (*q1) *= norm;
|
||||||
|
(*q2) *= norm; (*q3) *= norm;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/**---------------------------------------------------------------------
|
||||||
|
* Function : QuaternConj
|
||||||
|
* Description : 四元数求共轭四元数
|
||||||
|
* Date : 2022/09/21 logzhan
|
||||||
|
*---------------------------------------------------------------------**/
|
||||||
|
void QuaternConj(float qc[], float q[]) {
|
||||||
|
qc[0] = q[0];
|
||||||
|
qc[1] = -q[1];
|
||||||
|
qc[2] = -q[2];
|
||||||
|
qc[3] = -q[3];
|
||||||
|
}
|
||||||
|
|
||||||
|
/**---------------------------------------------------------------------
|
||||||
|
* Function : QuaternProd
|
||||||
|
* Description : 四元数乘法
|
||||||
|
* Date : 2022/09/21 logzhan
|
||||||
|
*---------------------------------------------------------------------**/
|
||||||
|
void QuaternProd(float qab[], float qa[], float qb[]) {
|
||||||
|
qab[0] = qa[0] * qb[0] - qa[1] * qb[1] - qa[2] * qb[2] - qa[3] * qb[3];
|
||||||
|
qab[1] = qa[0] * qb[1] + qa[1] * qb[0] + qa[2] * qb[3] - qa[3] * qb[2];
|
||||||
|
qab[2] = qa[0] * qb[2] - qa[1] * qb[3] + qa[2] * qb[0] + qa[3] * qb[1];
|
||||||
|
qab[3] = qa[0] * qb[3] + qa[1] * qb[2] - qa[2] * qb[1] + qa[3] * qb[0];
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,33 @@
|
||||||
|
#ifndef _PDR_QUATERNION_H_
|
||||||
|
#define _PDR_QUATERNION_H_
|
||||||
|
|
||||||
|
#ifdef __cplusplus
|
||||||
|
extern "C" {
|
||||||
|
#endif
|
||||||
|
|
||||||
|
/**---------------------------------------------------------------------
|
||||||
|
* Function : QuaternionNorm
|
||||||
|
* Description : 四元数归一化
|
||||||
|
* Date : 2022/09/21 logzhan
|
||||||
|
*---------------------------------------------------------------------**/
|
||||||
|
void QuaternionNorm(float* q0, float* q1, float* q2, float* q3);
|
||||||
|
|
||||||
|
/**---------------------------------------------------------------------
|
||||||
|
* Function : QuaternConj
|
||||||
|
* Description : 四元数求共轭四元数
|
||||||
|
* Date : 2022/09/21 logzhan
|
||||||
|
*---------------------------------------------------------------------**/
|
||||||
|
void QuaternConj(float qc[], float q[]);
|
||||||
|
|
||||||
|
/**---------------------------------------------------------------------
|
||||||
|
* Function : QuaternProd
|
||||||
|
* Description : 四元数乘法
|
||||||
|
* Date : 2022/09/21 logzhan
|
||||||
|
*---------------------------------------------------------------------**/
|
||||||
|
void QuaternProd(float qab[], float qa[], float qb[]);
|
||||||
|
|
||||||
|
#ifdef __cplusplus
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#endif
|
||||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
After Width: | Height: | Size: 1.5 MiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.5 MiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.5 MiB |
Binary file not shown.
|
After Width: | Height: | Size: 1.5 MiB |
Binary file not shown.
|
After Width: | Height: | Size: 52 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 204 KiB |
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
Loading…
Reference in New Issue