制导、导航系统算法讨论专题

fkepdcjgd

粒子滤波编辑本段粒子滤波（PF：Particle Filter）
　　与卡尔曼滤波（Kalman Filter）相比较 　　粒子滤波(PF: Particle Filter)的思想基于蒙特卡洛方法(Monte Carlo methods)，它是利用粒子集来表示概率，可以用在任何形式的状态空间模型上。其核心思想是通过从后验概率中抽取的随机状态粒子来表达其分布，是一种顺序重要性采样法(Sequential Importance Sampling)。简单来说，粒子滤波法是指通过寻找一组在状态空间传播的随机样本对概率密度函数 进行近似，以样本均值代替积分运算，从而获得状态最小方差分布的过程。这里的样本即指粒子,当样本数量N→∝时可以逼近任何形式的概率密度分布。 　　尽管算法中的概率分布只是真实分布的一种近似，但由于非参数化的特点，它摆脱了解决非线性滤波问题时随机量必须满足高斯分布的制约，能表达比高斯模型更广泛的分布，也对变量参数的非线性特性有更强的建模能力。因此，粒子滤波能够比较精确地表达基于观测量和控制量的后验概率分布，可以用于解决SLAM问题。 　　粒子滤波的应用 　　粒子滤波技术在非线性、非高斯系统表现出来的优越性，决定了它的应用范围非常广泛。另外，粒子滤波器的多模态处理能力，也是它应用广泛的原因之一。国际上，粒子滤波已被应用于各个领域。在经济学领域，它被应用在经济数据预测；在军事领域已经被应用于雷达跟踪空中飞行物，空对空、空对地的被动式跟踪；在交通管制领域它被应用在对车或人视频监控；它还用于机器人的全局定位。 　　粒子滤波的缺点 　　虽然粒子滤波算法可以作为解决SLAM问题的有效手段，但是该算法仍然存在着一些问题。其中最主要的问题是需要用大量的样本数量才能很好地近似系统的后验概率密度。机器人面临的环境越复杂，描述后验概率分布所需要的样本数量就越多，算法的复杂度就越高。因此，能够有效地减少样本数量的自适应采样策略是该算法的重点。另外，重采样阶段会造成样本有效性和多样性的损失，导致样本贫化现象。如何保持粒子的有效性和多样性，克服样本贫化，也是该算法研究重点。 　　粒子滤波的发展 　　1.MCMC改进策略 　　马尔可夫链蒙特卡洛(MCMC)方法通过构造Markov链，产生来自目标分布的样本，并且具有很好的收敛性。在SIS的每次迭代中，结合MCMC使粒子能够移动到不同地方，从而可以避免退化现象，而且Markov链能将粒子推向更接近状态概率密度函数(probability density function,(PDF))的地方，使样本分布更合理。基于MCMC改进策略的方法有许多，常用的有Gibbs采样器和MetropolisHasting方法。 　　2.Unscented粒子滤波器(UPF) 　　Unscented Kalman滤波器(UKF)是Julier等人提出的。EKF(Extended Kalman Filter)使用一阶Taylor展开式逼近非线性项，用高斯分布近似状态分布。UKF类似于EKF，用高斯分布逼近状态分布，但不需要线性化只使用少数几个称为Sigma点的样本。这些点通过非线性模型后，所得均值和方差能够精确到非线性项Taylor展开式的二阶项，从而对非线性滤波精度更高。Merwe等人提出使用UKF产生PF的重要性分布，称为Unscented粒子滤波器(UPF)，由UKF产生的重要性分布与真实状态PDF的支集重叠部分更大，估计精度更高。 　　3.Rao－Blackwellised粒子滤波器(RBPF) 　　在高维状态空间中采样时,PF的效率很低。对某些状态空间模型，状态向量的一部分在其余部分的条件下的后验分布可以用解析方法求得，例如某些状态是条件线性高斯模型，可用Kalman滤波器得到条件后验分布，对另外部分状态用PF，从而得到一种混合滤波器，降低了PF采样空间的维数，RBPF样本的重要性权的方差远远低于SIR方法的权的方差，为使用粒子滤波器解决 SLAM问题提供了理论基础。而Montemerlo等人在2002年首次将Rao-Blackwellised粒子滤波器应用到机器人SLAM中，并取名为FastSLAM算法。该算法将SLAM问题分解成机器人定位问题和基于位姿估计的环境特征位置估计问题，用粒子滤波算法做整个路径的位姿估计，用EKF估计环境特征的位置，每一个EKF对应一个环境特征。该方法融合EKF和概率方法的优点，既降低了计算的复杂度，又具有较好的鲁棒性。 　　最近几年，粒子方法又出现了一些新的发展，一些领域用传统的分析方法解决不了的问题，现在可以借助基于粒子仿真的方法来解决。在动态系统的模型选择、故障检测、诊断方面，出现了基于粒子的假设检验、粒子多模型、粒子似然度比检测等方法。在参数估计方面，通常把静止的参数作为扩展的状态向量的一部分，但是由于参数是静态的，粒子会很快退化成一个样本，为避免退化，常用的方法有给静态参数人为增加动态噪声以及Kernel平滑方法，而Doucet等提出的点估计方法避免对参数直接采样，在粒子框架下使用最大似然估计(ML)以及期望值最大(EM)算法直接估计未知参数。

