- 博客(36)
- 收藏
- 关注
RSS订阅原创 捷联惯导系统学习6.1(一些卡尔曼滤波处理技术 )
狄拉克冲击函数系统的离散化 实际应用中随机系统的建模往往往往时时间连续的，为了进行计算机仿真和kalman估计，需要对量需时间系统离散化系统的离散化 F(t),G(t):时间参数t的确定性时变矩阵F(t),G(t):时间参数t的确定性时变矩阵F(t),G(t):时间参数t的确定性时变矩阵X˙(t)=F(t)X(t)+G(t)w(t)\dot X(t)=F(t)X(t)+G(t)w(t)X˙(t)=F(t)X(t)+G(t)w(t)...
2020-09-30 13:44:07
549
原创 捷联惯导系统学习5.3(kalman 滤波方程推导 )
什么是状态空间 什么是状态空间 状态是指在系统中可决定系统状态、最小数目变量的有序集合。 而所谓状态空间则是指该系统全部可能状态的集合。状态空间表示法 即为一种将物理系统表示为一组输入、输出及状态的数学模式，而输入、输出及状态之间的关系可用许多一阶微分方程来描述。为了使数学模式不受输入、输出及状态的个数所影响，输入、输出及状态都会以向量的形式表示，而微分方程（若是线性非时变系统，可将微分方程转变为代数方程）则会以矩阵的形式来来表示。状态转移矩阵 （转移概率矩阵）状态转移是指客观事物由一种状态转
2020-09-28 15:16:56
477
原创 捷联惯导系统学习5.2(递归最小二乘法 )
采用递推方式求解最小二乘法（Recursive Least Square RLS）可以有效减小数据存储量。测量方程Z‾k=H‾kX+V‾k\overline Z_k=\overline H_kX+\overline V_kZk=HkX+VkZ‾k=[Z1Z2Z3...Zi],H‾k=[H1H2H3...Hi],V‾k=[V1V2V3...Vi]E[V‾i]=0,E[V‾iV‾iT]=R‾i=diag(R1,R2,R3,...)\overline Z_k=\left[\begin{matrix}Z
2020-09-27 10:37:03
357
原创 捷联惯导系统学习5.1(贝叶斯估计、维纳滤波)
贝叶斯估计贝叶斯风险为最小值时的值为最优值损失函数X~:损失值\tilde X:损失值X~:损失值∣∣.∣∣:范数||.||:范数∣∣.∣∣:范数L(X~)=L(X−X~(Z))L(\tilde X)=L(X-\tilde X(Z))L(X~)=L(X−X~(Z))必须满足：非负性：∣∣X~1∣∣>∣∣X~2∣∣||\tilde X_1||>||\tilde X_2||∣∣X~1∣∣>∣∣X~2∣∣时，有L(X~1)≥L(X~2)≥0L(\tilde X_1)\geq
2020-09-26 21:29:17
204
原创 捷联惯导系统学习5.1(极大似然估计、极大验后估计\加权最小二乘估计)
极大似然估计(Maximum Likelihood,ML)已知：条件概率密度函数：L(x)=p(z∣x)(st:X=x)条件概率密度函数：L(x)=p(z|x) (st:X=x)条件概率密度函数：L(x)=p(z∣x)(st:X=x)此时L(x1)L(x1)L(x1)表示：在X=x1X=x1X=x1处，出现观测点的可能性，L(x1)L(x1)L(x1)可能吸纳越高。定义:在：条件概率密度函数：L(x)=p(z∣x)(st:X=x)条件概率密度函数：L(x)=p(z|x) (st:X=x)条件概
2020-09-25 17:39:01
307
原创 捷联惯导系统学习5.1(最小方差估计和线性最小方差估计)
最小方差估计(最小均方误差MMSE,也称条件期望估计)使方差最小即：X:系统状态量X:系统状态量X:系统状态量V:干扰噪声V:干扰噪声V:干扰噪声X~:估计值与系统状态误差\tilde{X}:估计值与系统状态误差X~:估计值与系统状态误差EX:数学期望E_X:数学期望EX:数学期望MMSE[X^]=MSE[X^]min=tr(E[X~X~T])minMMSE[\hat X]=MSE[\hat X]_{min}=tr(E[\tilde{X}\tilde{X}^T])_{min}MMSE[X^]=
2020-09-25 12:01:46
1815
原创 捷联惯导系统学习5.1(几种最优估计方法)
随机向量的概率密度函数需要掌握概念：随机变量、联合密度函数、边缘分布函数、条件概率密度函数随机变量定义：对任意实数x，{ω：X（ω）<=x}∈F(理解)X(w)：（1）实值函数 （2）X(w)是可测的离散型随机变量的X的取值的期望：散型随机变量baiX的取值和为X对应取值的概率为：X1、X2、X3……Xn，p(X1)、p(X2)、p(X3)……p(Xn)、可则：期望为连续型随机变量的X的取值的期望：f(x):为概率密度函数f(x):为概率密度函数f(x):为概率密度函数联合概率密
2020-09-24 20:08:20
1033
原创 使用Sophus和pangolin实现轨迹误差
绝对轨迹误差（Absolute Trahextiry,ATE）姿态的真实值与姿态的理想值之间的差，实际上是李代数的均方根误差（Root-Mean-Aquared,RMSE）绝对平移误差（Average Translational Error）平移部分真实值与姿态的理想值之间的差实现：...
2020-09-24 11:41:13
570
转载 sophus 使用
非模板类使用转换关系图如下由于我的下载的是模板类参靠SLAM十四讲SO#include <Eigen/Core>#include <Eigen/Dense>#include <Eigen/Geometry>#include <sophus/geometry.hpp>#include <sophus/se3.hpp>#include <sophus/so3.hpp>#include <iostream>
2020-09-23 17:02:35
1961
原创 sophus安装
报错：ubuntu16.04 默认支持的版本为3.2.9,安装sophus 需要3.3 Eigen3 version 3.2.92 found in /usr/include/eigen3, but at least version 3.3.0 is required升级git clone https://gitlab.com/libeigen/eigen.gitmkdir buildcd buildcmake …sudo make installsudo ldconfig -v..
2020-09-23 11:40:08
2709
原创 捷联惯导系统学习4.3(静基座误差)
捷联惯导的动态误差传播本身是一个复杂的时变系统，与特定航行轨迹密切相关，很难去求解。但在静基座条件下，误差传播退化成一个线性定常系统，通过获得其解析，解获得惯导误差传播特性。静基座误差方程静基座条件：vn=0;p=[Lλh]T(固定已知);(roll,yaw,pitch,不变)v^n=0;p=\left[\begin{matrix} L&\lambda&h\end{matrix}\right]^T(固定已知);(roll,yaw,pitch,不变)vn=0;p=[Lλh]T(固定
2020-09-23 10:48:41
1169
原创 李群与李代数二（求导与扰动模型）
BCH公式与近似公式（Baker-Campbell-haudsorff）给出了李代数指数映射乘积的完整公式:[,]:为李括号[ ,]:为李括号[,]:为李括号ln(eAeB)=A+B+12[A,B]+112[A,[A,B]]−112[B,[A,B]]+...ln(e^{A}e^{B})=A+B+\frac{1}{2}[A,B]+\frac{1}{12}[A,[A,B]]-\frac{1}{12}[B,[A,B]]+...ln(eAeB)=A+B+21[A,B]+121[A,[A,B]]−121[
2020-09-22 21:06:55
565
转载 李群与李代数一（定义与映射）
为什要使用？ SLAM中，位姿是未知的，需要求出最符合当前状态的位姿！因为旋转矩阵本身具有约束的（正交且行列式的值为1），需要通过李群代数把位姿估计变成无约束问题群特殊正交群(对乘法是封闭的[即乘法的结果仍属于该集合])SO(3)={R∈R3×3∣RRT=I,det(R)=1}SO(3)=\{R\in R^{3×3}|RR^T=I,det(R)=1\}SO(3)={R∈R3×3∣RRT=I,det(R)=1}特殊欧氏群(对乘法是封闭的[即乘法的结果仍属于该集合])SO(3)={T=[Rt0T1]T
2020-09-21 17:27:25
368
原创 捷联惯导系统学习4.2(捷联惯导误差方程)
惯性传感器测量误差（静态误差）三轴陀螺仪与加速度计测量误差模型一样，只介绍陀螺仪的。静态误差主要包含：陀螺刻度系数误差、失准角误差、不正交误差。w:真实值;w=[wxwywz]w:真实值;w=\left[\begin{matrix} w_x\\w_y\\w_z \end{matrix}\right]w:真实值;w=⎣⎡wxwywz⎦⎤wg:测量值wg=[wgxwgywgz]w_g:测量值w_g=\left[\begin{matrix} w_{gx}\\w_{gy}\\w_{gz} \en
2020-09-21 13:05:28
1630
转载 pangolin库使用三（曲线）
绘制曲线#include <iostream>#include <pangolin/pangolin.h>int main(/*int argc, char* argv[]*/){ // Create OpenGL window in single line pangolin::CreateWindowAndBind("Main",640,480); // Data logger object pangolin::DataLog log;//常规数据格式
2020-09-20 21:57:19
1602
转载 pangolin库使用三（子图, imageTexture,）
#include <pangolin/pangolin.h>//更新数组函数int main(int argc,char ** argv){ int w=640; int h=480; float Aspect=w/h; pangolin::CreateWindowAndBind("mult",w,h); glEnable(GL_DEPTH_TEST); glEnable(GL_BLEND); pangolin::OpenGlRenderState cam(
2020-09-19 21:49:20
903
原创 pangolin库使用二（点，划线，画图,按钮）
创建一个相机跟界面相机内参意义#include <pangolin/pangolin.h>#include <unistd.h>using namespace std;int main(int argc,char **argv){ pangolin::CreateWindowAndBind("Main",640,480); //创建一个窗口，宽：640，高：480 glEnable(GL_DEPTH_TEST); //用来开启更新深度缓冲区的功能，也就是
2020-09-18 22:09:48
1780
原创 捷联惯导系统学习4.1(惯导数值更新算法)
1 常用坐标系的定义（1）地心惯性坐标系(i 系,inertial frame)用oixiyizio_ix_iy_iz_ioixiyizi表示，原点以地球为中心，原点oio_ioi在地球中心oixio_ix_ioixi在地球平面内，指向春分点oizio_iz_ioizi为地球自转轴，指向北极oiyio_iy_ioiyi轴在地球平面内，垂直于oixio_ix_ioixi和oizio_iz_ioizi（2）地球坐标系(e 系,earth frame)（地心固坐标系）
2020-09-17 18:05:03
1106
1
转载 pangolin库使用一 （安装配置）
Eigen容器#include <Eigen/StdVector>错误vector<Eigen::Matrix4d>std::map<int, Eigen::Vector4f>//虽然会编译通过但是运行时会报错应当使用以下方式std::vector<Eigen::Matrix4d,Eigen::aligned_allocator<Eigen::Matrix4d> >std::map<int, Eigen::Vecto
2020-09-16 20:05:53
2094
1
原创 捷联惯导系统学习3.3(重力位函数和重力计算)
重力位函数（1）定义引力位函数为：www为地球自转速度Q=w22(x2+y2)=w22r2sin2θQ=\frac{w^2}{2}(x^2+y^2)=\frac{w^2}{2}r^2sin^2\thetaQ=2w2(x2+y2)=2w2r2sin2θ引力位函数的梯度函数：gradQ=[∂Q∂x∂Q∂y∂Q∂z]gradQ=\left[\begin{matrix}\frac{\partial Q}{\partial x}&\frac{\partial Q}{\partial y}&
2020-09-16 11:14:24
1091
原创 捷联惯导系统学习3.3(引力位函数)
参数说明：M:地球总质量M:地球总质量M:地球总质量dm:地球上的质量微元dm:地球上的质量微元dm:地球上的质量微元m′:地球外部空间上的一个质点m':地球外部空间上的一个质点m′:地球外部空间上的一个质点p:m与dm之间的距离p:m与dm之间的距离p:m与dm之间的距离万有引力大小为：f=Gdm∗m′p2f=\frac{Gdm*m'}{p^2}f=p2Gdm∗m′引力位函数若外有引力做工(从无穷远移动到半径p处)为:A=∫∞p−Gdm∗mp2=Gdm∗m′p若外有引力做工(从无穷远移.
2020-09-15 15:50:52
1759
原创 Eigen （reshape，别名问题）
通过赋值重塑大小1 直接赋值 MatrixXf M1(3,3); M1<<1,2,3,4,5,6,7,8,9; cout<<"M1:"<<endl<<M1<<endl; Map<RowVectorXf> v1(M1.data(),M1.size()); cout<<"v1:"<<endl<<v1<<endl;2 指定间隔和长度使用InnerStride Ro
2020-09-14 20:09:33
225
原创 捷联惯导系统学习3.3(球谐函数的基本概念)
拉普拉斯方程如果三元函数u(x,y,z)u(x,y,z)u(x,y,z)在空间Ω\OmegaΩ的偏微分满足如下条件:则u为球谐函数/调和函数∇u=∂2u∂x2+∂2u∂y2+∂2u∂z2=0(拉普拉斯方程)\nabla u=\frac{\partial^2 u}{\partial x^2}+\frac{\partial^2 u}{\partial y^2}+\frac{\partial^2 u}{\partial z^2}=0(拉普拉斯方程)∇u=∂x2∂2u+∂y2∂2u+∂z2∂2u=0(拉.
2020-09-11 16:50:32
1043
原创 Eigen 块操作
官方链接#include <iostream>#include <cmath>访问矩阵数据块 //访问矩阵数据块 Eigen::MatrixXf mat(4,4); mat<<1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16; cout<<"middle block"<<endl; cout<<mat.block<2,2>(0,1)<<endl; //可移动
2020-09-10 21:24:07
1050
原创 Eigen 几何变换
头文件#include <Eigen/Geometry>平移T.pretranslate(Vector3d(1, 3, 4)); // 把平移向量设成(1,3,4)缩放Scaling(sx, sy)//2维 x轴sx缩放,y轴sy缩放Scaling(sx, sy, sz)//3维 x轴sx缩放,y轴sy缩放,z轴sz缩放Scaling(s)//全部安s等比缩放旋转矢量、欧拉角、旋转矩阵之间转化 Matrix3f m;m =
2020-09-10 17:33:42
730
原创 Eigen 基本类型的简单使用
初始化 Matrix<float,2,3> mat_23; //数据类型float，2行，3列 Vector3d v_3d;Matrix<float, 3, 1> vd_3d; // v_3d,vd_3d是一样的 Matrix3d matrix_33 = Matrix3d::Zero(); //初始化为零 // Matrix3d 实质上是 Eigen::Matrix<double, 3, 3>Matrix<floa
2020-09-09 16:29:19
1005
原创 捷联惯导系统学习3.1(地球形状描述)
地球的三种模型：（1）近似为圆，半径R=6371（2）近似为旋转椭球体，椭圆度约为1300\frac{1}{300}3001长短半轴相差21km（常用）（3）近似成三轴椭球体，赤道为椭圆，长短轴相差60m，椭圆度约为1100000\frac{1}{100000}1000001...
2020-09-09 10:02:08
1585
原创 捷联惯导系统学习2.7(多项式条件下的等效旋转矢量精确求解)
在使用角增量多样子式求解等效旋转时，原理是使用Bortz方程2阶近似性得到的（忽略高阶项），导致原理上不可避免的会差生误差，使用Bortz方程给出基于多项时迭代的等效旋转矢量，可以避免原理性近似误差。角运动的多项式描述：假设：角速度w(t)w(t)w(t)w(t)=W[tN−1tN−2...1]w(t)=W\left[\begin{matrix} t^{N-1}\\t^{N-2}\\...\\1\\\end{matrix}\right]w(t)=W⎣⎢⎢⎡tN−1tN−2...1⎦⎥⎥⎤
2020-09-07 17:09:00
315
原创 捷联惯导系统学习2.6(圆锥误差补偿多子样算法)
若圆锥运动的四元数更新方程为：Q(tm)=Q(Tm−1)。Q(T)Q(t_m)=Q(T_{m-1})。Q(T)Q(tm)=Q(Tm−1)。Q(T)（ 。。。四元数乘法）（ Q(T)Q(T)Q(T)四元数周期内变化量）得到：（Ω\OmegaΩ震动频率;θ=∣ϕ∣\theta=|\phi|θ=∣ϕ∣）Q(T)=Q∗(tm−1)Q(Tm)=MQ∗(tm−1)Q(tm)Q(T)=Q^*(t_{m-1})Q(T_{m})=M_{Q^*(t_{m-1})}Q(t_m)Q(T)=Q∗(tm−1)Q(Tm
2020-09-07 11:54:13
1691
原创 捷联惯导系统学习2.6(圆锥运动的4种表达方式)
机械陀螺仪的基本特性（章动）：当陀螺仪收到瞬时冲击力矩后，自转轴在原位置附近做微小的圆锥运动，期转子轴的大小方向不变。圆锥运动(震动环境中产生 )：刚体在受到环境影响或者本身具有角速度时，在两个正交轴方向存在频率相同但相角不同的震动速率，会造成刚体围绕第三个正交轴做近似圆锥运动参数说明：www相对角速度;Ω\OmegaΩ震动频率;a,b,ca,b,ca,b,c均为常数在进行圆锥运动下的陀螺仪角速度为w(t)=[aΩsin(Ωt)bΩcos(Ωt)c]w(t)=\left[\begin{matri
2020-09-04 14:19:36
1995
原创 捷联惯导系统学习2.5（等效旋转矢量微分方程的泰勒级数解）
在高精度的捷联惯导系统中，陀螺仪姿态的解算往往是通过采集一定时间内的角增量信息，计算角增量信息计算出等效旋转矢量，在通过等效旋转矢量递推余弦阵或者四元数，完成姿态更新。等效旋转矢量微分方程的泰勒级数解利用泰勒级，计算出数角增量的等效旋转矢量。二字样法：假设：0-T时间（4元数更新时间段）内陀螺仪为线性输出即：w(t)=a+2bt(t∈[0,T])w(t)=a+2bt( t\in[0,T])w(t)=a+2bt(t∈[0,T])Δθ(t)=∫0tw(t′)dt′=at+bt2\Delta \th
2020-09-03 10:56:22
702
原创 坐标系转动的4种数学表示方式
(1)(等效旋转矢量)ϕ=w+12ϕ×w+1θ2(1−θ2cotθ2)(ϕ)2w\phi=w+\frac{1}{2}\phi\times w+\frac{1}{\theta^2}(1-\frac{\theta}{2}cot\frac{\theta}{2})(\phi)^2wϕ=w+21ϕ×w+θ21(1−2θcot2θ)(ϕ)2w(2)(4元数)q=cosθ2+usinθ2q=cos\frac{\theta}{2}+usin\frac{\theta}{2}q=cos2θ+usin2θ(3).
2020-09-02 20:58:40
893
原创 捷联惯导系统学习2.5（等效旋转矢量微分方程）
已知三维旋转矢量关系如下：（证明略）参数说明：ViV_iVi表示三维空间矢量v=∣V∣=VVTv=|V|=\sqrt{VV^T}v=∣V∣=VVT表示矢量模值uuu为与V同方向的单位矢量即：∣u∣=1|u|=1∣u∣=1(1)Vv˙=V×V×V˙v+vV˙V\dot v=\frac{V\times V\times \dot V}{v}+v\dot VVv˙=vV×V×V˙+vV˙(2)u˙=−V×V×V˙v3\dot u=-\frac{V\times V\times \dot V}{v^3
2020-09-02 20:11:36
899
原创 捷联惯导系统学习2.4（4元数微分方程求解）
QQQ表示四元组已知：(1) Q˙(t)=12Mw(t)Q(t)\dot Q(t)=\frac{1}{2}M_{w(t)}Q(t)Q˙(t)=21Mw(t)Q(t)(2) w(t)w(t)w(t)角速度在[0 T]时间内满足定轴转动即： [w(t1)×][w(t2)×]=[w(t2)×][w(t1)×][w(t1)\times] [w(t_2)\times]=[w(t_2)\times] [w(t1)\times][w(t1)×][w(t2)×]=[w(t2)×][w(t1)×]Mw(t
2020-09-01 20:28:55
269
空空如也
空空如也
TA创建的收藏夹 TA关注的收藏夹
TA关注的人