飞控姿态解算理解

姿态解算理解

1、姿态的描述方法

前几天在论坛里偶尔看到一个帖子，帖子的内容是问的为什么不用倾斜角表示姿态，我认为他说的倾斜角是指的斜面与斜面的夹角，或者说是物体与垂线的夹角吧，这种想法可能来源于我们日常生活的思维。

图1立方体

比如有一个立方体，我们放在水平面上的时候它的底面是和水平面平行的，但是当我们把立方体的一个脚垫起一个角度时，这样一来，立方体的一条棱与水平面的垂线就有了一定的夹角了。我们所说倾斜了多少多少度就是指的这个夹角，这是我们直观的反应。我认为这样直观的反应甚至比欧拉角还要来的直观，因为欧拉角是基于旋转的，肯定不会说这个立方体X、Y轴各旋转了多少度（假设Z旋转无效），我们可能也没那个概念，我们直观的反应就是它倾斜了一定度数。

但是我们在姿态解算的时候为啥不用的这种描述方法呢，个人认为是虽然方便我们直观的表达但不适合数学上的计算，还有就是我们仅仅知道这个倾斜角我们怎么施加控制量呢？高中物理学习物体运动的时候我们知道，物体的运动是合运动，我们可以把它的运动矢量正交分解为几个运动的合成（不正交也是可以的，但那不是在自找麻烦吗），同样道理，我们可以把刚体的旋转分解为三个轴上的旋转，这个旋转的角度就是欧拉角，如图2。

图2zxz序规欧拉角

欧拉角

欧拉角的定义不仅仅和旋转角度有关系，还和旋转轴的旋转顺序有关系，任何一种旋转顺序都是合法的。根据定义，欧拉角有12种旋转顺序（维基），一个物体通过任意一个旋转顺序都可以达到同样的姿态，在各个学科里所以为了统一，航空航天领域规定XYZ为欧拉角的旋转顺序。

上面已经说了欧拉角的定义。欧拉角的定义也是很直观而且容易理解的，也利于我们的计算，因为我们用的惯性器件也是按照单个轴向运动来测量的。定义上的欧拉角还和我们所说的Yaw、Pitch、Roll不是一回事。因为定义上的欧拉角就是刚体绕三个轴的旋转角度，

欧拉旋转和外界的东西（参考系）是没有关系的。Yaw、Pitch、Roll就是载体对于参考系来说的了，这意味着第一次的旋转不会影响第二、三次的转轴，因为选的参考系都是地球了（这也是产生Gimbal Lock的原因，下面会讲到）。

我们现在说的飞机都是在近地表附近飞行，所以我们习惯是拿地球作为参考系，我们的飞机总是在一点起飞在另一点降落。所以我们规定地理方位东、北、天为参考初始点，也就是说，我们的飞机头朝北水平放置时载体坐标系和参考坐标系是重合的，那么接下来我们绕飞机的Z轴旋转30°，这个旋转的欧拉角就是我们所说的Yaw，同样，绕飞机的X轴旋转30°，我们得到Pitch，绕飞机Y轴旋转得到Roll，如图3。

图3

这和规定欧拉角的旋转顺序是一样的，所以说是和欧拉角是一一对应的，要注意欧拉角是基于飞机本身轴旋转得到的，但是得到的姿态却是对于参考坐标系而说的，要不然对我们来说也没有实际的意义，来个更直观的：

欧拉角是有很多优点的。但是也有致命的缺点，那就是Gimbal Lock（万向节死锁），要理解Gimbal Lock所说的情况，这可能有点不好理解，让我们看个现实中的场景。

假如我们有一个望远镜和一个用来放望远镜的三脚架，（我们将）三脚架放在地面上，使支撑望远镜的三脚架的顶部是平行于地平面（参考平面）的，以便使得竖向的旋转轴（记为x轴）是完全地垂直于地平面的。现在，我们就可以将望远镜饶x轴旋转360度，从而观察（以望远镜为中心的）水平包围圈的所有方向。通常将正北朝向方位角度记为0度方位角。第二个坐标轴，即平行于地平面的横向的坐标轴（记为y轴）使得望远镜可以饶着它上下旋

转，通常将地平面朝向的仰角记为0度，这样，望远镜可以向上仰+90度指向天顶，或者向下-90度指向脚底。好了，万事俱备。现在，天空中（包括地面上）的每个点只需要唯一的一对x和y度数就可以确定。比如x=90度,y=45度指向的点是位于正东方向的半天空上。现在，看看万向节死锁是怎么发生的。一次，我们探测到有一个飞行器贴地飞行，位于望远镜的正东方向（x=90度，y=10度），朝着我们直飞过来，我们跟踪它。飞行器飞行方向是保持x轴角度90度不变，而y向的角度在慢慢增大。随着飞行器的临近，y轴角增长的越来越快且当y向的角度达到90度时（即将超越），突然它急转弯朝南飞去。这时，我们发现我们不能将望远镜朝向南方，因为此时y向已经是90度，造成我们失去跟踪目标。这就是万向节死锁！

为什么说不能将望远镜朝向南方呢，让我们看看坐标变化，从开始的（x=90度，y=10度）到（x=90度，y=90度），这个过程没有问题，望远镜慢慢转动跟踪飞行器。当飞行器到达（x=90度，y=90度）后，坐标突然变成（x=180度，y=90度）（因为朝南），x由90突变成180度,所以望远镜需要饶垂直轴向x轴旋转180-90=90度以便追上飞行器,但此时，望远镜已经是平行于x轴，我们知道饶平行于自身的中轴线的的旋转改变不了朝向，就象拧螺丝一样，螺丝头的指向不变。所以望远镜的指向还是天顶。而后由于飞行器飞远，坐标变成（x=180度，y<90度）时，y向角减小，望远镜只能又转回到正东指向，望'器'兴叹。这说明用x,y旋转角（又称欧拉角）来定向物体有时并不能按照你想像的那样工作，象上面的例子中从（x=90度，y=10度）到（x=90度，y=90度），按照欧拉角旋转确实可以正确地定向，但从（x=90度，y=90度）到（x=180度，y=90度），再到（x=180度，y<90度）,按照欧拉角旋转后的定向并非正确。我的理解是坐标值的变化和飞行器空间的位置变化一一对应，但是从（x=90度，y=90度）到（x=180度，y=90度），再到（x=180度，y<90度）这个变化，飞行器位置是连续的变化，但坐标值的变化却不是连续的（从90突变到180），其原因在于（x=90度，y=90度）和（x=180度，y=90度）甚至和（x=任意度，y=90度）这些不同的坐标值对应空间同一个位置，这种多个坐标值对应同一个位置的不一致性是造成死锁的根源。

上面是2维坐标系中的例子，同样，对于3维的也一样。比如有一个平行于x轴的向量，我们先将它饶y旋转直到它平行于z轴，这时，我们会发现任何饶z的旋转都改变不了向量的方向，即万向节死锁，所以说传统的欧拉角是不能做到全姿态解析的。（抄的）方向余弦

一个向量在坐标系中的位置也可以用方向余弦表示，也就是这个向量分别到三个坐标轴的夹角余弦值，实际上就是这个向量到各个坐标轴的投影啦，角度范围是0~π（维基）。所以推广到载体坐标系和参考坐标系当中，我们就有了载体坐标轴xyz分别与参考轴XYZ的方向余弦，这里就是所说的方向余弦矩阵了，它是由两组不同的标准正交基的基底向量之间的方向余弦所形成的3x3矩阵。方向余弦矩阵可以用来表达一组标准正交基与另一组标准正交基之间的关系。余弦矩阵的列表示载体坐标系中的单位矢量在参考坐标系中的投影。分量形式如下:

第i行、j列的元素表示参考坐标系i轴和载体坐标系j轴夹角的余弦。

其实方向余弦和欧拉角没有本质上的区别，因为这是用欧拉角表示的方向余弦。一个坐标系到另一个坐标系的变换，在《捷联惯性导航技术》、和《惯性导航》中都是有介绍的，

特别是《惯性导航》有推导的过程。

推广到三轴的单次旋转，我们用矩阵表示为：

这里要说一下矩阵的含义，C21表示坐标系1到坐标系2的变换矩阵，那么，

这是绕Z轴的基本旋转，我们可以看到X2、Y2投影为0，Z2投影为1，这是为什么呢？自己想想。。。咱们看下面的单位1，这是什么？所以说这就是上面说的坐标系2到坐标系1轴的三个方向余弦！对吧。。。这在大家熟知的imu.c里面就有直接的计算。那么单独旋转各个轴，我们得到：

