//一阶互补
// a=tau / (tau + loop time)
// newAngle = angle measured with atan2 using the accelerometer
//加速度传感器输出值
// newRate = angle measured using the gyro
// looptime = loop time in millis()
float tau = 0.075;
float a = 0.0;
float Complementary(float newAngle, float newRate, int looptime)
{
float dtC = float(looptime) / 1000.0;
a = tau / (tau + dtC);
x_angleC = a * (x_angleC + newRate * dtC) + (1 - a) * (newAngle);
return x_angleC;
}
//二阶互补
// newAngle = angle measured with atan2 using the accelerometer
// newRate = angle measured using the gyro
// looptime = loop time in millis()
float Complementary2(float newAngle, float newRate, int looptime)
{
float k = 10;
float dtc2 = float(looptime) / 1000.0;
x1 = (newAngle - x_angle2C) * k * k;
y1 = dtc2 * x1 + y1;
x2 = y1 + (newAngle - x_angle2C) * 2 * k + newRate;
x_angle2C = dtc2 * x2 + x_angle2C;
return x_angle2C;
}
//Here too we just have to set the k and magically we get the angle. 卡尔曼滤波// KasBot V1 - Kalman filter module
float Q_angle = 0.01; //0.001
float Q_gyro = 0.0003; //0.003
float R_angle = 0.01; //0.03
float x_bias = 0;
float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;
float y, S;
float K_0, K_1;
// newAngle = angle measured with atan2 using the accelerometer
// newRate = angle measured using the gyro
// looptime = loop time in millis()
float kalmanCalculate(float newAngle, float newRate, int looptime)
{
float dt = float(looptime) / 1000;
x_angle += dt * (newRate - x_bias);
P_00 += - dt * (P_10 + P_01) + Q_angle * dt;
P_01 += - dt * P_11;
P_10 += - dt * P_11;
P_11 += + Q_gyro * dt;
y = newAngle - x_angle;
S = P_00 + R_angle;
K_0 = P_00 / S;
K_1 = P_10 / S;
x_angle += K_0 * y;
x_bias += K_1 * y;
P_00 -= K_0 * P_00;
P_01 -= K_0 * P_01;
P_10 -= K_1 * P_00;
P_11 -= K_1 * P_01;
return x_angle;
}
//To get the answer, you have to set 3 parameters: Q_angle, R_angle, R_gyro.
//详细卡尔曼滤波
/* -*- indent-tabs-mode:T; c-basic-offset:8; tab-width:8; -*- vi: set ts=8:
* $Id: tilt.c,v 1.1 2003/07/09 18:23:29 john Exp $
*
* 1 dimensional tilt sensor using a dual axis accelerometer
* and single axis angular rate gyro. The two sensors are fused
* via a two state Kalman filter, with one state being the angle
* and the other state being the gyro bias. *
* Gyro bias is automatically tracked by the filter. This seems
* like magic.
*
* Please note that there are lots of comments in the functions and
* in blocks before the functions. Kalman filtering is an already complex
* subject, made even more so by extensive hand optimizations to the C code
* that implements the filter. I've tried to make an effort of explaining
* the optimizations, but feel free to send mail to the mailing list,
* autopilot-devel@https://www.doczj.com/doc/9310352147.html,, with questions about this code.
*
*
* (c) 2003 Trammell Hudson
*
*************
*
* This file is part of the autopilot onboard code package.
*
* Autopilot is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* Autopilot is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Autopilot; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA *
*/
#include
/*
* Our update rate. This is how often our state is updated with
* gyro rate measurements. For now, we do it every time an
* 8 bit counter running at CLK/1024 expires. You will have to
* change this value if you update at a different rate.
*/
static const float dt = ( 1024.0 * 256.0 ) / 8000000.0;
/*
* Our covariance matrix. This is updated at every time step to
* determine how well the sensors are tracking the actual state.
*/
static float P[2][2] =
{
{ 1, 0 },
{ 0, 1 },
};
/*
* Our two states, the angle and the gyro bias. As a byproduct of computing
* the angle, we also have an unbiased angular rate available. These are
* read-only to the user of the module.
*/
float angle;
float q_bias;
float rate;
/*
* R represents the measurement covariance noise. In this case,
* it is a 1x1 matrix that says that we expect 0.3 rad jitter
* from the accelerometer.
*/
static const float R_angle = 0.3;
/*
* Q is a 2x2 matrix that represents the process covariance noise.
* In this case, it indicates how much we trust the acceleromter
* relative to the gyros.
*/
static const float Q_angle = 0.001;
static const float Q_gyro = 0.003;
/*
* state_update is called every dt with a biased gyro measurement
* by the user of the module. It updates the current angle and
* rate estimate.
*
* The pitch gyro measurement should be scaled into real units, but * does not need any bias removal. The filter will track the bias.
*
* Our state vector is:
*
* X = [ angle, gyro_bias ]
*
* It runs the state estimation forward via the state functions:
*
* Xdot = [ angle_dot, gyro_bias_dot ]
*
* angle_dot = gyro - gyro_bias
* gyro_bias_dot = 0
*
* And updates the covariance matrix via the function:
*
* Pdot = A*P + P*A' + Q
*
* A is the Jacobian of Xdot with respect to the states:
*
* A = [ d(angle_dot)/d(angle) d(angle_dot)/d(gyro_bias) ]
* [ d(gyro_bias_dot)/d(angle) d(gyro_bias_dot)/d(gyro_bias) ]
*
* = [ 0 -1 ]
* [ 0 0 ]
*
* Due to the small CPU available on the microcontroller, we've
* hand optimized the C code to only compute the terms that are
* explicitly non-zero, as well as expanded out the matrix math
* to be done in as few steps as possible. This does make it harder
* to read, debug and extend, but also allows us to do this with
* very little CPU time.
*/
void state_update( const float q_m /* Pitch gyro measurement */)
{
/* Unbias our gyro */
const float q = q_m - q_bias;
/*
* Compute the derivative of the covariance matrix
*
* Pdot = A*P + P*A' + Q
*
* We've hand computed the expansion of A = [ 0 -1, 0 0 ] multiplied * by P and P multiplied by A' = [ 0 0, -1, 0 ]. This is then added
* to the diagonal elements of Q, which are Q_angle and Q_gyro.
*/
const float Pdot[2 * 2] =
{
Q_angle - P[0][1] - P[1][0], /* 0,0 */
- P[1][1], /* 0,1 */
- P[1][1], /* 1,0 */
Q_gyro /* 1,1 */
};
/* Store our unbias gyro estimate */
rate = q;
/*
* Update our angle estimate
* angle += angle_dot * dt
* += (gyro - gyro_bias) * dt
* += q * dt
*/
angle += q * dt;
/* Update the covariance matrix */
P[0][0] += Pdot[0] * dt;
P[0][1] += Pdot[1] * dt;
P[1][0] += Pdot[2] * dt;
P[1][1] += Pdot[3] * dt;
}
/*
* kalman_update is called by a user of the module when a new
* accelerometer measurement is available. ax_m and az_m do not * need to be scaled into actual units, but must be zeroed and have
* the same scale.
*
* This does not need to be called every time step, but can be if
* the accelerometer data are available at the same rate as the
* rate gyro measurement.
*
* For a two-axis accelerometer mounted perpendicular to the rotation * axis, we can compute the angle for the full 360 degree rotation
* with no linearization errors by using the arctangent of the two
* readings.
* * As commented in state_update, the math here is simplified to * make it possible to execute on a small microcontroller with no
* floating point unit. It will be hard to read the actual code and
* see what is happening, which is why there is this extensive
* comment block.
*
* The C matrix is a 1x2 (measurements x states) matrix that
* is the Jacobian matrix of the measurement value with respect
* to the states. In this case, C is:
*
* C = [ d(angle_m)/d(angle) d(angle_m)/d(gyro_bias) ]
* = [ 1 0 ]
*
* because the angle measurement directly corresponds to the angle * estimate and the angle measurement has no relation to the gyro
* bias.
*/
void kalman_update(
const float ax_m, /* X acceleration */
const float az_m /* Z acceleration */
)
{
/* Compute our measured angle and the error in our estimate */ const float angle_m = atan2( -az_m, ax_m );
const float angle_err = angle_m - angle;
/*
* C_0 shows how the state measurement directly relates to
* the state estimate.
*
* The C_1 shows that the state measurement does not relate
* to the gyro bias estimate. We don't actually use this, so
* we comment it out.
*/
const float C_0 = 1;
/* const float C_1 = 0; */
/*
* PCt<2,1> = P<2,2> * C'<2,1>, which we use twice. This makes * it worthwhile to precompute and store the two values.
* Note that C[0,1] = C_1 is zero, so we do not compute that
* term. */
const float PCt_0 = C_0 * P[0][0]; /* + C_1 * P[0][1] = 0 */
const float PCt_1 = C_0 * P[1][0]; /* + C_1 * P[1][1] = 0 */
/*
* Compute the error estimate. From the Kalman filter paper:
*
* E = C P C' + R
*
* Dimensionally,
*
* E<1,1> = C<1,2> P<2,2> C'<2,1> + R<1,1>
*
* Again, note that C_1 is zero, so we do not compute the term.
*/
const float E =
R_angle
+ C_0 * PCt_0
/* + C_1 * PCt_1 = 0 */
;
/*
* Compute the Kalman filter gains. From the Kalman paper:
*
* K = P C' inv(E)
*
* Dimensionally:
*
* K<2,1> = P<2,2> C'<2,1> inv(E)<1,1>
*
* Luckilly, E is <1,1>, so the inverse of E is just 1/E.
*/
const float K_0 = PCt_0 / E;
const float K_1 = PCt_1 / E;
/*
* Update covariance matrix. Again, from the Kalman filter paper: *
* P = P - K C P
*
* Dimensionally:
*
* P<2,2> -= K<2,1> C<1,2> P<2,2>
* * We first compute t<1,2> = C P. Note that:
*
* t[0,0] = C[0,0] * P[0,0] + C[0,1] * P[1,0]
*
* But, since C_1 is zero, we have:
*
* t[0,0] = C[0,0] * P[0,0] = PCt[0,0]
*
* This saves us a floating point multiply.
*/
const float t_0 = PCt_0; /* C_0 * P[0][0] + C_1 * P[1][0] */
const float t_1 = C_0 * P[0][1]; /* + C_1 * P[1][1] = 0 */
P[0][0] -= K_0 * t_0;
P[0][1] -= K_0 * t_1;
P[1][0] -= K_1 * t_0;
P[1][1] -= K_1 * t_1;
/*
* Update our state estimate. Again, from the Kalman paper:
*
* X += K * err
*
* And, dimensionally,
*
* X<2> = X<2> + K<2,1> * err<1,1>
*
* err is a measurement of the difference in the measured state
* and the estimate state. In our case, it is just the difference
* between the two accelerometer measured angle and our estimated * angle.
*/
angle += K_0 * angle_err;
q_bias += K_1 * angle_err;
}
卡尔曼滤波计算举例 ?计算举例 ?卡尔曼滤波器特性
假设有一个标量系统，信号与观测模型为 [1][][]x k ax k n k +=+[][][] z k x k w k =+其中a 为常数，n [k ]和w [k ]是不相关的零均值白噪声，方差分别为和。 系统的起始变量x [0]为随机变量，其均值为零，方差为。2n σ2 σ[0]x P （1）求估计x [k ]的卡尔曼滤波算法；（2）当时的卡尔曼滤波增益和滤波误差方差。 22 0.9,1,10,[0]10 n x a P =σ=σ==1. 计算举例
根据卡尔曼算法，预测方程为: ??[/1][1/1]x k k ax k k -=--预测误差方差为: 2 2 [/1][1/1]x x n P k k a P k k -=--+σ 卡尔曼增益为: () 1 22 22 22 [][/1][/1][1/1][1/1]x x x n x n K k P k k P k k a P k k a P k k -=--+σ --+σ=--+σ+σ ???[/][/1][]([][/1])??[1/1][]([][1/1])?(1[])[1/1][][]x k k x k k K k z k x k k ax k k K k z k ax k k a K k x k k K k z k =-+--=--+---=---+滤波方程:
()() 2 2222222 222 22 [/](1[])[/1] [1/1]1[1/1][1/1][1/1][1/1]x x x n x n x n x n x n P k k K k P k k a P k k a P k k a P k k a P k k a P k k =--??--+σ=---+σ ?--+σ+σ??σ--+σ = --+σ+σ 滤波误差方差 起始：?[0/0]0x =[0/0][0] x x P P =
一个应用实例详解卡尔曼滤波及其算法实现 标签：算法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
卡尔曼滤波简介及其算法实现代码 卡尔曼滤波算法实现代码（C，C＋＋分别实现） 卡尔曼滤波器简介 近来发现有些问题很多人都很感兴趣。所以在这里希望能尽自己能力跟大家讨论一些力所能及的算法。现在先讨论一下卡尔曼滤波器，如果时间和能力允许，我还希望能够写写其他的算法，例如遗传算法，傅立叶变换，数字滤波，神经网络，图像处理等等。 因为这里不能写复杂的数学公式，所以也只能形象的描述。希望如果哪位是这方面的专家，欢迎讨论更正。 卡尔曼滤波器– Kalman Filter 1．什么是卡尔曼滤波器 （What is the Kalman Filter?） 在学习卡尔曼滤波器之前，首先看看为什么叫“卡尔曼”。跟其他著名的理论（例如傅立叶变换，泰勒级数等等）一样，卡尔曼也是一个人的名字，而跟他们不同的是，他是个现代人！ 卡尔曼全名Rudolf Emil Kalman，匈牙利数学家，1930年出生于匈牙利首都布达佩斯。1953，1954年于麻省理工学院分别获得电机工程学士及硕士学位。1957年于哥伦比亚大学获得博士学位。我们现在要学习的卡尔曼滤波器，正是源于他的博士论文和1960年发表的论文《A New Approach to Linear Filtering and Prediction Problems》（线性滤波与预测问题的新方法）。如果对这编论文有兴趣，可以到这里的地址下载： https://www.doczj.com/doc/9310352147.html,/~welch/media/pdf/Kalman1960.pdf。 简单来说，卡尔曼滤波器是一个“optimal recursive data processing algorithm（最优化自回归数据处理算法）”。对于解决很大部分的问题，他是最优，效率最高甚至是最有用的。他的广泛应用已经超过30年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。近年来更被应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。 2．卡尔曼滤波器的介绍 （Introduction to the Kalman Filter） 为了可以更加容易的理解卡尔曼滤波器，这里会应用形象的描述方法来讲解，而不是像大多数参考书那样罗列一大堆的数学公式和数学符号。但是，他的5条公式是其核心内容。结合现代的计算机，其实卡尔曼的程序相当的简单，只要你理解了他的那5条公式。 在介绍他的5条公式之前，先让我们来根据下面的例子一步一步的探索。 假设我们要研究的对象是一个房间的温度。根据你的经验判断，这个房间的温度是恒定的，也就
文章编号:1008-8652(2005)01-001-004 联合卡尔曼滤波器在数据融合中的应用 胡宏灿1,2 郭 立1 朱俊株1 (1.中国科学技术大学 合肥 230026; 2.海军大连舰艇学院 大连 116018) 【摘要】 介绍多传感器数据融合中联合卡尔曼滤波器的设计步骤,并将此方法用于舰船组合导航系统,计算机仿真和理论分析表明,该滤波器可以做到全局最优,其结构遵循信息分配原则,提高了系统的数值稳定性和容错性,减小了数据传输的工作量与计算量,便于计算机实现,能够满足组合导航系统需要。 关键词:组合导航系统;数据融合;联合卡尔曼滤波 中图分类号:T P391.7 文献标识码:A The Application of Federal Kalman Filter in Data Fusion System Hu Ho ng can1,2 Guo Li1 Zhu Junzhu1 (1.University of Science and T echnology of China H ef ei230026; 2.Dalian N av al Vessels A cademy Dalian116018) Abstract:A new design o f Kalman filter based on data fusion is presented in the paper.Fistly,the fr ame Kalman filter is intr oduced.T hen,the algo rithm is given.T he simulatio n results show that the metho d is useful in integr ated navigation sy stem because it can impr ove accur acy and r eliability,and it has hig h fault-tolerant ability. Keywords:integ rated nav igatio n sy stem;data fusio n;feder al Kalman filter 1 引言 数据融合技术是近年来新兴的一门实践性较强的技术,它是对系统多个传感器的数据进行处理的过程。众所周知,由于任何传感器都有自身的不足之处,所以单一传感器具有误报风险大,可靠性和容错能力低等缺点。为了对测量环境或对象的特征有个全面、正确的认识,克服单一传感器的上述缺点,多传感器数据融合技术应运而生。简单的说,多传感器融合技术就是融合多个传感器的信息,以产生比单个传感器更可靠、更准确的信息。常用方法有贝叶斯估计法和DS证据理论法及经典推算法等,神经网络、小波分析等智能方法近年来也是研究数据融合的重要方法和手段。卡尔曼滤波器自上世纪六十年代被提出以后,作为一种新型的滤波手段在控制、跟踪、测量领域得到广泛应用。由于卡尔曼滤波器对数据的估计是无偏最优估计,滤波器结构简单等特点,使得卡尔曼滤波器在多传感器数据融合中应用极为广泛。过去使用的集中式卡尔曼滤波器要集中处理所有传感器的数据,计算量大,实时性差,并且不具备容错性。本文基于Car lson提出的联合卡尔曼滤波算法,介绍了利用信息分配原则实现多传感器信息最优融合的滤波器的设计,不仅使系统具备了一定的容错能力,实时性也有较大幅度的提高。最后给出了联合卡尔曼滤波器在舰船组合导航中的应用实例。 2 联合卡尔曼滤波器的设计步骤 联合卡尔曼滤波器的设计主要围绕两个方面,第一是对数据进行分散处理,第二是分散处理过的数据X收稿日期:2004-11-26
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
卡尔曼滤波算法实现代码 C++实现代码如下： ============================kalman.h================= =============== // kalman.h: interface for the kalman class. // ////////////////////////////////////////////////////////////////////// #if !defined(AFX_KALMAN_H__ED3D740F_01D2_4616_8B74_8BF57636F2C0__IN CLUDED_) #define AFX_KALMAN_H__ED3D740F_01D2_4616_8B74_8BF57636F2C0__INCLU DED_ #if _MSC_VER > 1000 #pragma once #endif// _MSC_VER > 1000 #include
kalman(int x=0,int xv=0,int y=0,int yv=0); //virtual ~kalman(); }; #endif// !defined(AFX_KALMAN_H__ED3D740F_01D2_4616_8B74_8BF57636F2C 0__INCLUDED_) ============================kalman.cpp=============== ================= #include "kalman.h" #include
/********************************************************* // 卡尔曼滤波 //********************************************************* //在程序中利用Angle+=(Gyro - Q_bias) * dt计算出陀螺仪积分出的角度，其中Q_bias是陀螺仪偏差。 //此时利用陀螺仪积分求出的Angle相当于系统的估计值，得到系统的观测方程；而加速度计检测的角度Accel相当于系统中的测量值，得到系统状态方程。 //程序中Q_angle和Q_gyro分别表示系统对加速度计及陀螺仪的信任度。根据Pdot = A*P + P*A' + Q_angle计算出先验估计协方差的微分，用于将当前估计值进行线性化处理。其中A 为雅克比矩阵。 //随后计算系统预测角度的协方差矩阵P。计算估计值Accel与预测值Angle间的误差Angle_err。 //计算卡尔曼增益K_0,K_1，K_0用于最优估计值，K_1用于计算最优估计值的偏差并更新协方差矩阵P。 //通过卡尔曼增益计算出最优估计值Angle及预测值偏差Q_bias，此时得到最优角度值Angle 及角度值。 //Kalman滤波，20MHz的处理时间约0.77ms； void Kalman_Filter(float Accel,float Gyro) { Angle+=(Gyro - Q_bias) * dt; //先验估计 Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先验估计误差协方差的微分 Pdot[1]=- PP[1][1]; Pdot[2]=- PP[1][1]; Pdot[3]=Q_gyro; PP[0][0] += Pdot[0] * dt; // Pk-先验估计误差协方差微分的积分 PP[0][1] += Pdot[1] * dt; // =先验估计误差协方差 PP[1][0] += Pdot[2] * dt; PP[1][1] += Pdot[3] * dt; Angle_err = Accel - Angle; //zk-先验估计 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;
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
卡尔曼滤波的原理说明 2009年10月23日星期五 01:19 在学习卡尔曼滤波器之前，首先看看为什么叫“卡尔曼”。跟其他著名的理论（例如傅立叶变换，泰勒级数等等）一样，卡尔曼也是一个人的名字，而跟他们不同的是，他是个现代人！ 卡尔曼全名Rudolf Emil Kalman，匈牙利数学家，1930年出生于匈牙利首都布达佩斯。1953，1954年于麻省理工学院分别获得电机工程学士及硕士学位。1957年于哥伦比亚大学获得博士学位。我们现在要学习的卡尔曼滤波器，正是源于他的博士论文和1960年发表的论文《A New Approach to Linear Filtering and Prediction Problems》（线性滤波与预测问题的新方法）。如果对这编论文有兴趣，可以到这里的地址下 载：.edu/~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），也就是这些偏差跟前后时间是没有关系的而且符合高斯分配（Gaussian Distribution）。另外，我们在房间里放一个温度计，但是这个温度计也不准确的，测量值会比实际值偏差。我们也把这些偏差看成是高斯白噪声。 好了，现在对于某一分钟我们有两个有关于该房间的温度值：你根据经验的预测值（系统的预测值）和温度计的值（测量值）。下面我们要用这两个值结合他们各自的噪声来估算出房间的实际温度值。 假如我们要估算k时刻的是实际温度值。首先你要根据k-1时刻的温度值，来预测k时刻的温度。因为你相信温度是恒定的，所以你会得到k时刻的温度预测值
具体步骤分述如下 1、确定系统的模型 根据对系统的充分了解,建立一个真实系统的完整模型,并用状态空间描述之。这里包括选择状态变量,观察量,建立系统的动力方程和观察方程,以及建立误差的统计模型。同时建立地面计算机模拟试验用的“模拟器”。这些“模拟器”实质上是一套计算机程序。它模拟了噪声发生,传感器信息产生 及传递过程以及研究对象的运动等等。模拟器是滤波器模拟分析的工具和鉴别标准。2、建立完整滤波器及模拟试验 根据系统的完整模型建立一个最佳的完整滤波器。它包括了所有的误差源。其维数一般较高。完整滤波器用来反映一个精确工作的最佳滤波器性能,并作为鉴定简化滤波器的标准。同时建立一个地面计算机模拟分析程序工,对完整滤波器进行模拟鉴定。这种程序包括了详细的模拟器,并模拟了完整滤波器方程。模拟目的是鉴定一个精确工作滤波器所能达到的理论精度,当然它应该超过系统所希望的精度,否则就没有必要继续进行设计了。 3、建立简化滤波器及模拟试验 这项工作主要是简化系统。系统的完整模型一般比较复杂,完整滤波器的维数较高。例如,飞机导航方程可达、个变量。因此运算要求较高。实际应用中必须简化模型。先根据工程经验简化模型,设计出相应的简化滤波器,然后作理论上的模型误差分析,但更重要的是通过计算机模拟分析来完成设计和鉴定。这里同样要借助于地面计算机模拟分析程序。程序既包括了多种模拟器,反映了真实系统,又能方便地模拟简化滤波器方程。通过程序鉴定分析简化滤波器,并与完整滤波器结果作比机一边模拟分析,一边删去对总系统影响不大的状态量,最后完成了一个维数较少且能满足性能要求的简化滤波器,这阶段的工作反映了一个不完整滤波器在精确运算时的理论精度,它至少要达到系统所希望的精度。 4、建立确定性滤波器及模拟试验 这项工作是建立一个能在实际工作环境下实时完成系统任务的确定性滤波器。建立过程中要用各种滤波技术,使得滤波器对传感器误差恶化不灵敏,并能符合计算机实时要求、容量要求以及精度限制,而又能满足系统性能的要求。建立确定性滤波器,先是根据工程经验作理论上的设计和分析,而更重要的是利用了地面计算机模拟分析程序。
多传感器融合中的卡尔曼滤波探讨 1 引言 目前靠单一的信息源很难保证获取环境信息的快速性和准确性的要求，会给系统对周围环境的理解及系统的决策带来影响，另外，单一传感器获得的仅仅是环境特征的局部、片面的信息，它的信息量是十分有限的。而且每个传感器采集到的信息还受到自身品质、性能噪声的影响，采集到的信息往往是不完整的，带有较大的不确定性，偶尔甚至是错误的。而且在传统方式中，各传感器采集的信息单独、孤立的进行加工处理，不仅会导致处理工作量增加，而且割断了各传感器信息的联系丢失了信息的有机组合蕴涵的信息特征，也造成信息资 源的浪费[3-7]。在运动控制系统中，传统上就往往将速度传感器测量到的速度和加速度计测量到的加速度进行单独处理，没有将两者的信息进行数据融合。由物理定律可知，加速度与速度成导数关系，所以两者的数据是存在内在联系的，完全可以根据信息融合理论对两者数据进行综合处理，从而得到更加准确的结果。卡尔曼滤波器是常用的一种数据融合技术，它利用迭代递推计算的方式，对存贮空间要求很小，适合于存贮空间和计算速度受限的场合 [1,2]。本文分析了数度传感器和加速度计各自的优缺点，给出了一种应用卡尔曼滤波器原理对两者进行数据融合的方法。 2 传感器简介 2.1 光电编码器 光电编码器通常用于角度、位移、或转速测量，通过对光脉冲的个数进行计数再经过计算而得到测量值。假设在周长为L 的圆盘上有M 个过光孔，离散系统中，在周期时间T 内对脉冲进行计数值为N ，则第k 次测量的线速度v 可表达为 MT k Le k v MT k Le MT k LN k e k N MT L k v )()()()())()(()(+=+=+=∧ (2-1) e 是随机误差，为光脉冲取整后的剩余值，取值范围为（－1， 1），可看作均匀分布。∧v 为实际的观测值，与真值v 之间相差MT Le 。可见，在固定长度的L 上，加大M 或T 的值，都可以减小误差。但是加大M 需要付出昂贵的成本，使传感器价格大幅提高，如光栅式光电传感器；而加大T 又会降低系统的动态响应性能，所以在实际应用中，这两者均难如愿。 在需要同时测量加速度的场合，理论上可以由对速度求差分方程得出，即 2 2))1(()(())1()(()1()()(MT k e k Le MT k N k N L T k v k v k a --+--=--= (2-2) 容易看出，相对误差显著提高，数据几乎不可用，所以需要专门的加速度计对加速度进行测量。 2.2 加速度计 加速度计用于测量物体的线性加速度，根据不同的测量原理，有很多种类，本文中使用的MMA7260是一款低成本、低功耗、小体积、功能完善的单芯片加速度计，主要用于运动检测、惯性导航、震动检测、交通安全等。MMA7260响应快、带宽可调整、可响应高频率输入，但是其测量数据噪声与带宽的平方根成正比，会随着带宽增加而增加。 5.1350)(?=BW g rms Noise μ (2-3) 式中BW 为传感器带宽(HZ)。因此在设计时，首先要确定被测加速度的频率范围，然后再
第十三章 卡尔曼滤波 在本章中，我们介绍一种被称为卡尔曼滤波的十分有用的工具。卡尔曼滤波的基本思想是将动态系统表示成为一种称为状态空间表示的特殊情形。卡尔曼滤波是对系统线性投影进行序列更新的算法。除了一般的优点以外，这种算法对计算确切的有限样本预测、计算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就可以了。
卡尔曼滤波简介及其算法实现代码（C++/C/MATLAB） 卡尔曼滤波器简介 近来发现有些问题很多人都很感兴趣。所以在这里希望能尽自己能力跟大家讨论一些力所能及的算法。现在先讨论一下卡尔曼滤波器，如果时间和能力允许，我还希望能够写写其他的算法，例如遗传算法，傅立叶变换，数字滤波，神经网络，图像处理等等。 因为这里不能写复杂的数学公式，所以也只能形象的描述。希望如果哪位是这方面的专家，欢迎讨论更正。 卡尔曼滤波器– Kalman Filter 1．什么是卡尔曼滤波器 （What is the Kalman Filter?） 在学习卡尔曼滤波器之前，首先看看为什么叫“卡尔曼”。跟其他著名的理论（例如傅立叶变换，泰勒级数等等）一样，卡尔曼也是一个人的名字，而跟他们不同的是，他是个现代人！ 卡尔曼全名Rudolf Emil Kalman，匈牙利数学家，1930年出生于匈牙利首都布达佩斯。1953，1954年于麻省理工学院分别获得电机工程学士及硕士学位。1957年于哥伦比亚大学获得博士学位。我们现在要学习的卡尔曼滤波器，正是源于他的博士论文和1960年发表的论文《A New Approach to Linear Filtering and Prediction Problems》（线性滤波与预测问题的新方法）。如果对这编论文有兴趣，可以到这里的地址下载： https://www.doczj.com/doc/9310352147.html,/~welch/media/pdf/Kalman1960.pdf。 简单来说，卡尔曼滤波器是一个“optimal recursive data processing algorithm（最优化自回归数据处理算法）”。对于解决很大部分的问题，他是最优，效率最高甚至是最有用的。他的广泛应用已经超过30年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。近年来更被应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。 2．卡尔曼滤波器的介绍 （Introduction to the Kalman Filter） 为了可以更加容易的理解卡尔曼滤波器，这里会应用形象的描述方法来讲解，而不是像大多数参考书那样罗列一大堆的数学公式和数学符号。但是，他的5 条公式是其核心内容。结合现代的计算机，其实卡尔曼的程序相当的简单，只要你理解了他的那5条公式。 在介绍他的5条公式之前，先让我们来根据下面的例子一步一步的探索。
卡尔曼滤波的基本原理及应用卡尔曼滤波在信号处理与系统控制领域应用广泛，目前，正越来越广泛地应用于计算机应用的各个领域。为了更好地理解卡尔曼滤波的原理与进行滤波算法的设计工作，主要从两方面对卡尔曼滤波进行阐述：基本卡尔曼滤波系统模型、滤波模型的建立以及非线性卡尔曼滤波的线性化。最后，对卡尔曼滤波的应用做了简单介绍。 卡尔曼滤波属于一种软件滤波方法，其基本思想是：以最小均方误差为最佳估计准则，采用信号与噪声的状态空间模型，利用前一时刻的估计值和当前时刻的观测值来更新对状态变量的估计，求出当前时刻的估计值，算法根据建立的系统方程和观测方程对需要处理的信号做出满足最小均方误差的估计。 最初的卡尔曼滤波算法被称为基本卡尔曼滤波算法，适用于解决随机线性离散系统的状态或参数估计问题。卡尔曼滤波器包括两个主要过程：预估与校正。预估过程主要是利用时间更新方程建立对当前状态的先验估计，及时向前推算当前状态变量和误差协方差估计的值，以便为下一个时间状态构造先验估计值；校正过程负责反馈，利用测量更新方程在预估过程的先验估计值及当前测量变量的基础上建立起对当前状态的改进的后验估计。这样的一个过程，我们称之为预估－校正过程，对应的这种估计算法称为预估－校正算法。以下给出离散卡尔曼滤波的时间更新方程和状态更新方程。 时间更新方程： 状态更新方程： 在上面式中，各量说明如下： A：作用在X k-1上的n×n 状态变换矩阵 B：作用在控制向量U k-1上的n×1 输入控制矩阵 H：m×n 观测模型矩阵，它把真实状态空间映射成观测空间 P k－：为n×n 先验估计误差协方差矩阵 P k：为n×n 后验估计误差协方差矩阵 Q：n×n 过程噪声协方差矩阵 R：m×m 过程噪声协方差矩阵 I：n×n 阶单位矩阵K k：n×m 阶矩阵，称为卡尔曼增益或混合因数 随着卡尔曼滤波理论的发展，一些实用卡尔曼滤波技术被提出来，如自适应滤波，次优滤波以及滤波发散抑制技术等逐渐得到广泛应用。其它的滤波理论也迅速发展，如线性离散系统的分解滤波（信息平方根滤波，序列平方根滤波，UD 分解滤波），鲁棒滤波（H∞波）。 非线性样条自适应滤波：这是一类新的非线性自适应滤波器，它由一个线性组合器后跟挠性无记忆功能的。涉及的自适应处理的非线性函数是基于可在学习