随笔分类 - Matlab练习程序
摘要：Sinkhorn算法是为了解决最优传输问题，该问题是给定两个概率分布u和v，找到一个方法，使其从u转换到v的代价最小。 具体到这里是找到了一个转移矩阵。 算法步骤如下： 1. 给定两个概率分布u和v，其中u和v是归一化后数据，维度分别为m和n。 2. 给定矩阵K(m,n)，K=1.0/(m*n)。
阅读全文
摘要：LQR 是一种优化控制方法，设计目标是找到一组控制输入，使得线性系统的状态轨迹尽可能地接近目标，同时使控制输入尽可能小。其目标函数是一个二次型成本函数。 分为以下几个步骤： 1. 设系统动态方程为： 其中x为状态量，u为控制输入，A和B为状态转移和控制矩阵。 2. 定义一个性能指标，即控制器的优化目
阅读全文
摘要：之前有通过ode和simulink解线性常微分方程组。 除了上面两种方法，线性常微分方程组还可以通过矩阵的方法求解。 比如下面这个之前使用的方程组： x'' = x' - x + y' -z' y'' = y' - y - x' z'' = z' - z + x' 可以写成下面矩阵形式： 设这个矩阵
阅读全文
摘要：当时写stanley就实现了，贴上来记录一下。 方法示意图： 控制率公式： 其中L为轴距，e为横向误差，v为车辆速度，lambda和c为控制参数。 算法步骤如下： 1. 根据当前定位结果找到路径最邻近点。 2. 计算该点与定位结果横向误差e。 3. 根据控制率公式计算出前轮转角。 4. 将前轮转角转
阅读全文
摘要：Schur 补是一种矩阵分解方法，通过将一个大的矩阵分解为几个较小的矩阵来简化计算，通常能够提高矩阵求逆的速度。 对于形如下面的矩阵： 可以把矩阵划分为左上、右上、左下、右下四个分块矩阵。 得到矩阵： 根据A和D的奇异性，可以分两种情况。 如果A可逆，则有： 如果D可逆，则有： 公式两边同时求逆，得
阅读全文
摘要：BA作为视觉SLAM中后端优化的一个核心点还是比较重要的。 BA根据优化量的不同可以分为三种形式。 Full BA：观测点和位姿同时优化，一般是视觉SLAM后端的核心。 Motion BA：已知观测点，优化位姿，一般用来定位。之前的文章中有用到BA单做位姿计算的方法。 Struct BA：已知位姿，
阅读全文
摘要：正交分解可以将多个向量分解为互相正交的多个向量。 可以用QR分解方法或施密特正交化方法，施密特正交化方法一般数值不稳定。 假设有{V1...Vn}向量组，施密特正交化算法原理如下： 结果中{β1...βn}为一组正交基，{η1...ηn}为其标准正交基。 matlab代码如下： clear all;
阅读全文
摘要：通过3D-2D匹配点计算位姿，除了用上篇的DLT来求解，还可以用非线性优化方式求解。 这篇就用BA的方法求解PnP问题。 使用非线性优化通常要先确定下面四个要素： 1. 待优化模型，模型和上一篇是一样的，三维点通过旋转平移矩阵变换到像空间，然后通过内参投影到二维像平面上，可以用下面几个方程表示： 其
阅读全文
摘要：在计算位姿的时候，一般我们有一些观测量，这些观测量有些是三维的、有些是二维的，因此需要用到不同的方法。 如果是3D-3D的位姿计算，一般可以用这几种方法（【1】,【2】,【3】,【4】）。 如果是3D-2D的位姿计算，一般可以用PnP-BA或者是本篇的DLT（直接线性变换）方法。 如果是2D-2D的
阅读全文
摘要：对于两组点集，要计算其旋转平移矩阵，可以用点云配准算法。 也可以用非线性优化的方法计算，不过由于待优化量包含旋转量，做迭代求雅克比矩阵时如果用欧拉角表示旋转矩阵会比较麻烦。 因此这里用李群李代数的方法求解。 李群与李代数互转公式见下图： 通常用三维变换SE(3)多一些，三维空间中一般都是包含旋转和平
阅读全文
摘要：比如我们拿到了一组imu的原始加速度和角速度数据。 通过卡尔曼或互补滤波得到了三个旋转角，想要可视化一下。 可以用下面这种方法，转成旋转矩阵后简单组合一下即可。 matlab代码如下： clear all;close all;clc; ang = load('ang.txt'); roll = an
阅读全文
摘要：当数据通过非线性函数后，分布不再是高斯分布时，可以用无迹变换估计新数据的均值与方差。 算法原理就是在原始数据均值周围根据方差选取一些待使用点，然后将这些点通过非线性函数，再通过加权平均的方式求出新分布的均值与方差。 如果我们选取的点非常多，并且将这些点都通过非线性函数，再估计均值与方差，那么就是粒子
阅读全文
摘要：要用傅里叶级数表示二维图形，首先要找到数学表达式，然后做傅里叶拟合即可。 我最初想的是$R= f(theta)$这样的式子，$R$是极径，$theta$是极角。 不过这样似乎处理不了$theta$一样的情况，比如图形有凹陷的情况。 后来看了一些文章说可以把$x$和$y$分开表示，即$x=f(t)$,
阅读全文
摘要：Frechet距离用于描述路径的相似性。 可以用一个二维矩阵$Ca$来表示。 设$d(i,j)$为A,B路径第$i$和第$j$个点的欧式距离。 首先计算A路径第一个点到B路径第一个点的欧式距离，设为$Ca$矩阵的第一个元素。 然后再计算$Ca$矩阵的第一行和第一列： $Ca(1,j)=Max(Ca(
阅读全文
摘要：回旋曲线能够比较好的表示驾驶员匀速转动方向盘从直行道进入转弯道的路径。 公式如下： 其中$a=1/(RL)$，$L$是曲线长度，$R$是曲线半径，$R$越大，曲线越平缓。 下面生成半径从0.2米到2米，长度从1米到3米的一系列回旋曲线。 matlab代码如下： clear all;close all
阅读全文
摘要：球谐函数是拉普拉斯方程的球坐标系形式解的角度部分，在古典场论、量子力学等领域广泛应用，在计算机图形学里面可以模拟环境光照。 球谐函数$Y_{l}^{m}$可以表示为： $Y_{l}^{m}(\theta ,\varphi )=(-1)^{m}\sqrt{\frac{(2l+1))}{4\pi }\f
阅读全文
摘要：拟合优度可以确定回归曲线对原始数据的拟合程度，用$R^{2}$表示，最大值为1，值越大拟合程度越好。 公式如下： 其中： 回归平方和SSR(regression sum of squares)公式： 总体平方和SST(total sum of squares)公式： 残差平方和SSE(error s
阅读全文
摘要：泽尼克多项式是一个正交多项式，分为奇偶两类。 奇多项式： 偶多项式： 其中： 这里fai为方位角，范围[0-2pi]；p为径向距离，范围[0,1]；n-m大于等于0； 如果n-m=0，则R=0。 根据不同的m和n值，可以得到不同的多项式，用j表示不同的多项式，通常称为Noll序列： n,m 0,0
阅读全文
摘要：最近在看三体电视剧，正好看到了计算三体数值解那一部分，就想起了上学时看三体，也用matlab实现了三体的运动模拟。 不过当时是通过时域外推的方式实现的，不是很严谨。 下面通过微分方程求解三体问题，三体模型的微分方程： 公式中x1是质点1的位置向量(px,py,pz)，x1''是质点1的加速度向量(a
阅读全文
摘要：门限回归和门限自回归是对基本的回归和自回归加上一个阈值判断。 下面简单的表述一下公式。 回归模型可以用下述公式表示： 则门限回归可以表示为： 自回归模型可以用下述公式表示： 则门限自回归可以表示为： 计算参数的方法和之前ARMA中类似，根据输入输出和模型通过最优化方法计算。 门限回归代码如下： cl
阅读全文