实际上，两坐标系任何复杂的角位置关系都可以看做有限次基本旋转的组合，变换矩阵等于基本旋转确定的变换矩阵的连乘，要是不知道矩阵和矩阵乘法，那就看线性代数吧，连乘的基本顺序依据基本旋转的顺序向右排列。之所以有顺序是因为矩阵有“左乘”和“右乘”之分。那么我们得到：

按上面推论，此矩阵的下面三个就是三轴对应的方向余弦，γ、θ、ψ就是欧拉角，现在明白了欧拉角和方向余弦矩阵的关系了吧。

四元数

四元数要说的实在太多，因为它的优点很多，利用起来很方便，但是理解起来就有点蹩脚了。我们百度四元数，一开始看到的就是四元数来历，还有就是四元数的基本计算。对于来历，还是想说一下，四元数（Quaternions）是由威廉·卢云·哈密尔顿(William Rowan Hamilton,1805-1865）在1843年爱尔兰发现的数学概念（百度百科）。

将实数域扩充到复数域，并用复数来表示平面向量，用复数的加、乘运算表示平面向量的合成、伸缩和旋，这就是我们熟知的复数的二维空间含义，所以人们会继续猜想，利用三维复数不就可以表达三维空间的变换了吗，历史上有很多数学家试图寻找过三维的复数，但后来证明这样的三维复数是不存在的。有关这个结论的证明，我没有查到更明确的版本，据《古今数学思想》中的一个理由，三维空间中的伸缩旋转变换需要四个变量来决定：两个变量决定轴的方向，一个变量决定旋转角度，一个变量决定伸缩比例。这样，只有三个变量的三维复数无法满足这样的要求。但是历史上得到的应该是比这个更强的结论，即使不考虑空间旋转，只从代数角度来说，三维的复数域作为普通复数域的扩张域是不存在的。并且，据《古今数学思想》叙述，即使像哈密尔顿后来引入四元数那样，牺牲乘法交换律，这样的三维复数也得不到。经过一些年的努力之后，Hamilton发现自己被迫应作两个让步，第一个是他的新数包含四个分量，而第二个是他必须牺牲乘法交换律。（《古今数学思想》第三册177页）但是四元数用作旋转的作用明显，简化了运算，而且避免了Gimbal Lock，四元数是最简单的超复数，我们不能把四元数简单的理解为3D空间的矢量，它是4维空间中的的矢量，也是非常不容易想像的（抄的）。

1、四元数的表示方式：

2、四元数的计算：

关于四元数的基本运算，加减乘除，大家从别的地方看看吧，维基百科https://www.sodocs.net/doc/191389984.html,/wiki/%E5%9B%9B%E5%85%83%E6%95%B0

要注意的是，四元数的元的平方定义为-1，这点等同于复数，四元数不同元相乘得到另外一个，这点有点像向量叉乘。可见，四元数相乘不是虚数相乘也不是向量相乘，这一运算特点是由四元数的性质决定的。四元数由一个实数和三个虚数构成，所以是一个四维空间的向量，但是它的三个虚数又有三维空间的性质，因此，三维空间中的一个矢量，可以看作一个实部为0的四元数，这个四元数是这个三维空间的一个矢量在四维空间里的“映像”，或者叫做这个矢量的“四元数映像”。这样，我们就把三维空间和一个四维空间联系起来，用四维空间中四元数的性质和运算规律来研究三维空间中刚体定点转动的问题。

四元数的乘法不可逆性，这里说的是四元数的外积，定义两个四元数:

它们相乘得到：

（看明白这个！既得到了平行又得到了垂直分量）

四元数乘法的非可换性，pq并不等于qp。我们发现，两个四元数相乘，我们不仅仅的得到内机，而且还得到外积！这是由四元数运算性质决定的。qp乘积的向量部分是:

，四元数乘法不可逆。

下面要说四元数是如何表示旋转的，这个确实有点不好理解，我看到的资料有的说的还是有点出入的，或许只有自己理解了才能明白吧。“一个单位四元数可以表示一个旋转”，这句话的信息量确实够大的。它基于的思路是:一个坐标系到另一个坐标系的变换可以通过绕一个定义在参考坐标系中的矢量μ的单次转动来实现，四元数提供了这种数学描述。我们直观的想一下，我们可以把刚体仅仅旋转一次就能达到任意的姿态，前提是确定了旋转轴线和旋转的角度，大家想一想，感觉也是可以实现的吧。

设Q=a+bi+cj+dk，我们可以写成[w，v],其中w=a，v=bi+cj+dk。

那么，v是矢量，表示三维空间里的旋转轴。w标量，表示旋转角度。所以，一个四元数可以表示一个完整的旋转。但要注意只有单位四元数才可以表示旋转，至于为什么，那是因为这就是四元数表示旋转的约束条件。

就如上面说的我们利用角度和旋转轴构造了一个四元数，下面就是要满足的这个关系：

定义μ的大小和方向是使参考坐标系绕μ转动一个角度μ，就能与载体坐标系重合。这可能不好理解，为什么说绕矢量μ还转动μ，(μx/μ)就相当于

方向余弦，这里并不矛盾。个人认为，也可以旋转载体坐标系和参考坐标系重合。这样定义也是一样的：

其中是绕旋转轴旋转的角度，为旋转轴在x,y,z方

向的分量，就是方向余弦（由此确定了旋转轴，为啥？）。

3、利用四元数进行矢量变换

那么利用四元数代表旋转是如何实现的，在载体系定义的一个矢量r b可以直接利用四元数将其在参考系中表示为r n。首先定义一个四元数r b'，它的虚部等于r b的相应分量，标量分量为零：

参考系中的r n’表示为，其中q=a+bi+cj+dk，q*的复共轭。因此有：

写成矩阵式：

这个矩阵就对应方向余弦矩阵了，从而对应欧拉角了。

肯定还有没回过神的，上面仅仅说明了一个“结果”：。要直观说清楚四

元数如何工作的确实有点抽象。我们把三维空间里的点用实部为0的四元数表示成q=bi+cj +dk，我们可以理解为虚部就是表示我们生活的三维空间，那么我们把q左乘一个标准四元数后，我们能得到什么？

为了更清楚地看到两个四元数乘积到底是什么样子，我们把上节用到的向量空间的观点

拿过来，四元数的全体构成的集合F是实数域上的四维向量空间，可以把四元数q=a+bi+ cj+dk看成四维实数元组（a，b，c，d）。然后用另外一个四元数Q左乘q，这个运算就相当于q在四维空间F上的线性变换，这就如同两个虚数相乘（1+i）*（2+i）=1+3i，（2+i）在复数坐标系上的一个线性变换——大小和方向都发送了变化。如果Q是个纯实数（虚部为0的四元数），那么Q*q就是q简单的伸缩比例，方向什么的没有发生变化；如果Q是个虚部不为0的四元数，同样道理q不仅仅是做了伸缩，而且方向也发生了旋转，不仅如此，所有的向量长度都伸缩相同的倍数。根据线性代数理论，一个等距线性变换要么是单纯的旋转，要么是单纯的对称变换，要么是二者的复合。而四维空间上这样的线性变换必有两个垂

直的二维不变子空间（就是后三相），也就是说，可以在四维空间中找到两个相垂直的平面，在每个平面上的向量经过变换之后还是在这个平面上。

4、欧拉角、方向余弦、四元数的关系

上面已经说的很清楚了，看一下：

通过比较上述等式的各个元素，四元数可以直接用欧拉角或方向余弦表示。同样

地，欧拉角也可以用方向余弦或四元数表示。

用方向余弦表示四元数：

用欧拉角直接表示四元数：

用方向余弦表示欧拉角：

可以按下述方法直接由方向余弦推导出欧拉角。当0！=90。时，欧拉角可由下式计算:

这个公式是在imu.c里有直接应用的。

5、传感器

关于传感器，价格贵的肯定性能就好点。还有就是传感器的安装方向，个人喜好X模式，飞行更灵活，适合航拍。

6、数据融合

数据融合也算是姿态解算的核心了，好的解算算法可以更好的补偿传感器误差，目前就研究了论坛里的imu.c滤波思想，感觉很巧妙。

//Description:

//

//Quaternion implementation of the'DCM filter'[Mayhony et al].

//

//User must define'halfT'as the(sample period/2),and the filter gains'Kp'and'Ki'.

//

//Global variables'q0','q1','q2','q3'are the quaternion elements representing the estimated //orientation.See my report for an overview of the use of quaternions in this application.

//

//User must call'IMUupdate()'every sample period and parse calibrated gyroscope('gx','gy', 'gz')

//and accelerometer('ax','ay','ay')data.Gyroscope units are radians/second,accelerometer //units are irrelevant as the vector is normalised.

//

//================================================================= ====================================

#define Kp10.0f//proportional gain governs rate of convergence to accelerometer/magnetometer

#define Ki0.008f//integral gain governs rate of convergence of gyroscope biases

#define halfT0.001f//half the sample period采样周期的一半

float q0=1,q1=0,q2=0,q3=0;//quaternion elements representing the estimated orientation

float exInt=0,eyInt=0,ezInt=0;//scaled integral error

void IMUupdate(float gx,float gy,float gz,float ax,float ay,float az)

{

float norm;

float vx,vy,vz;//wx,wy,wz;

float ex,ey,ez;

//先把这些用得到的值算好

float q0q0=q0*q0;

float q0q1=q0*q1;

float q0q2=q0*q2;

float q1q1=q1*q1;

float q1q3=q1*q3;

float q2q2=q2*q2;

float q2q3=q2*q3;

float q3q3=q3*q3;

if(ax*ay*az==0)

return;

//normalise the measurements

norm=sqrt(ax*ax+ay*ay+az*az);//加计数据归一化,把加计的三维向量转换为单位向量。因为是单位矢量到参考性的投影，所以要把acc单位化

ax=ax/norm;//其实归一化改变的只是这三个向量的长度，也就是只

ay=ay/norm;//改变了相同的倍数，方向并没改变，也是为了与单位

az=az/norm;//四元数对应。

//estimated direction of gravity and flux(v and w)，估计重力方向和流量/变迁

//这是把四元数换算成方向余弦中的第三行的三个元素，根据余弦矩阵和欧拉角的定义，地里坐标系的Z轴重力向量，

//转到机体坐标系，正好是这三个元素，所以这里的vx,vy,vz其实就是上一次的欧拉角（四元数）的机体坐标参考系

//换算出来的重力的单位向量。(是带有误差的姿态。)

vx=2*(q1q3-q0q2);//参考系Z轴与载体系x轴之间方向余弦向量

vy=2*(q0q1+q2q3);//参考系Z轴与载体系y轴之间方向余弦向量

vz=q0q0-q1q1-q2q2+q3q3;//参考系Z轴与载体系z轴之间方向余弦向量

//error is sum of cross product between reference direction of fields and direction measured by sensors

//ax，ay，az是机体坐标参考系上，加计测出来的重力向量，也就是实际测出来的重力向量（这明显就是重力在三个轴

//上的“分担”）。那他们之间的误差向量就是陀螺积分后的姿态和加计测出来

//的姿态之间的误差。向量间的误差可以用向量的叉积（外积）来表示，ex，ey，ez就是两重力方向的叉积，注意这个

//叉积向量仍是在机体坐标系上的，而陀螺的积分误差也是在机体坐标系上的，而且叉积的大小和陀螺的积分误差成正

//比，正好用来修正陀螺，这是因为，由于陀螺是对机体直接积分的，所以对陀螺的误差修正会直接会直接体现在对机

//体坐标系的修正，就是对机体修正。说白了，就是用加计测量标定陀螺积分。

//这里误差没说清楚，不是指向量差。这个叉积误差是指将带有误差的加计向量转动到与重力向量重合，需要绕什么轴，

//转多少角度。逆向推理一下，这个叉积在机体三轴上的投影，就是加计和重力之间的角度误差。也就是说，如果陀螺

//按这个叉积误差的轴，转动叉积误差的角度（也就是转动三轴投影的角度）那就能把加计和重力向量的误差消除掉。

//（具体可看向量叉积的定义）如果完全按叉积误差转过去，那就是完全信任加计。如果一点也不转，那就是完全信任

//陀螺。那么把这个叉积的三轴乘以X%，加到陀螺的积分角度上去，就是这个x%互补

系数的互补算法了。

//ps：实际上叉积的length是两向量夹角的正弦，而且必须在±90度以内，并不完全与误差角度成线性正比。如果转成三

//轴夹角，按欧拉角的转动顺序分解到三轴上去，会很麻烦。这里的叉积算法把sin误差当成角度误差，并无视欧拉角的

//转动顺序，在误差较小的时候，并不会有影响。因为这个修正对误差来说，是收敛的，正常情况下误差只会越来越小。

//为了解释方便，所以上面就把叉积等同于角度误差了。

ex=ay*vz-az*vy;//上面说了，vx,vy,vz就是方向余弦，

ey=az*vx-ax*vz;

ez=ax*vy-ay*vx;

//对误差进行积分

exInt=exInt+ex*Ki;

eyInt=eyInt+ey*Ki;

ezInt=ezInt+ez*Ki;

//adjusted gyroscope measurements

gx=gx+Kp*ex+exInt;//将误差PI后补偿到陀螺仪，即补偿零点漂移

gy=gy+Kp*ey+eyInt;//修正陀螺输出

gz=gz+Kp*ez+ezInt;//这里的gz由于没有观测者进行矫正会产生漂移，表现出来的就是积分自增或自减

//integrate quaternion rate and normalise//四元素的微分方程

q0=q0+(-q1*gx-q2*gy-q3*gz)*halfT;

q1=q1+(q0*gx+q2*gz-q3*gy)*halfT;

q2=q2+(q0*gy-q1*gz+q3*gx)*halfT;

q3=q3+(q0*gz+q1*gy-q2*gx)*halfT;

这是四元数的微分方程，用于更新四元数，

所以说halfT定义为0.001，因为我们的姿态解算的周期是2ms，这里有个0.5系数。

//normalise quaternion

//由于误差的引入，使计算的变换四元数的模不再等于1，变换四元数失去规范性，因此在利用更新四元数

//计算欧拉角时必须要对四元数规范化处理。

norm=sqrt(q0*q0+q1*q1+q2*q2+q3*q3);

q0=q0/norm;

q1=q1/norm;

q2=q2/norm;

q3=q3/norm;

//由计算四元数计算欧拉角

//Q_ANGLE.Z=atan2(2*q1*q2+2*q0*q3,-2*q2*q2-2*q3*q3+1)*Rad;//yaw Q_ANGLE.Y=asin(-2*q1*q3+2*q0*q2)*Rad;//pitch

Q_ANGLE.X=atan2(2*q2*q3+2*q0*q1,-2*q1*q1-2*q2*q2+1)*Rad;// roll

if(Q_ANGLE.X>90||Q_ANGLE.X<-90)

{

if(Q_ANGLE.Y>0)

Q_ANGLE.Y=180-Q_ANGLE.Y;

if(Q_ANGLE.Y<0)

Q_ANGLE.Y=-(180+Q_ANGLE.Y);

}

}

卡尔曼滤波算法与matlab实现

一个应用实例详解卡尔曼滤波及其算法实现 标签：算法filtermatlabalgorithm优化工作 2012-05-14 10:48 75511人阅读评论(25) 收藏举报分类： 数据结构及其算法（4） 为了可以更加容易的理解卡尔曼滤波器，这里会应用形象的描述方法来讲解，而不是像大多数参考书那样罗列一大堆的数学公式和数学符号。但是，他的5条公式是其核心内容。结合现代的计算机，其实卡尔曼的程序相当的简单，只要你理解了他的那5条公式。 在介绍他的5条公式之前，先让我们来根据下面的例子一步一步的探索。 假设我们要研究的对象是一个房间的温度。根据你的经验判断，这个房间的温度是恒定的，也就是下一分钟的温度等于现在这一分钟的温度（假设我们用一分钟来做时间单位）。假设你对你的经验不是100%的相信，可能会有上下偏差几度。 我们把这些偏差看成是高斯白噪声（White Gaussian Noise），也就是这些偏差跟前后时间是没有关系的而且符合高斯分配（Gaussian Distribution）。另外，我们在房间里放一个温度计，但是这个温度计也不准确的，测量值会比实际值偏差。我们也把这些偏差看成是高斯白噪声。 好了，现在对于某一分钟我们有两个有关于该房间的温度值：你根据经验的预测值（系统的预测值）和温度计的值（测量值）。下面我们要用这两个值结合他们各自的噪声来估算出房间的实际温度值。 假如我们要估算k时刻的是实际温度值。首先你要根据k-1时刻的温度值，来预测k时刻的温度。因为你相信温度是恒定的，所以你会得到k时刻的温度预测值是跟k-1时刻一样的，假设是23度，同时该值的高斯噪声的偏差是5度（5是这样得到的：如果k-1时刻估算出的最优温度值的偏差是3，你对自己预测的不确定度是4度，他们平方相加再开方，就是5）。然后，你从温度计那里得到了k时刻的温度值，假设是25度，同时该值的偏差是4度。 由于我们用于估算k时刻的实际温度有两个温度值，分别是23 度和25度。究竟实际温度是多少呢？相信自己还是相信温度计呢？究竟相信谁多一点，我们可以用他们的covariance（协方差）来判断。因为Kg^2=5^2/(5^2+4^2)，所以Kg=0.78，我们可以估算出k时刻的实际温度值是：23+0.78*(25-23)=24.56度。 可以看出，因为温度计的covariance比较小（比较相信温度计），所以估算出的最优温度值偏向温度计的值。 现在我们已经得到k时刻的最优温度值了，下一步就是要进入k+1时刻，进行新的最优估算。到现在为止，好像还没看到什么自回归的东西出现。对了，在进入k+1时刻之前，我们还要算出k时刻那个最优值（24.56 度）的偏差。算法如下：((1-Kg)*5^2)^0.5=2.35。这里的5就是上面的k时刻你预测的那个23度

基于互补滤波的飞行器姿态解算

基于互补滤波的飞行器姿态解算

————————————————————————————————作者：————————————————————————————————日期： ?

姿态解算 一、主线 姿态表示方式：矩阵表示,轴角表示,欧拉角表示，四元数表示。 惯性测量单元IMU(Inertｉal Ｍeasureｍｅnt Unｉｔ）：MＰU60５0芯片，包含陀螺仪和加速度计,分别测量三轴加速度和三轴角速度。注意,传感器所测数据是原始数据，包含了噪声,无法直接用于飞行器的姿态解算,因此需要对数据进行滤波。 滤波算法:非线性互补滤波算法，卡尔曼滤波算法,Maｈonｙ互补滤波算法。 二、知识点补充 加速度计和陀螺仪 加速度计：加速度计，可以测量加速度，包括外力加速度和重力加速度,因此,当被测物体在静止或匀速运动（匀速直线运动)的时候,加速度计仅仅测量的是重力加速度,而重力加速度与R坐标系(绝对坐标系）是固连的，通过这种关系,可以得到加速度计所在平面与地面的角度关系也就是横滚角和俯仰角。把加速度传感器水平静止放在桌子上，它的Ｚ轴输出的是1g的加速度。因为它Z轴方向被重力向下拉出了一个形变。可惜的是，加速度传感器不会区分重力加速度与外力加速度。所以，当系统在三维空间做变速运动时,它的输出就不正确了,或者说它的输出不能表明物体的姿态和运动状态。 陀螺仪:陀螺仪测量角速度。陀螺仪模型如图1所示，陀螺仪的每个通道检测一个轴的旋转。 图1[引自网络] 上图中，Rxz是Ｒ在XZ面上的投影,与Z轴的夹角为Ａxz。Ryｚ是Ｒ在ZＹ面上的投影，与Z轴的夹角为Ayz。陀螺仪就是测量上面定义角度的变化率，换句话说，它会输出一个与上面这些角度变化率线性相关的值。 加速度计工作原理介绍（摘自网络) 大多数加速度计可归为两类:数字和模拟。数字加速度计可通过I2C，SPI或USART方式获取信息,而模拟加速度计的输出是一个在预定范围内的电压值，你需要用ＡDC（模拟量转数字量）模块将其转换为数字值。不管使用什么类型的ADC模块,都会得到一个在一定范围内的数值。例如一个1０位ADＣ模块的输出值范围在0－1023间。假设我们从10位ADC模块得到了以下的三个轴的数据： === 586,630,561 AdcRx AdcRy AdcRz

几种非线性滤波算法的研究-内附程序

2017 年秋季学期研究生课程考核 （读书报告、研究报告） 考核科目:雷达系统导论 学生所在（系）:电子与信息工程学院 学生所在学科:电子与同学工程 学生姓名: 学号: 学生类别: 考核结果阅卷人 第 1 页（共页）

几种非线性滤波算法的介绍与性能分析 作者姓名：学号： 专业院系：电信学院电子工程系 电子邮件： 摘要—非线性滤波算法在雷达目标跟踪中有着重要的应用，对雷达的跟踪性能有着至关重要的影响。好的滤波算法有利于目标航迹的建立及保持，能够得到较精确的目标位置，为发现目标后的后续工作提供可靠的数据依据。本文重点介绍了雷达数据处理中的几种非线性滤波算法：扩展卡尔曼滤波（EKF）、不敏卡尔曼滤波（UKF）、粒子滤波（PF），并且给出了一个利用这三种算法进行数据处理的一个实例，通过这个实例对比分析了这三种算法的性能以及优劣。 关键字—非线性滤波算法；扩展卡尔曼滤波；不敏卡尔曼滤波；粒子滤波； I.概述（一级表题格式） 在雷达对目标进行跟踪前要先对目标进行检测。对于满足检测条件的目标就需要进行跟踪，在跟踪的过程中可以利用新获得的数据完成对目标的进一步检测比如去除虚假目标等，同时利用跟踪获得数据可以进一步完成对目标动态特性的检测和识别。因此对目标进行准确的跟踪是雷达性能的一个重要指标。在检测到满足条件的目标后，根据目标运动状态建立目标运动模型，然后对目标跟踪算法进行设计，这是雷达目标跟踪中的核心部分。 目前主要的跟踪算法包括线性自回归滤波，两点外推滤波、维纳滤波、- αβ滤波、加权最小二乘滤波、维纳滤波和卡尔曼滤波[1]。对于线性系统而言最优滤波的方法就是卡尔曼滤波，卡尔曼滤波是线性高斯模型下的最优状态估计算法。但是实际问题中目标的运动模型往往不是线性的，因此卡尔曼滤波具有很大的局限性。目前主要用的非线性滤波算法可以分为高斯滤波和粒子滤波[2]。不敏卡尔曼滤波和扩展卡尔曼滤波就是高斯滤波中的典型代表，也是应用相对较为广泛的。粒子滤波的应用范围比高斯滤波的适用范围要广，对于系统状态非线性，观测模型非高斯等问题都有很好的适用性。本文具体分析阐述了扩展卡尔曼滤波算法，不敏卡尔曼滤波算法，粒子滤波算法，并且通过一个实例利用仿真的方法分析了这三种算法在滤波性能上的优劣，最后对这三种算法做了一定的总结。 我本科毕业设计题目为《基于历史数据的路径生成算法研究》，由于我是跨专业保研到电信学院，该课题所研究内容不属于雷达系统研究范围，是一种城市路网最快路径生成算法。 II.几种非线性滤波算法 A.扩展卡尔曼滤波 扩展卡尔曼滤波是将非线性系统转换为近似的线性系统的一种方法，其核心思想是围绕滤波值将非线性函数展开成泰勒级数并略去二阶及以上的项，得到一个近似的线性化模型，然后应用卡尔曼滤波完成状态估计。 扩展卡尔曼滤波状态空间模型： k k k w x f+ = + ) ( x 1 状态方程 k k k v x h+ =) ( z观测方程 其中(.) f和(.) h为非线性函数 在扩展卡尔曼滤波中，状态的预测以及观测值的预测由非线性函数计算得出，线性卡尔曼滤波中的状态转移矩阵A阵和观测矩阵H阵由f和h函数的雅克比矩阵代替。 对 (.) f和(.) h Taylor展开，只保留一次项有： ) ? ( ) ?( ) ( k k k k k x x A x f x f- + ≈ ) ? ( ) ?( ) ( k k k k k x x H x h x h- + ≈ 其中： k k x x k k dx df A ?= =为f对 1- k x求导的雅克比矩阵 k k x x k k dx dh H ?= =为h对 1- k x求导的雅克比矩阵 ) ?( ? 1-k k x f x=，于是可以得出： k k k k k k k w x A x f x A x+ - + ≈ + ) ? ) ?( ( 1 k k k k k k k v x H x h x H z+ - + ≈ + ) ? ) ?( ( 1 通过以上变换，将非线性问题线性化。接下来EKF 滤波过程同线性卡尔曼滤波相同，公式如下: )) | (?( ) |1 ( X?k k X f k k= + ) ( ) ( ) | ( ) ( ) |1 (P k Q k k k P k k k+ Φ' Φ = + )1 ( )1 ( ) |1 ( )1 ( )1 (S+ + + ' + + = +k R k H k k P k H k )1 ( )1 ( ) |1 ( )1 ( K1+ + ' + = +-k S k H k k P k

基于互补滤波的飞行器姿态解算

姿态解算 一、主线 姿态表示方式：矩阵表示，轴角表示，欧拉角表示，四元数表示。 惯性测量单元IMU（Inertial Measurement Unit）：MPU6050芯片，包含陀螺仪和加速度计，分别测量三轴加速度和三轴角速度。注意，传感器所测数据是原始数据，包含了噪声，无法直接用于飞行器的姿态解算，因此需要对数据进行滤波。 滤波算法：非线性互补滤波算法，卡尔曼滤波算法，Mahony互补滤波算法。 二、知识点补充 加速度计和陀螺仪 加速度计：加速度计,可以测量加速度,包括外力加速度和重力加速度,因此，当被测物体在静止或匀速运动（匀速直线运动）的时候,加速度计仅仅测量的是重力加速度,而重力加速度与R坐标系（绝对坐标系）是固连的,通过这种关系,可以得到加速度计所在平面与地面的角度关系也就是横滚角和俯仰角。把加速度传感器水平静止放在桌子上，它的Z轴输出的是1g的加速度。因为它Z轴方向被重力向下拉出了一个形变。可惜的是，加速度传感器不会区分重力加速度与外力加速度。所以，当系统在三维空间做变速运动时，它的输出就不正确了，或者说它的输出不能表明物体的姿态和运动状态。 陀螺仪：陀螺仪测量角速度。陀螺仪模型如图1所示，陀螺仪的每个通道检测一个轴的旋转。 图1[引自网络] 上图中，Rxz是R在XZ面上的投影，与Z轴的夹角为Axz。Ryz是R在ZY面上的投影，与Z轴的夹角为Ayz。陀螺仪就是测量上面定义角度的变化率，换句话说，它会输出一个与上面这些角度变化率线性相关的值。 加速度计工作原理介绍（摘自网络） 大多数加速度计可归为两类：数字和模拟。数字加速度计可通过I2C，SPI或USART方式获取信息，而模拟加速度计的输出是一个在预定围的电压值，你需要用ADC（模拟量转数字量）模块将其转换为数字值。不管使用什么类型的ADC模块，都会得到一个在一定围的数值。例如一个10位ADC模块的输出值围在0-1023间。假设我们从10位ADC模块得到了以下的三个轴的数据： === 586,630,561 AdcRx AdcRy AdcRz

卡尔曼滤波算法总结

Kalman_Filter(float Gyro,float Accel) { Angle+=(Gyro - Q_bias) * dt; Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; Pdot[1]= - PP[1][1]; Pdot[2]= - PP[1][1]; Pdot[3]=Q_gyro; PP[0][0] += Pdot[0] * dt; PP[0][1] += Pdot[1] * dt; PP[1][0] += Pdot[2] * dt; PP[1][1] += Pdot[3] * dt; Angle_err = Accel - Angle; PCt_0 = C_0 * PP[0][0]; PCt_1 = C_0 * PP[1][0]; E = R_angle + C_0 * PCt_0; K_0 = PCt_0 / E; K_1 = PCt_1 / E; t_0 = PCt_0; t_1 = C_0 * PP[0][1]; PP[0][0] -= K_0 * t_0; PP[0][1] -= K_0 * t_1; PP[1][0] -= K_1 * t_0; PP[1][1] -= K_1 * t_1; Angle += K_0 * Angle_err; Q_bias += K_1 * Angle_err; Gyro_x = Gyro - Q_bias; } 首先是卡尔曼滤波的5个方程： -=--+（1）先验估计 X k k AX k k Bu k (|1)(1|1)() -=--+（2）协方差矩阵的预测(|1)(1|1)' P k k AP k k A Q

时间序列分析方法之卡尔曼滤波

第十三章 卡尔曼滤波 在本章中，我们介绍一种被称为卡尔曼滤波的十分有用的工具。卡尔曼滤波的基本思想是将动态系统表示成为一种称为状态空间表示的特殊情形。卡尔曼滤波是对系统线性投影进行序列更新的算法。除了一般的优点以外，这种算法对计算确切的有限样本预测、计算Gauss ARMA 模型的确切似然函数、估计具有时变参数的自回归模型等，都提供了重要方法。 §13.1 动态系统的状态空间表示 我们已经介绍过一些随机过程的动态表示方法，下面我们在以前的假设基础上，继续分析动态系统的表示方法。 13.1.1 继续使用的假设 假设表示时刻观测到的n 维随机向量，一类非常丰富的描述动态性的模型可以利用一些可能无法观测的被称为状态向量(state vector)的r 维向量表示，因此表示动态性的状态空间表示(state-space representation)由下列方程系统给出： 状态方程(state model) (13.1) 量测方程(observation model) (13.2) 这里，和分别是阶数为，和的参数矩阵，是的外生或者前定变量。方程(13.1)被称为状态方程(state model)，方程(13.2)被称为量测方程(observation model)，维向量和维向量都是向量白噪声，满足： (13.3) (13.4) 这里和是和阶矩阵。假设扰动项和对于所有阶滞后都是不相关的，即对所有和，有： (13.5) t x 是外生或者前定变量的假定意味着，在除了包含在121,,,y y y t t 内的信息以外，t x 没有为s t ξ和s t w ( ,2,1,0 s )提供任何新的信息。例如，t x 可以包括t y 的滞后值，也可以包括与 ξ和 w (任意 )不相关的变量。 方程系统中方程(13.1)至方程(13.5)可以表示有限观测值的序列},,,{21T y y y ，这时需要状态向量初始值1ξ。假设1ξ与t v 和t w 的任何实现都不相关：

几种卡尔曼滤波算法理论

自适应卡尔曼滤波 卡尔曼滤波发散的原因 如果卡尔曼滤波是稳定的，随着滤波的推进，卡尔曼滤波估计的精度应该越来越高，滤波误差方差阵也应趋于稳定值或有界值。但在实际应用中，随着量测值数目的增加，由于估计误差的均值和估计误差协方差可能越来越大，使滤波逐渐失去准确估计的作用，这种现象称为卡尔曼滤波发散。 引起滤波器发散的主要原因有两点： （1）描述系统动力学特性的数学模型和噪声估计模型不准确，不能直接真实地反映物理过程，使得模型与获得的量测值不匹配而导致滤波发散。这种由于模型建立过于粗糙或失真所引起的发散称为滤波发散。 （2）由于卡尔曼滤波是递推过程，随着滤波步数的增加，舍入误差将逐渐积累。如果计算机字长不够长，这种积累误差很有可能使估计误差方差阵失去非负定性甚至失去对称性，使滤波增益矩阵逐渐失去合适的加权作用而导致发散。这种由于计算舍入误差所引起的发散称为计算发散。 针对上述卡尔曼滤波发散的原因，目前已经出现了几种有效抑制滤波发散的方法，常用的有衰减记忆滤波、限定记忆滤波、扩充状态滤波、有限下界滤波、平方根滤波、和自适应滤波等。这些方法本质上都是以牺牲滤波器的最优性为代价来抑制滤波发散，也就是说，多数都是次优滤波方法。 自适应滤波 在很多实际系统中，系统过程噪声方差矩阵Q和量测误差方差阵R事先是不知道的，有时甚至连状态转移矩阵 或量测矩阵H也不能确切建立。如果所建立的模型与实际模型不符可能回引起滤波发散。自适应滤波就是这样一种具有抑制滤波发散作用的滤波方法。在滤波过程中，自适应滤波一方面利用量测值修正预测值，同时也对未知的或不确切的系统模型参数和噪声统计参数进行估计修正。自适应滤波的方法很多，包括贝叶斯法、极大似然法、相关法与协方差匹配法，其中最基本也是最重要的是相关法，而相关法可分为输出相关法和新息相关法。 在这里只讨论系统模型参数已知，而噪声统计参数Q和R未知情况下的自适应滤波。由于Q和R等参数最终是通过增益矩阵K影响滤波值的，因此进行自适应滤波时，也可以不去估计Q和R等参数而直接根据量测数据调整K就可以了。

卡尔曼滤波器总结

1. 卡尔曼全名Rudolf Emil Kalman ，匈牙利数学家，1930年出生于匈牙利首都布达佩斯。1953，1954年于麻省理工学院分别获得电机工程学士及硕士学位。1957年于哥伦比亚大学获得博士学位。我们现在要学习的卡尔曼滤波器，正是源于他的博士论文和1960年发表的论文《A New Approach to Linear Filtering and Prediction Problems 》（线性滤波与预测问题的新方法）。 基于状态空间描述对混有噪声的信号进行滤波的方法，简称卡尔曼滤波。这种方法是R.E.卡尔曼和R.S.布什于1960和1961年提出的。卡尔曼滤波是一种切实可行和便于应用的滤波方法，其计算过程通常需要在计算机上实现。实现卡尔曼滤波的装置或软件称为卡尔曼滤波器。 卡尔曼滤波器（Kalman Filter ）是在克服以往滤波方法局限性的基础上提出来的，是一个最优化自回归数据处理算法（optimal recursive data processing algorithm ）。它是针对系统的部分状态或是部分状态的线性组合，且量测值中有随机误差（常称为量测噪声）。将仅与部分状态有关的测量进行处理，得出从某种统计意义上讲误差最小的更多状态的估值，从而将混有噪声（干扰）的信号中噪声滤除、提取有用信号。 卡尔曼滤波是一种递推线性最小方差估计，以最小均方误差为估计的最佳准则，来寻求一套递推估计的算法，其基本思想是：采用信号与噪声的状态空间模型，利用前一时刻地估计值和现时刻的观测值来更新对状态变量的估计，求出现时刻的估计值。 现设线性时变系统的离散状态方程和观测方程为： ()()()()()X k+1F k X k G k u k ()w k =?++ ()()()()k+1H k+1X k+1k+1Y v =?+ 其中 ()k X 和()k Y 分别是k 时刻的状态矩阵和测量矩阵 ()k F 为状态转移矩阵 ()k G 为系统控制项矩阵 ()k u 为k 时刻对系统的控制量 ()k w 为k 时刻动态噪声，其协方差()Q k ()k H 为k 时刻观测矩阵 ()k v 为k 时刻测量噪声, 其协方差()R k 则卡尔曼滤波的算法流程为： 状态的一步预估计()()()()()??X k+1k F k X k k G k u k |=?|+ 一步预估计协方差矩阵 ()()()()()C k+1k F k C k k F k Q k '|=?|+' 计算卡尔曼增益矩阵

卡尔曼滤波的原理及应用自己总结

卡尔曼滤波的原理以及应用 滤波，实质上就是信号处理与变换的过程。目的是去除或减弱不想要成分，增强所需成分。卡尔曼滤波的这种去除与增强过程是基于状态量的估计值和实际值之间的均方误差最小准则来实现的，基于这种准则，使得状态量的估计值越来越接近实际想要的值。而状态量和信号量之间有转换的关系，所以估计出状态量，等价于估计出信号量。所以不同于维纳滤波等滤波方式，卡尔曼滤波是把状态空间理论引入到对物理系统的数学建模过程中来，用递归方法解决离散数据线性滤波的问题，它不需要知道全部过去的数据，而是用前一个估计值和最近一个观察数据来估计信号的当前值，从而它具有运用计算机计算方便，而且可用于平稳和不平稳的随机过程(信号)，非时变和时变的系统的优越性。 卡尔曼滤波属于一种软件滤波方法，概括来说其基本思想是：以最小均方误差为最佳估计准则，采用信号与噪声的状态空间模型，利用前一时刻的估计值和当前时刻的观测值来更新对状态变量的估计，求出当前时刻的估计值，算法根据建立的系统方程和观测方程对需要处理的信号做出满足最小均方误差的估计。其所得到的解是以估计值的形式给出的。 卡尔曼滤波过程简单来说主要包括两个步骤：状态变量的预估以及状态变量的校正。预估过程是不考虑过程噪声和量测噪声，只是基于系统本身性质并依靠前一时刻的估计值以及系统控制输入的一种估计；校正过程是用量测值与预估量测值之间的误差乘以一个与过程

噪声和量测噪声相关的增益因子来对预估值进行校正的，其中增益因子的确定与状态量的均方误差有关，用到了使均方误差最小的准则。而这一过程中体现出来的递归思想即是：对于当前时刻的状态量估计值以及均方误差预估值实时进行更新，以便用于下一时刻的估计，使得系统在停止运行之前能够源源不断地进行下去。 下面对于其数学建模过程进行详细说明。 1.状态量的预估 （1）由前一时刻的估计值和送给系统的可控制输入来预估计当前时刻状态量。 X(k|k-1)=A X(k-1|k-1)+B U(k) 其中，X(k-1|k-1)表示前一时刻的估计值，U(k)表示系统的控制输入，X(k|k-1)表示由前一时刻估计出来的状态量的预估计值，A表示由k-1时刻过渡到k时刻的状态转移矩阵，B表示控制输入量与状态量之间的一种转换因子，这两个都是由系统性质来决定的。 （2）由前一时刻的均方误差阵来预估计当前时刻的均方误差阵。 P(k|k-1)=A P(k-1|k-1)A’+Q 其中，P(k-1|k-1)是前一时刻的均方误差估计值，A’代表矩阵A 的转置，Q代表过程噪声的均方误差矩阵。该表达式具体推导过程如下： P(k|k-1)=E{[Xs(k|k)-X(k|k-1)][Xs(k|k)-X(k|k-1)]’}------ 其中Xs(k|k)=A Xs(k-1|k-1)+B U(k)+W(k-1)表示当前时刻的实际值，Xs(k-1|k-1)表示前一时刻的实际值，可以看出与当前时刻的预估计值

卡尔曼滤波算法(KF)

卡尔曼滤波器Kalman Filter (zz) 关键词：卡尔曼滤波器Kalman Filter 在学习卡尔曼滤波器之前，首先看看为什么叫“卡尔曼”。跟其他著名的理论（例如傅立叶变换，泰勒级数等等）一样，卡尔曼也是一个人的名字，而跟他们不同的是，他是个现代人！ 卡尔曼全名Rudolf Emil Kalman，匈牙利数学家，1930年出生于匈牙利首都布达佩斯。1953，1954年于麻省理工学院分别获得电机工程学士及硕士学位。1957年于哥伦比亚大学获得博士学位。我们现在要学习的卡尔曼滤波器，正是源于他的博士论文和1960年发表的论文《A New Approach to Linear Filtering and Predict ion Problems》（线性滤波与预测问题的新方法）。如果对这编论文有兴趣，可以到这里的地址下载： https://www.sodocs.net/doc/191389984.html,/~welch/kalman/media/pdf/Kalman1960.pdf 简单来说，卡尔曼滤波器是一个“optimal recursive data processing algorithm（最优化自回归数据处理算法）”。对于解决很大部分的问题，他是最优，效率最高甚至是最有用的。他的广泛应用已经超过30年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。近年来更被应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。 2．卡尔曼滤波器的介绍 （Introduction to the Kalman Filter） 为了可以更加容易的理解卡尔曼滤波器，这里会应用形象的描述方法来讲解，而不是像大多数参考书那样罗列一大堆的数学公式和数学符号。但是，他的5条公式是其核心内容。结合现代的计算机，其实卡尔曼的程序相当的简单，只要你理解了他的那5条公式。 在介绍他的5条公式之前，先让我们来根据下面的例子一步一步的探索。 假设我们要研究的对象是一个房间的温度。根据你的经验判断，这个房间的温度是恒定的，也就是下一分钟的温度等于现在这一分钟的温度（假设我们用一分钟来做时间单位）。假设你对你的经验不是100%的相信，可能会有上下偏差几度。我们把这些偏差看成是高斯白噪声（White Gaussian Noise），也就是这些偏差跟前后

卡尔曼滤波算法

卡尔曼滤波算法 卡尔曼滤波器是一个“最优化自回归数据处理算法”。对于解决大部分的问题，它是最优，效率最高甚至是最有用的。其广泛应用已经超过30年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等。近年来更被应用于计算机图像处理，列入，面部识别，图像分割，图像边缘检测等方面。 卡尔曼滤波原理 首先要引入一个离散控制过程的系统，该系统可用一个线性随机微分方程来秒速： X（k）=AX（k-1）+BU（k）+W（k）（1）再加上系统的测量值： Z（k）=HX（k）+V（k）（2）上两式子中，X（k）是k时刻的系统状态，U（k）是k时刻对系统的控制量。A和B 是系统参数，对于多模型系统，它们为矩阵。Z（k）是k时刻的测量值，H是测量系统的参数，对于多测量系统，H为矩阵。W（k）和V(k)分别表示过程和测量的噪声。它们被假设成高斯白噪声，其协方差分别是Q,R,这里假设它们不随系统状态变化而变化。 由于满足上面的条件（线性随机微分系统，过程和测量都是高斯白噪声），卡尔曼滤波器是最优的信息处理器。下面来估算系统的最优化输出。 首先利用系统的过程模型预测下个状态的系统。假设现在的系统状态是k，根据系统的模型，可以基于系统的上一状态而预测出现在状态： X（k|k-1）=AX（k-1|k-1）+BU（k）（3）式（3）中，X（k|k-1）是利用上一个状态预测的结果，X（k-1|k-1）是上一个状态最优的结果，U（k）为现在状态的控制量，如果没有控制量，它可以为0. 到现在为止，系统结果已经更新了，可是对应于X(k|k-1)的协方差还没有更新。用P 表示协方差： P（k|k-1）=AP（k|k-1）A’+Q（4）式子（4）中P（k|k-1）是X（k|k-1）对应的协方差，P（k-1|k-1）是X（k-1|k-1）对应的协方差，A’表示A的转置矩阵，Q时系统过程的协方差。式（3），式（4）就是卡尔曼滤波器5个公式当中的前两个，也就是对系统的预测。 有了现在状态的预测结果，再收集现在状态的测量值。结合预测值和测量值，可以得到现在状态（k）的最优化估算值X（k|k）： X（k|k）=X（k|k-1）+Kg（k）（Z（k）-HX（k|k-1））（5）其中Kg为卡尔曼增益： Kg(k)=P(k|k-1)H’|(HP(k|k-1)H’+R) （6）到此为止，已经得到了k状态下最优的估算值X（k|k）。但是，为了要令卡尔曼滤波器不断地运行下去，直到系统过程结束，还要更新k状态下X（k|k）的covariance: P（k|k）=（I-Kg(k)H）P(k|k-1) （7）其中I为1的矩阵，对于单模型单测量，I=1。当系统进入式（k+1）状态时，P(k|k)就是式（4）中的P（k-1|k-1）。这样，算法就可以自回归地运算下去。 卡尔曼滤波器的原理基本描述了，式（3），（4），（5），（6），（7）是5个基本公式。根据这5个公式，可以很容易地实现计算机程序。

卡尔曼滤波算法总结

2015.12.12 void Kalman_Filter(float Gyro,float Accel) { Angle+=(Gyro - Q_bias) * dt; Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; Pdot[1]= - PP[1][1]; Pdot[2]= - PP[1][1]; Pdot[3]=Q_gyro; PP[0][0] += Pdot[0] * dt; PP[0][1] += Pdot[1] * dt; PP[1][0] += Pdot[2] * dt; PP[1][1] += Pdot[3] * dt; Angle_err = Accel - Angle; PCt_0 = C_0 * PP[0][0]; PCt_1 = C_0 * PP[1][0]; E = R_angle + C_0 * PCt_0; K_0 = PCt_0 / E; K_1 = PCt_1 / E; t_0 = PCt_0; t_1 = C_0 * PP[0][1]; PP[0][0] -= K_0 * t_0; PP[0][1] -= K_0 * t_1; PP[1][0] -= K_1 * t_0; PP[1][1] -= K_1 * t_1; Angle += K_0 * Angle_err; Q_bias += K_1 * Angle_err; Gyro_x = Gyro - Q_bias; }

首先是卡尔曼滤波的5个方程： (|1)(1|1)()X k k AX k k Bu k -=--+（1）先验估计 (|1)(1|1)'P k k AP k k A Q -=--+（2）协方差矩阵的预测 ()(|1)'/(|1)') Kg k P k k H HP k k H R =--+（3）计算卡尔曼增益 (|)(|1)()(()(|1)) X k k X k k Kg k Z k HX k k =-+--（4）进行修正 5个式子比较抽象，现在直接用实例来说： 一、卡尔曼滤波第一个式子 对于角度来说，我们认为此时的角度可以近似认为是上一时刻的角度值加上上一时刻陀螺仪测得的角加速度值乘以时间，因为d dt θω=?，角度微分等于时间的微分乘以角速度。但是陀螺仪有个静态漂移（而且还是变化的），静态漂移就是静止了没有角速度然后陀螺仪也会输出一个值，这个值肯定是没有意义的，计算时要把它减去。 由此我们得到了当前角度的预测值Angle Angle=Angle+(Gyro - Q_bias) * dt; 其中等号左边Angle 为此时的角度，等号右边Angle 为上一时刻的角度，Gyro 为陀螺仪测的角速度的值，dt 是两次滤波之间的时间间隔，我们的运行周期是4ms 或者6ms 。 同时 Q_bias 也是一个变化的量。 但是就预测来说认为现在的漂移跟上一时刻是相同的，即 Q_bias=Q_bias 将上面两个式子写成矩阵的形式 1_01_0 Angle dt Angle dt Q bias Q bia o s Gyr -=+ 得到上式，这个式子对应于卡尔曼滤波的第一个式子 (|1)(1|1)()X k k AX k k Bu k -=--+ ()|1X k k -为2维列向量 _Angle Q bias ，A 为2维方阵101dt -，()|-11X k k -为2维列向量_Angle Q bias ，B 为2维列向量0dt ， ()u k 为Gyro (|)(|1) P k k I Kg k H P k k =--（（））（5）更新协方差阵

基于Matlab的卡尔曼滤波算法仿真设计

基于Matlab的卡尔曼滤波算法仿真 1.卡尔曼滤波器原理 卡尔曼滤波是解决以均方误差最小为准则的最佳线性滤波问题，它根据前一个估计值和最近一个观察数据来估计信号的当前值。它是用状态方程和递推方法进行估计的，而它的解是以估计值（常常是状态变量的估计值）的形式给出其信号模型是从状态方程和量测方程得到的。 卡尔曼滤波号和噪声是用状态方程和测量方程来表示的。因此设计卡尔曼滤波器要求已知状态方程和测量方程。它不需要知道全部过去的数据，采用递推的方法计算，它既可以用于平稳和不平稳的随机过程，同时也可以应用解决非时变和时变系统，因而它比维纳过滤有更广泛的应用。 卡尔曼几个重要公式： s?(n|n) = a s?(n-1|n-1) + G n [x(n) –ac s?(n-1|n-1)] (1) P(n) = a2ξ(n-1) + Q (2) G n = ??(?) ?+?2?(?) (3) ξ(n) = ? ??? = (1 – cG n )P(n) (4) 这组方程的递推计算过程如图1所示。 图1. 卡尔曼滤波器递推运算流程图

卡尔曼滤波过程实际上是获取维纳解的递推运算过程，这一过程从某个初始状态启 动，经过迭代运算，最终到达稳定状态，即维纳滤波状态。递推计算按图1所示进行。假设已经有了初始值s?(0|0)和ξ（0），这样便可由式（2）计算P(1)，由式（3）计算G 1 ，由式（4）计算ξ（1），由式（1）计算s?(1|1)。ξ（1）和s?(1|1)便成为下一轮迭代运算的已知数据。在递推运算过程中，随着迭代次数n的增加，ξ（n）将 逐渐下降，知道最终趋近于某个稳定值ξ 。这时 ξ（n） = ξ（n - 1）= ξ 为求得这个稳定值，将式（3）和式（2）代入式（4），得到 ξ 02 + (1??2)?+?2? ?2?2 ξ0??? ?2?2 =0 解此方程即可求出ξ 。 2.基于Matlab的卡尔曼滤波器的仿真Matlab代码如下： clear N=200; w(1)=0; x(1)=5; a=1; c=1; %过程噪声 Q1=randn(1,N)*1; %测量噪声 Q2=randn(1,N); %状态矩阵 for k=2:N;x(k)=a*x(k-1)+Q1(k-1);end for k=1:N;Y(k)=c*x(k)+Q2(k);end p(1)=10; s(1)=1; for t=2:N; Rww = cov(Q1(1:t)); Rvv = cov(Q2(1:t)); p1(t)=a.^2*p(t-1)+Rww; %kalman 增益 b(t)=c*p1(t)/(c.^2*p1(t)+Rvv); s(t)=a*s(t-1)+b(t)*(Y(t)-a*c*s(t-1)); p(t)=p1(t)-c*b(t)*p1(t);

卡尔曼滤波算法

卡尔曼滤波器的介绍 为了可以更加容易的理解卡尔曼滤波器，这里会应用形象的描述方法来讲解，而不是像大多数参考书那样罗列一大堆的数学公式和数学符号。但是，他的5条公式是其核心内容。结合现代的计算机，其实卡尔曼的程序相当的简单，只要你理解了他的那5条公式。 在介绍他的5条公式之前，先让我们来根据下面的例子一步一步的探索。 假设我们要研究的对象是一个房间的温度。根据你的经验判断，这个房间的温度是恒定的，也就是下 一分钟的温度等于现在这一分钟的温度（假设我们用一分钟来做时间单位） 。假设你对你的经验不是100%的相信，可能会有上下偏差几度。我们把这些偏差看成是高斯白噪声，也就是这些偏差跟前后时间是没有关系的而且符合高斯分配。另外，我们在房间里放一个温度计，但是这个温度计也不准确的，测量值会比实际值偏差。我们也把这些偏差看成是高斯白噪声。 好了，现在对于某一分钟我们有两个有关于该房间的温度值：你根据经验的预测值（系统的预测值）和温度计的值（测量值）。下面我们要用这两个值结合他们各自的噪声来估算出房间的实际温度值。 假如我们要估算k时刻的是实际温度值。首先你要根据k-1时刻

的温度值，来预测k时刻的温度。因为你相信温度是恒定的，所以你会得到k时刻的温度预测值是跟k-1时刻一样的，假设是23度，同时该值的高斯噪声的偏差是5度（5是这样得到的：如果k-1时刻估算出的最优温度值的偏差是3，你对自己预测的不确定度是4度，他们平方相加再开方，就是5）。然后，你从温度计那里得到了k时刻的温度值，假设是25 度，同时该值的偏差是4度。 由于我们用于估算k时刻的实际温度有两个温度值，分别是23度和25度。究竟实际温度是多少呢？相信自己还是相信温度计呢？究竟相信谁多一点，我们可以用他们的covariance来判断。因为Kg^2=5^2/(5^2+4^2)所以Kg=0.78，我们可以估算出k时刻的实际温度值是：23+0.78*(25-23)=24.56度。可以看出，因为温度计的covariance比较小（比较相信温度计），所以估算出的最优温度值偏向温度计的值。 现在我们已经得到k时刻的最优温度值了，下一步就是要进入k+1时刻，进行新的最优估算。到现 在为止，好像还没看到什么自回归的东西出现。对了，在进入k+1时刻之前，我们还要算出k时刻那个最优值（24.56度）的偏差。算法如下：((1-Kg)*5^2)^0.5=2.35。这里的5就是上面的k时刻你预测的那个23度温度值的偏差，得出的2.35就是进入k+1时刻以后k时刻估算出的最优温度值的偏差（对应于上面的

卡尔曼滤波算法平衡小车

先上程序，这是抄的不知道谁的代码。。。抱歉了。。不过这程序好像都写的差不多void Kalman_Filter(float Gyro,float Accel) { Angle+=(Gyro - Q_bias) * dt; Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; / Pdot[1]= - PP[1][1]; Pdot[2]= - PP[1][1];/ Pdot[3]=Q_gyro; PP[0][0] += Pdot[0] * dt; PP[0][1] += Pdot[1] * dt; PP[1][0] += Pdot[2] * dt; PP[1][1] += Pdot[3] * dt; Angle_err = Accel - Angle; PCt_0 = C_0 * PP[0][0]; PCt_1 = C_0 * PP[1][0]; E = R_angle + C_0 * PCt_0; K_0 = PCt_0 / E; K_1 = PCt_1 / E; t_0 = PCt_0; t_1 = C_0 * PP[0][1]; PP[0][0] -= K_0 * t_0; PP[0][1] -= K_0 * t_1; PP[1][0] -= K_1 * t_0; PP[1][1] -= K_1 * t_1; Angle += K_0 * Angle_err; Q_bias += K_1 * Angle_err; Gyro_x = Gyro - Q_bias; } 首先是卡尔曼滤波的5个方程 X(k|k-1)=A X(k-1|k-1)+B U(k) ……….. (1)//先验估计 P(k|k-1)=A P(k-1|k-1) A’+Q ……… (2)//协方差矩阵的预测 Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R) ……… (3)//计算卡尔曼增益 X(k|k)= X(k|k-1)+Kg(k) (Z(k) - H X(k|k-1)) ……… (4)通过卡尔曼增益进行修正 P(k|k)=（I-Kg(k) H）P(k|k-1) ……… (5)//跟新协方差阵

卡尔曼滤波入门、简介及其算法MATLAB实现代码

卡尔曼滤波入门： 卡尔曼滤波是用来进行数据滤波用的，就是把含噪声的数据进行处理之后得出相对真值。卡尔曼滤波也可进行系统辨识。 卡尔曼滤波是一种基于统计学理论的算法，可以用来对含噪声数据进行在线处理，对噪声有特殊要求，也可以通过状态变量的增广形式实现系统辨识。 用上一个状态和当前状态的测量值来估计当前状态，这是因为上一个状态估计此时状态时会有误差，而测量的当前状态时也有一个测量误差，所以要根据这两个误差重新估计一个最接近真实状态的值。 信号处理的实际问题，常常是要解决在噪声中提取信号的问题，因此，我们需要寻找一种所谓有最佳线性过滤特性的滤波器。这种滤波器当信号与噪声同时输入时，在输出端能将信号尽可能精确地重现出来，而噪声却受到最大抑制。 维纳(Wiener)滤波与卡尔曼(Kalman)滤波就是用来解决这样一类从噪声中提取信号问题的一种过滤(或滤波)方法。 (1)过滤或滤波 - 从当前的和过去的观察值x(n)，x(n-1)，x(n-2)，…估计当前的信号值称为过滤或滤波; (2)预测或外推 - 从过去的观察值，估计当前的或将来的信号值称为预测或外推; (3)平滑或内插 - 从过去的观察值，估计过去的信号值称为平滑或内插; 因此，维纳过滤与卡尔曼过滤又常常被称为最佳线性过滤与预测或线性最优估计。这里所谓“最佳”与“最优”是以最小均方误差为准则的。 维纳过滤与卡尔曼过滤都是解决最佳线性过滤和预测问题，并且都是以均方误差最小为准则的。因此在平稳条件下，它们所得到的稳态结果是一致的。然而，它们解决的方法有很大区别。 维纳过滤是根据全部过去的和当前的观察数据来估计信号的当前值，它的解是以均方误差最小条件下所得到的系统的传递函数H(z)或单位样本响应h(n)的形式给出的，因此更常称这种系统为最佳线性过滤器或滤波器。 而卡尔曼过滤是用前一个估计值和最近一个观察数据(它不需要全部过去的观察数据)来估计信号的当前值，它是用状态方程和递推的方法进行估计的，它的解是以估计值(常常是状态变量值)形式给出的。因此更常称这种系统为线性最优估计器或滤波器。 维纳滤波器只适用于平稳随机过程，而卡尔曼滤波器却没有这个限制。维纳过滤中信号和噪声是用相关函数表示的，因此设计维纳滤波器要求已知信号和噪声的相关函数。 卡尔曼过滤中信号和噪声是状态方程和量测方程表示的，因此设计卡尔曼滤波器要求已知状态方程和量测方程(当然，相关函数与状态方程和量测方程之间会存在一定的关系。卡尔曼过滤方法看来似乎比维纳过滤方法优越，它用递推法计算，不需要知道全部过去的数据，从而运用计算机计算方便，而且它可用于平稳和不平稳的随机过程(信号)，非时变和时变的系统。 但从发展历史上来看维纳过滤的思想是40年代初提出来的，1949年正式以书的形式出版。卡尔曼过滤到60年代初才提出来，它是在维纳过滤的基础上发展起来的，虽然如上所述它比维纳过滤方法有不少优越的地方，但是最佳线性过滤问题是由维纳过滤首先解决的，维纳过滤的物理概念比较清楚，也可以认为卡尔曼滤波仅仅是对最佳线性过滤问题提出的一种新的算法。 卡尔曼滤波在数学上是一种统计估算方法，通过处理一系列带有误差的实际量测数据而得到的物理参数的最佳估算。例如在气象应用上，根据滤波的基本思想，利用前一时刻预报误差的反馈信息及时修正预报方程，以提高下一时刻预报精度。作温度预报一般只需要连续两个月的资料即可建立方程和递推关系。