fkepdcjgd

我正在向算法进军，在行进中有太多的辛酸，用一个字形容---苦。可它并不可怕，都是人搞出来的，我想我也行。。。
现在正在搞卡尔曼算法和捷联惯导，愿有兴趣的朋友加入，更愿有高手指点指点。。。

fkepdcjgd

经过一段时间的学习，对制导、导航算法以及信息融合有了一定的认识，可还有太多的不解，请高人出来指点指点。。。

fkepdcjgd

由加速度传感器和角速率传感器测得机体坐标相对于惯性空间的比力和角度后，再把比力按角度关系分解到导航坐标系中，从而得出速度、位置和姿态角；要得出这些正确的信息，还得处理好初对准，这里常用扩展卡尔曼算法，但它要求模型线性高斯条件，精度也不高，现在多用粒子滤波；可这些都是些让人难以明白的数学公式，要把它搞明白，再写成C语言代码，还得继续痛苦。。。。
  


     坚信只有在痛苦中，才能获得真知！

fkepdcjgd

技术的道路还真够长的。。。

kawayi213

顶一下，捷联惯导听说过……不知研究的怎么样了

admvip

鼓励一下楼主，我到现在还什么都不懂呢，也想学点。

fkepdcjgd

还在正在进行卡尔曼算法处理中。。。

fkepdcjgd

有请高人发点扩展卡尔曼算法的源代码上来参考一下。。。

fkepdcjgd

我上传一些常用算法的资料上来，只想有人更多的人参与这块。。。
点击此处下载 ourdev_612797E5ONA8.pdf(文件大小:301K) (原文件名:捷联式惯性导航系统算法研究.pdf)

点击此处下载 ourdev_612798YCAF5Y.pdf(文件大小:273K) (原文件名:AT89C51基于PID算法的锅炉温度控制系统的实现.pdf)

点击此处下载 ourdev_612799HBNKAN.doc(文件大小:59K) (原文件名:卡尔曼滤波器轻松入门.doc)


点击此处下载 ourdev_612800VGDKFI.doc(文件大小:78K) (原文件名:遗传算法的经典解析.doc)

NUAA_hp

都是成熟的算法，网上一大堆资料，看用在什么地方~~~

litao8421

一般的EKF就足够了，其他的partical filter, unscented filter, genetic algrithm 都是为了写论文而研究出来，如果是工程应用，不建议看了，了解干什么的就行了。毕竟在导航领域，非线性滤波用处不大，大部分都可以线性化。

即使是为了写论文，这样的论文也已经一大把了，肯定发不出好论文。不如以实际问题入手，找出一些解决问题的方法。这样的论文才更有意义。

个人见解。欢迎拍砖。

fkepdcjgd

谢谢楼上！用在无人机方面，比如说航拍。。。

fkepdcjgd

现在，我觉得在状态方程和观测方程的建立上，还有就是对泰勒级数得得到雅可比矩阵时，还没有怎么搞明白，楼上可能是老手了，新手还是觉得有些乱的。。。

fkepdcjgd

http://search.codesoso.com/default.aspx　　有很多的源码。。。

sarcophile

顶下楼主，可怜的楼主，前面一大堆都是楼主自己。。。。。
我也想玩点有技术含量的东西，可是真正能深入的人太少了，都很难有人探讨一下。。。我和楼主玩的又不是一个方向。我学自控的，想玩个平衡仪，应用自控理论来控制直升机的姿态稳定。和楼主一样也觉得建模是个麻烦，可是咱俩用的理论都不一样。。。

稍微可以给楼主参考一下的是，正在看一个老美写的自控原理，比国内的容易懂，而且更贴近实际

zhiweihappy

顶一下楼主，佩服楼主的学习精神，虽然我是个菜鸟，但是心里默默支持你啦！

fkepdcjgd

回复【15楼】sarcophile
-----------------------------------------------------------------------

回顶！都差不多，我也得从你搞的地方开始，其实我们是一样的；平衡与导航及自驾仪差不多是一家的。。。
国外的书整体都要比国内的书好懂些，这与写书人的观念有关；我觉得国内的人写书不是让人把它的理论变为实际的为人类某福的成果，而是想让自己出名、想让人知道他懂个什么东西、同时他还想让人走他已走过的路，以便体验他研究时的痛苦过程，这是中国学者的自私的表现。。。
　　前几天，我看了个报道，我国的科研学术论文发表量世界第一，可引用量排在世界一百多位；我就不信是中国人为了显示自己外文的能力才专门探讨国外学术刊文的。。。

fkepdcjgd

如果说我的观念是错的，那么在现行模式中，还应再出现一部份人，就是名人的理论工程化，要不然，工程应用就远远小于学术上的科研成果，如此，再多的学术理论多是摆设。。。

fkepdcjgd

#define uchar unsigned char  
#define   uint    unsigned int  
#define   m_Q     0.01f//state  
#define   m_R     0.01f//watch  
//###################################################//  
//把测量的量直接作为估计量，所以Ｈ为“１”，
//同时认为状态转移也是“1”，就像对待测温一样，认为下一时刻与这时刻恒定，（这样的假设可以？）
//当Ｈ和Ｑ都为1时，直接把表达式简化了，还有，就是只作单变量估计。。。
unsigned char sdKF(OptimalOutput,watch_Z)  
unsigned int  *OptimalOutput,watch_Z;  
{  
static float Optimal_X=1.0;// 状态量
static float m_P=10.0;//作为一步预测误差方差和估计误差方差  
static float m_K; //滤波增益
m_P=m_P+m_Q;//一步预测误差方差计算  
m_K=m_P/(m_P+m_R);//增益计算  
m_P=m_P*m_R; //估计误差方差
Optimal_X=Optimal_X+m_K*(watch_Z-Optimal_X); //最优估计计算
//watch_Z是观察量，Optimal_X是状态量，Optimal_X=Ｈ*Ｑ*Optimal_X＝Optimal_X
*OptimalOutput=(unsigned int)Optimal_X; //结果输出
return;
}

fkepdcjgd

呵呵

fkepdcjgd

用加速度传感器来测资态，还让它与陀螺仪相互修正的可行性不知如何？
　　我觉得，只有在飞行器静止时，才可以，而我们的飞行器，要的是在动态中同样可用，如上的设计可以在动态中实用吗？

robertwang

加速度计测姿态，并与陀螺仪修正是可行的，关键是要什么样的精度。一般的航姿系统有的就是用传感器的输出进行测姿，而且可以补偿掉载体的运动加速度。

sarcophile

我的方案就是加速度加陀螺仪，三轴加速度测三维运动，三轴陀螺测三个轴上的转动，我觉得理论上是互相独立的，不存在互相修正吧？为什么动态不行呢？我没明白。可行与否要等东西出来再说，我也只是觉得可以。
我玩这个是业余爱好，最近工作很忙，就没有时间弄他，也不知什么时候才能闲一点
关于国内的“著作”，唉，我应该算比你看得开一点吧，人家写书的自己也不咋明白，又要混饭吃，因为国内评职称发奖金都看论文数量，怎么办呢，只好能混就混，理解一下吧，一个刚开始有一点发展的国家，底子那么弱，风气又浮躁，你还想指望什么呢？
我倒是相信类似胡寿松（自控领域的国内一权威）这样的人自己是很明白这些理论的，从最大的善意出发，我相信他也不是想让人重复他的痛苦，但他追求的是理论的严谨和完整，却也不是易学易懂。唉，看老外的教材，真爽

liguole

我为什么 不能下载 你提供的资料呢 奇怪

liguole

哈哈 下载成功啦
我最近也在学习 测量飞行器姿态 的知识。起初只是觉得 对角速度积分就行啦（之前不明白加计的作用，似乎现在也不是很明白，哈哈） 但是经实验证明似乎可能性不大（很短时间内就漂飞啦积分出来的角度根本不能用），后来无意中发现加计输出的电压是和x、y轴的倾角有某种关系的，于是做了实验 效果不错（虽然精度差点），现在也考虑到运动状态下可能会对角度测量有影响及目前量程过小的问题，做如下的改进，我的设想是：
一.我用的是mma7260目前只能测正负90度（大于90度如果继续旋转就会减小角度），但考虑到飞机可能处于倒飞状态 所以想用角速度传感器（陀螺）判断滚转方向 来决定量程外的角度是应该继续增加还是减小，我不知道这样说大家是否能明白。

二.由加计得到的角度值是否可以利用陀螺的角速度输出来判断滚转角和俯仰角的变化是由运动加速度的变化引起的，还是由于俯仰滚转本身倾转引起的呢 ，设想飞行器在飞行时必然存在加速度的变化，固然会导致角度变化（前提：用加计测角度时），但此时检测陀螺角速度输出，如果角速度在一定范围内（飞行器飞行时自身固有波动就有几hz），应忽略由加计得到的角度变化（消除误差），但是似乎又有点矛盾
1.这个角速度范围定在多少合适。
2.如果超出范围，怎么取这个角度值。这两点我还没有想好。

这些只是个人的业余爱好，我没有学历，更看不懂卡尔曼滤波原理，只是出于兴趣爱好，所以言语之中难免有些错误，希望大家能谅解并指出，希望和大家探讨学习。

我的本职工作是无人机飞控手，负责飞机的起飞及降落，实践证明精度要求并不高，毕竟不是导_弹又不是长航时飞行器，由易到难吗？精度逐渐提高，自从弄这个东西，我的单片机知识又上了一个台阶。挺好玩！

jmp2002911

关于用加速度计修正陀螺仪，大家可以去四轴分论坛看看，那里的这方面的工作已经比较完善了

summerstar

支持楼主！加油！

xiaojuren

回复【8楼】fkepdcjgd 雾都溪风
-----------------------------------------------------------------------

好东西，谢谢了

xiaojuren

回复【21楼】fkepdcjgd 雾都溪风
-----------------------------------------------------------------------

我也想用加速度传感器和陀螺仪来控制飞机姿态，但现在还没有那么深的功力，只好慢慢来了

fkepdcjgd

NUAA_hp 发表于 2011-1-22 11:31
都是成熟的算法，网上一大堆资料，看用在什么地方~~~

你可能是真的通了九壳的人吧？

fkepdcjgd

litao8421 发表于 2011-1-22 12:02
一般的EKF就足够了，其他的partical filter, unscented filter, genetic algrithm 都是为了写论文而研究出 ...

嗯，我细细的听着你的意见！  你说的不错，不过我是做飞控上用，就不像你所说的那么一回事了，在现在航模飞控里，主要是用来作平横仪用，就是保持资态持久稳定，其实这并不是件易事，我们用的都是很差的陀螺，再加上在航模机架上非常剧烈的振动，还真不好办。不过你说EKF就够了，也不无道理，现在我们用的效果最好的，就是用的EKF，不过，对于EKF的数学建模不是一般人完成的了的。

hzfastzyj

看上去真的好复杂。

木君之上

不管是几轴，算法才是精华啊，算法才是核心啊，等我代码能跑了之后就潜下心学算法了

tlsmf

学习学习。。。

fkepdcjgd

q0q0 + q1q1 + q2q2 + q3q3 = 1;


  hx = 2*mx*(0.5f - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);
  hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5f - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);
  hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5f - q1q1 - q2q2);   
  
  bx = sqrt((hx*hx) + (hy*hy));
  bz = hz;     
  
  // estimated direction of gravity(ÖØÁ¦) and flux (v and w)
  vx = 2*(q1q3 - q0q2);
  vy = 2*(q0q1 + q2q3);
  vz = q0q0 - q1q1 - q2q2 + q3q3;
       
  wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);
  wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);
  wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);  
  
  // error is sum of cross product between reference direction of fields and direction measured by sensors
  ex = (ay*vz - az*vy) + (my*wz - mz*wy);
  ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
  ez = (ax*vy - ay*vx) + (mx*wy - my*wx);




对于在用磁传感器的数据修正陀螺中点时，有些不太理解。它把机体坐标下的测理值转换到水平面上和坚直面上，再由计算出的这两个分向量解算到机体坐标系，再求与测量值的差，用其差值来修正陀螺中点，这样做，可以么，感觉应产生什预期效果才对，反而会引入截断误差。是这么理解的么？

fkepdcjgd

lianglee 发表于 2016-7-4 16:23
这个好难，理论我都看不明白。

几年前看过，地对空，空对空，空对地。

，愿意去搞移动标跟踪锁定算法的人非常少，你还努力去整，已很牛了，哈哈。如上题，如果这样处理，其科学性到底在哪呢？

fkepdcjgd

求大神指点指点。

fkepdcjgd

它把磁传感器的读数通过捷联矩阵转到平台坐标系后，并求出合磁场在水平面的投影和在坚直z轴两个向量后，再把这两个向量通过捷联矩阵的逆矩阵求出固连在机体坐标上的磁传感器的读数，最后再与实际传感器的读数差来修正陀螺仪，这样的思路的科学性在哪里呢？

fkepdcjgd

  // compute reference direction of flux
  hx = 2*mx*(0.5f - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);
  hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5f - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);
  hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5f - q1q1 - q2q2);


  bx = sqrt((hx*hx) + (hy*hy));
  bz = hz;     

  wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);
  wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);
  wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);  
它把磁传感器的读数通过捷联矩阵转到平台坐标系后，并求出合磁场在水平面的投影和在坚直z轴两个向量后，再把这两个向量通过捷联矩阵的逆矩阵求出固连在机体坐标上的磁传感器的读数，最后再与实际传感器的读数差来修正陀螺仪，这样的思路的科学性在哪里呢？
个人觉得，它这样是不是起不到修正作用，反而引入了截断误差呢
hx,hy,hz是在水平坐标系下的值。
bx，bz是原合磁场知量在水平面和坚直平面的投影。
然后又用这两个向量通过逆矩阵求机体坐标下的值，而这做觉得本身就会引入较大误差的。
它然后是想用磁传感器的读数与wx,wy,wz的叉积来修正陀螺的零偏。

windy_mia

顶，我再用陀螺仪，不过是简单的用。只读取角加速度，根据角加速度做处理。

laoshuhunya

技术其实最后拼的就是数学
  

dory_m

学习，谢谢！！！

dgdzas

楼主还在坚持吗

windy_mia

laoshuhunya 发表于 2017-5-9 11:40
技术其实最后拼的就是数学

是的，最后是数学。