卡尔曼
卡尔曼滤波 公式
卡尔曼滤波公式
卡尔曼滤波是一种用于估计状态变量的数学算法，广泛应用于各种领域，如航空航天、无人驾驶、机器人等。
以下是卡尔曼滤波的公式：
1.状态预测方程：
x[k|k-1] = A[k|k-1] * x[k-1|k-1] + B[k|k-1] * u[k]
其中，x[k|k-1]表示在时间k对时间k-1的状态预测，A[k|k-1]是状态转移矩阵，B[k|k-1]是控制矩阵，u[k]是控制向量。
2.测量更新方程：
z[k|k] = H[k|k] * x[k|k] + v[k]
其中，z[k|k]表示在时间k对时间k的测量更新，H[k|k]是量测矩阵，v[k]是测量噪声。
3.协方差预测方程：
P[k|k-1] = A[k|k-1] * P[k-1|k-1] * A[k|k-1]' + Q
其中，P[k|k-1]表示在时间k对时间k-1的协方差预测，Q是过程噪声协方差。
4.协方差更新方程：
P[k|k] = (I - K[k] * H[k|k]) * P[k|k-1]
其中，P[k|k]表示在时间k对时间k的协方差更新，K[k]是卡尔曼增益矩阵。
5.卡尔曼增益计算：
K[k] = P[k|k-1] * H[k|k]' / (H[k|k] * P[k|k-1] * H[k|k]' + R)
其中，R是测量噪声协方差。
卡尔曼滤波的融合原理
卡尔曼滤波的融合原理
卡尔曼滤波（Kalman Filter）是一种基于贝叶斯估计理论的递归最优线性最小方差滤波器，它在信号处理和控制工程领域中广泛应用，尤其擅长于多传感器数据融合以及动态系统的状态估计。
其融合原理可以简化表述如下：
1.预测阶段：
1.利用系统的动态模型，根据上一时刻的状态估计值及其协方差矩
阵，结合当前时刻的系统输入（如果有），通过状态转移方程预测下一时刻的状态和相应的预测误差协方差矩阵。
2.更新阶段：
1.当新的观测数据可用时，通过观测模型计算出一个预测与实际观测
之间的残差（即所谓的卡尔曼增益K）。
2.卡尔曼增益是基于预测误差协方差和观测噪声的协方差之比确定
的，它反映了对预测的信任度和对观测的信任度的相对权重。
3.使用这个增益来调整预测状态，得到一个更加准确的状态估计，也
就是将预测结果与实际测量值进行加权融合。
4.同时更新后验状态误差协方差矩阵，以反映新信息被融合后的不确
定性。
整个过程的关键在于如何最优地结合来自系统动力学模型预测的信息（先验信息）与从传感器获取的实时观测信息（后验信息）。
由于假定噪声项服从高斯分布，卡尔曼滤波能够找到一种数学上的最优解，使得状态估计具有最小均方误差。
在实际应用中，这种融合方法非常强大且灵活，可以处理连续时间或离散时间的线性系统，对于非线性系统则可通过扩展如扩展卡尔曼滤波等方法来近似处理。
卡尔曼
比如，在雷达中，人们感兴趣的是跟踪目标，但目标的位置、速度、加速度的测量值往往在任何时候都有噪 声。卡尔曼滤波利用目标的动态信息,设法去掉噪声的影响，得到一个关于目标位置的好的估计。这个估计可以是 对当前目标位置的估计(滤波)，也可以是对于将来位置的估计(预测)，也可以是对过去位置的估计(插值或平滑)。
贡献
卡尔曼提出了一种现称为卡尔曼滤波的新的滤波方法和能控性、能观性的概念，为20世纪50年代末至60年代 初发展起来的现代控制理论作出了杰出贡献。他的工作直接针对着科学地理解现代工程中的创新过程(如已经知道 的控制、计算机和信息等组织)。他的方法着重于数学概念，这种抽象方法对工程的实用价值，已为1963年美国 宇宙飞船在卡尔曼滤波器导引下登上月球所证实。现在卡尔曼滤波器已被广泛地应用到时间序列分析、动态系统 辨识、水文学及流体动力学，甚至经济领域。在卡尔曼之前，已有柯尔莫哥洛夫和维纳的统计滤波，目的是要根 据过去的信息或事件预报未来。他们是从计算线性滤波器的随机输入输出函数协方差来达到这一目标的。维纳用 到了维纳-霍普夫方程，而柯尔莫哥洛夫使用了比希尔伯特空间更抽象的东西。因理论和计算都很困难，没能产生 重要的实际应用。1959年，卡尔曼重新表述了这一问题，引入了两个新的原理：只有在未来依赖于动态系统内对 过去的贮存的情况下，预报才是可能的;预报器必须模拟它所预报的过程，所以预报器本身必须是一个动态系统。 利用微分方程领域的新知识，他假定了要预报的动态过程已知但由于噪声的作用而模糊不清，并由此计算得到了 最优滤波器的显式描述。因此卡尔曼滤波器不仅是在输入-输出意义上，而且是按运动方程的意义上给出的。稍后， 布西利用和卡尔曼类似的假设得到了维纳-霍普夫方程的一个解，因此对线性系统的滤波方程又称为卡尔曼-布西 滤波器。卡尔曼在对数学系统理论的研究中，提出了能控性的概念。在一个常系数线性常微分方程x=Ax+Bu(其中 x为状态变量，u为控制向量)中完全能控当且仅当rank(B，AB，…)=dimx。他在1957年提出的这一简单的判别准 则，解释了构造控制系统的所有直觉工程方法成功的原因。这一概念大大简化了控制系统的研究，并提供了有效 手段;它在最优控制中起着重要作用。
卡尔曼滤波算法基本原理
卡尔曼滤波算法基本原理一、概述卡尔曼滤波算法是一种基于线性系统状态空间模型的递归滤波算法，主要用于估计含有噪声的测量数据，并能够有效地消除噪声对估计的影响，提高估计精度。
本篇文章将详细介绍卡尔曼滤波算法的基本原理。
二、基本原理1.状态方程：卡尔曼滤波算法基于线性系统状态空间模型，该模型可以用状态方程来表示。
状态方程通常包含系统的内部状态、输入和输出，可以用数学公式表示为：x(t+1)=Ax(t)+Bu(t)+w(t)。
其中，x(t)表示系统内部状态，u(t)表示输入，w(t)表示测量噪声。
2.测量方程：测量数据通常受到噪声的影响，卡尔曼滤波算法通过建立测量方程来处理噪声数据。
测量方程通常表示为：z(t)=h(x(t))+v(t)，其中z(t)表示测量数据，h(x(t))表示系统输出，v(t)表示测量噪声。
3.卡尔曼滤波算法：卡尔曼滤波算法通过递归的方式，根据历史状态和测量数据来估计当前系统的内部状态。
算法的核心是利用过去的估计误差和测量误差来预测当前的状态，并不断更新估计值，以达到最优估计的效果。
卡尔曼滤波算法主要包括预测和更新两个步骤。
预测步骤根据状态方程和上一步的估计值，预测当前的状态；更新步骤则根据当前的测量数据和预测值，以及系统协方差矩阵，来更新当前状态的估计值和系统协方差矩阵。
4.滤波器的选择：在实际应用中，需要根据系统的特性和噪声的性质来选择合适的卡尔曼滤波器。
常见的滤波器有标准卡尔曼滤波器、扩展卡尔曼滤波器等。
选择合适的滤波器可以提高估计精度，降低误差。
三、应用场景卡尔曼滤波算法在许多领域都有应用，如航空航天、自动驾驶、机器人控制等。
在上述领域中，由于系统复杂、噪声干扰大，使用卡尔曼滤波算法可以有效地提高系统的估计精度和控制效果。
四、总结卡尔曼滤波算法是一种基于线性系统状态空间模型的递归滤波算法，通过预测和更新的方式，能够有效地消除噪声对估计的影响，提高估计精度。
本篇文章详细介绍了卡尔曼滤波算法的基本原理和应用场景，希望能对大家有所帮助。
卡尔曼滤波_卡尔曼算法
卡尔曼滤波_卡尔曼算法1.引言1.1 概述卡尔曼滤波是一种用于估计系统状态的技术，通过融合传感器测量值和系统模型的预测值，提供对系统状态的最优估计。
它的应用十分广泛，特别在导航、图像处理、机器人技术等领域中发挥着重要作用。
在现实世界中，我们往往面临着各种噪声和不确定性，这些因素会影响我们对系统状态的准确估计。
卡尔曼滤波通过动态调整系统状态的估计值，可以有效地抑制这些干扰，提供更加精确的系统状态估计。
卡尔曼滤波的核心思想是利用系统模型的预测和传感器测量值之间的线性组合，来计算系统状态的最优估计。
通过动态地更新状态估计值，卡尔曼滤波可以在对系统状态的准确估计和对传感器测量值的实时响应之间进行平衡。
卡尔曼滤波算法包括两个主要步骤：预测和更新。
在预测步骤中，通过系统模型和上一时刻的状态估计值，预测当前时刻的系统状态。
在更新步骤中，将传感器测量值与预测值进行比较，然后根据测量误差和系统不确定性的权重，计算系统状态的最优估计。
卡尔曼滤波具有很多优点，例如它对传感器噪声和系统模型误差具有鲁棒性，可以提供较为稳定的估计结果。
此外，卡尔曼滤波还可以有效地处理缺失数据和不完全的测量信息，具有较高的自适应性和实时性。
尽管卡尔曼滤波在理论上具有较好的性能，但实际应用中还需考虑诸如系统模型的准确性、测量噪声的特性等因素。
因此，在具体应用中需要根据实际情况进行算法参数的调整和优化，以提高估计的准确性和可靠性。
通过深入理解卡尔曼滤波的原理和应用，我们可以更好地应对复杂环境下的估计问题，从而在实际工程中取得更好的效果。
本文将介绍卡尔曼滤波的基本原理和算法步骤，以及其在不同领域的应用案例。
希望通过本文的阅读，读者们可以对卡尔曼滤波有一个全面的了解，并能够在实际工程中灵活运用。
1.2文章结构文章结构部分的内容可以按照以下方式编写：1.2 文章结构本文将围绕卡尔曼滤波和卡尔曼算法展开论述。
首先，我们会在引言部分对卡尔曼滤波和卡尔曼算法进行简要概述，介绍其基本原理和应用领域。
卡尔曼滤波收敛
卡尔曼滤波收敛摘要：1.卡尔曼滤波的基本原理2.卡尔曼滤波的收敛性证明3.卡尔曼滤波在实际应用中的优势4.卡尔曼滤波的局限性及改进方向正文：一、卡尔曼滤波的基本原理卡尔曼滤波是一种线性高斯状态空间模型，主要用于估计系统状态和优化控制策略。
它通过将预测状态量的高斯分布和观测量的高斯分布进行融合，生成一个新的高斯分布，从而实现对系统状态的估计。
卡尔曼滤波主要包括五个步骤：预测、校正、更新、观测和修正。
预测步骤用于预测系统的状态，校正步骤用于根据测量值修正预测结果，更新步骤用于更新状态估计值，观测步骤用于观测系统状态，修正步骤用于根据观测结果修正状态估计值。
二、卡尔曼滤波的收敛性证明卡尔曼滤波的收敛性可以通过数学证明来阐述。
假设系统状态满足线性高斯状态空间模型，并且观测噪声和过程噪声都满足正态分布。
则卡尔曼滤波可以得到如下状态估计方程：x_hat = A^T * P * A * x + A^T * P * C * z其中，x_hat 表示状态估计值，P 表示状态协方差矩阵，A 表示系统状态转移矩阵，C 表示观测矩阵，z 表示观测值。
可以看出，卡尔曼滤波得到的状态估计值是观测值和预测值的加权平均，权重分别为卡尔曼增益和观测噪声方差。
由于卡尔曼增益和观测噪声方差都是正数，因此状态估计值会随着观测值的增加而逐渐趋近于真实值，即卡尔曼滤波具有收敛性。
三、卡尔曼滤波在实际应用中的优势卡尔曼滤波在实际应用中具有很多优势，主要体现在以下几个方面：1.高精度：卡尔曼滤波可以有效地融合预测和观测信息，提高状态估计的精度。
2.实时性：卡尔曼滤波可以在实时测量观测值的情况下进行状态估计，适用于动态系统的实时控制。
3.鲁棒性：卡尔曼滤波对噪声具有较强的鲁棒性，即使在噪声较大的情况下，仍然可以得到较为准确的状态估计结果。
4.适用性广泛：卡尔曼滤波适用于线性高斯状态空间模型，可以应用于各种领域的问题，如导航、定位、机器人控制等。
卡尔曼增益系数
卡尔曼增益系数
卡尔曼增益系数是卡尔曼滤波器中的一个重要参数，它决定了滤波器的性能和稳定性。
卡尔曼增益系数的大小直接影响到滤波器的收敛速度和估计精度。
卡尔曼增益系数是通过卡尔曼滤波器的递推公式计算得出的，其大小取决于系统模型的精度和测量数据的可靠性。
如果系统模型的精度较高，测量数据的可靠性较强，那么卡尔曼增益系数就会比较大，从而使得滤波器的收敛速度更快，估计精度更高。
反之，如果系统模型的精度较低，测量数据的可靠性较弱，那么卡尔曼增益系数就会比较小，从而使得滤波器的收敛速度较慢，估计精度较低。
在卡尔曼滤波器的应用中，需要根据具体问题选择合适的卡尔曼增益系数。
如果卡尔曼增益系数选择不当，可能会导致滤波器的性能下降或者不稳定。
因此，在实际应用中，需要根据系统的特性和数据的特点进行反复试验和调整，以选择最优的卡尔曼增益系数。
另外，除了卡尔曼增益系数之外，卡尔曼滤波器的性能还受到其他因素的影响，如系统模型的精度、测量数据的可靠性、初始状态的估计等。
因此，在应用卡尔曼滤波器时，还需要综合考虑各种因素，以获得最优的估计结果。
总之，卡尔曼增益系数是卡尔曼滤波器中的一个关键参数，其选择直接影响到滤波器的性能和稳定性。
在实际应用中，需要根据具体问题选择最优的卡尔曼增益系数，并综合考虑各种因素以获得最优的估计结果。
卡尔曼滤波原理
卡尔曼滤波原理卡尔曼滤波（Kalman Filtering）是一种用于估计、预测和控制的最优滤波方法，由美国籍匈牙利裔数学家卡尔曼（Rudolf E. Kalman）在1960年提出。
卡尔曼滤波是一种递归滤波算法，通过对测量数据和系统模型的融合，可以得到更准确、更可靠的估计结果。
在各种应用领域，如导航、机器人、航空航天、金融等，卡尔曼滤波都被广泛应用。
1. 卡尔曼滤波的基本原理卡尔曼滤波的基本原理是基于状态空间模型，将系统的状态用随机变量来表示。
它假设系统的状态满足线性高斯模型，并通过线性动态方程和线性测量方程描述系统的演化过程和测量过程。
具体而言，卡尔曼滤波算法基于以下两个基本步骤进行：1.1 预测步骤：通过系统的动态方程预测当前时刻的状态，并计算预测的状态协方差矩阵。
预测步骤主要是利用前一时刻的状态和控制输入来预测当前时刻的状态。
1.2 更新步骤：通过系统的测量方程，将预测的状态与实际测量值进行融合，得到最优估计的状态和状态协方差矩阵。
更新步骤主要是利用当前时刻的测量值来修正预测的状态。
通过不断迭代进行预测和更新，可以得到连续时间上的状态估计值，并获得最优的估计结果。
2. 卡尔曼滤波的优势卡尔曼滤波具有以下几个优势：2.1 适用于线性系统与高斯噪声：卡尔曼滤波是一种基于线性高斯模型的滤波方法，对于满足这些条件的系统，卡尔曼滤波能够给出最优的估计结果。
2.2 递归计算：卡尔曼滤波是一种递归滤波算法，可以在每个时刻根据当前的测量值和先前的估计结果进行迭代计算，不需要保存过多的历史数据。
2.3 最优性：卡尔曼滤波可以通过最小均方误差准则，给出能够最优估计系统状态的解。
2.4 实时性：由于卡尔曼滤波的递归计算特性，它可以实时地处理数据，并及时根据新的测量值进行估计。
3. 卡尔曼滤波的应用卡尔曼滤波在多个领域都有广泛的应用，以下是一些典型的应用例子：3.1 导航系统：卡尔曼滤波可以用于导航系统中的位置和速度估计，可以结合地面测量值和惯性测量传感器的数据，提供精确的导航信息。
卡尔曼滤波器的优缺点
卡尔曼滤波器的优点主要包括：适用于线性系统：卡尔曼滤波器特别适用于线性系统的状态估计，因为它的递归算法能够在线性系统中实现最优估计。
计算效率高：卡尔曼滤波器在估计过程中不需要存储所有的数据，只需要当前和前一时刻的状态，因此计算效率较高。
适用于多维数据：卡尔曼滤波器可以扩展到多维状态空间，因此可以用于处理多传感器、多目标跟踪等问题。
然而，卡尔曼滤波器也存在一些局限性：要求系统具有线性特性：卡尔曼滤波器要求系统具有线性特性，对于非线性系统，需要采用扩展卡尔曼滤波器等改进方法，但这些方法精度和稳定性可能受到影响。
对初值和参数敏感：卡尔曼滤波器的估计结果对初值和参数的选择非常敏感，如果初值或参数选择不当，可能会导致估计结果不稳定或不准确。
对噪声模型的要求：卡尔曼滤波器要求噪声服从高斯分布，如果噪声不服从高斯分布，可能会导致估计结果失真。
对系统动态模型的要求：卡尔曼滤波器要求系统动态模型是已知的，并且是准确的，如果模型不准确或存在误差，可能会导致估计结果不准确。
卡尔曼滤波 粗糙度
卡尔曼滤波粗糙度
卡尔曼滤波（Kalman Filter）是一种用于估计动态系统状态的算法，广泛应用于工程和科学领域。
它通过对系统的观测数据进行递归处理，估计出系统的状态，并最小化估计误差。
在涉及粗糙度的应用中，卡尔曼滤波可以用于对含有噪声或不确定性的数据进行处理，以提取出有用的信息。
例如，在机械工程中，可以使用卡尔曼滤波来估计机械部件的运动状态，同时考虑到测量噪声和系统误差的影响。
卡尔曼滤波的核心思想是基于线性系统的状态空间模型，通过预测和更新两个步骤来估计系统的状态。
预测步骤使用系统的运动模型来预测下一时刻的状态，而更新步骤则结合观测数据对预测的状态进行修正，以提高估计的准确性。
在处理粗糙度相关的数据时，卡尔曼滤波可以帮助减少噪声和异常值的影响，从而提供更可靠的估计结果。
它可以应用于各种领域，如传感器数据融合、导航系统、控制工程等。
需要注意的是，卡尔曼滤波的有效性取决于对系统模型和噪声特性的准确了解。
在实际应用中，需要对系统进行建模和参数辨识，以确保卡尔曼滤波的性能和准确性。
总的来说，卡尔曼滤波是一种强大的工具，可用于处理含有粗糙度的数据，提供对系统状态的准确估计。
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
1.为什么要采用卡尔曼滤波器，它的实现目标是什么？
答：卡尔曼滤波器是一个“optimal recursive data processing algorithm（最优化自回归数据处理算法）”。
对于解决很大部分的问题，他是最优，效率最高甚至是最有用的。
他的广泛应用已经超过30年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。
近年来更被应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。
卡尔曼滤波器是一个递归的估计，即只要获知上一时刻的状态估计和当前状态的观测就可以计算出当前状态的估计，不同于其他的估计技术，卡尔曼滤波器不需要观测或/和估计的历史记录，卡尔曼滤波器是一个纯粹的时域滤波器，而不像低通滤波器等频域滤波器那样，需要在频域中设计，然后转换到时域中应用。
卡尔曼滤波器包括两个阶段：预测和更新；在估计阶段，滤波器应用上一状态的估计做出对当前状态的估计。
在更新阶段，滤波器利用在当前状态的观测值优化预测阶段的预测值，以获的一个更精确的当前状态的估计。
在小车的应用之中，我必须要知道这一时刻的准确的角度值，加速度计可以得到它的角度，但是小车在运行过程中，会得到一个很大的噪声，会让主控器误判，使小车站立不起来。
陀螺仪得到的是角速度，对角速度进行积分就可以得到角度，但是用陀螺仪测量角度，如果角速度信号存在微小的偏差和漂移，经过积分运算之后，变化形成积累误差。
这个误差会随着时间延长逐步增
加，最终导致电路饱和，无法形成正确的角度信号。
对角度的测量中，我采用加速度计和陀螺仪数据融合的方法测量它的实时角度。
在这里，我用卡尔曼滤波器对他们的数据进行融合及滤波过程。
卡尔曼滤波器是一个递归的估计，即只要获知上一时刻的状态估计和当前状态的观测就可以计算出当前状态的估计。
所以，通过卡尔曼滤波器可以得到很准确角度值。
卡尔曼滤波器的另一个好处就是角度值变化过渡很平滑，不会发生突变。
对小车的控制很有好处，平稳性好。
在用卡尔曼滤波器，还要解决的另一个问题就是实时性要高，我解决的办法就是，减少卡尔曼处理一次的时间，提高它处理的速度。
所以，使用卡尔曼滤波器的最终目标是准确性要高，曲线过渡要平滑，实时性要强。
只要满足这三点，要小车立起来不难。
2.卡尔曼滤波及PID控制图。
3.卡尔曼滤波器公式，并加以说明。
答：卡尔曼滤波器中有五个基本公式，在这里我不加以说明了，
有兴趣的人可以阅读相关资料。
在本文中，我讲解一下我通过卡尔曼滤波器公式推导出来的三个应用公式。
进入卡尔曼滤波器的有两个参量，一个是加速度计得到的角度，一个是陀螺仪得到的角速度。
他们之间的关系式角度的微分就可以得到角速度。
现在通过融合滤波的方式，融合这两个参量，最后达到稳定准确的角度、角速度。
我用最简单的三个公式，表示一下，我这次所用的卡尔曼算法。
Temp=angle-1+gyro*k;
angle= Temp*g+(θ-Temp)*(1-g);
angle-1=angle;
其中，设加速度计计算出的角度数据为θ，陀螺仪只采数据为gyro,陀螺仪相对加速度计的因子为k,上次加速度计经过滤波后的角度数据为angle-1,卡尔曼增益选择固定值为g.滤波后的角度数据为angle;前两个公式可以融合一下可以得到以下两个公式：angle= (angle-1+gyro*k) *g+[θ-(angle-1+gyro*k)]*(1-g);
angle-1=angle;
程序不断循环，可以得到实时的角度即angle。
4.卡尔曼滤波器融合后波形图。
在设计中，通过串口不断发送它融合前的角度和角速度，及融合后的角度。
然后通过串口软件，不断在图形界面画图，就可以得到它的仿真图。
在本仿真图中，蓝色的线代表陀螺仪采集到的角速度，绿色采集到的为加速度计采集到的角度值。
黄色的线为通过卡尔曼滤波器融合后的角度。
通过图形可以看出，加速度计得到的角度噪声很大。
而融合后的波形实时性很强，准确度高，图形很平滑，噪声很小。
5. PID 算法公式。
e(t) dt
d K +e(τ)d τ K +e(t) K =u(t)d
t
0i p speed;*Kps +position *Kis +Gyro_x *Kd + Angle *Kp =PWM
第一个公式为PID 控制器的基本公式，第二个公式为小车控制器的基本算法，角度控制参数包括比例Kp 和微分Kd 两个参数。
在角度参数初步确定之后，便可以进行速度控制参数的整定。
速度控制参数
包括积分Kis和比例Kps两个控制参数，这里的积分和比例，是相对于反馈位置（速度的积分量）而言。
如果相对于车模的反馈速度，这两个参数也可以分别对应称作比例P和微分D参数。
Angle代表角度，Gyro_x代表x轴角速度，position代表速度的积分位置，speed代表测得的速度。
6.PID算法中各系数的确定及大致范围。
答：确定PID控制器的四个参数，我们要先通过MATLAB进行仿真，确定一个大致的范围及比例关系。
用MATLAB仿真时，需要事先测量的参数有小车重量，小车的长宽高，电机的重量，轮胎的重量及轮胎的直径。
我通过MATLAB仿真后的参数变量分别为Kp=39.8623，Kd=2.8586，Kis=0.9235，Kps=1.2474。
在这里我们可以得到一个比例关系Kp:Kd:Kis:Kps=40:2.8:0.9:1.25。
得到比例关系后，我们就要通过实验法来测定PID几个参数，下面就是调节参数的步骤：调节角度控制参数比例Kp和微分Kd这两个参数可以遵循先比例后微分的过程。
首先调整比例参数，使得车模能够保持直立并且开始来回摆动了。
说明此事比例参数已经可以克服重力的影像。
然后逐步增加微分参数，车模逐步保持直立稳定。
增大微分参数直到车模开始共振，这样就可以确定微分参数的最大值了。
然后适当减小微分参数，然后逐步增加比例参数，直到车模又开始震荡，这样便可以确定比例参数的最大值。
然后适当减小比例参数。
在这些参数附近进行试验，直到寻找到一组车模直立的控制最优参数。
在调整积分Kis和比例Kps两个控制参数时，由于是进行静态调
整，故此先将速度控制闭环的速度设定值置为0。
然后开始后面的积分和比例参数的调整。
具体的调试过程可以首先逐步积分参数，车模能够在一定平衡点附近来回运动。
然后在逐步增加比例参数，车模就会很快停留在平衡点出。
使用外力冲击车模，车模能够很快趋于静止。
然后再逐步增加积分和比例控制参数，使得车模抵抗外部干扰冲击的能力逐步增强。
当积分和比例参数达到一定的数值后，车模开始出现摆动现象。
至此可以知道积分和比例的大体范围。
在此范围内通过几次调整测试，可以初步确定速度控制参数的最佳组合。
通过仿真和实验我得出了四个参数的大致范围：
Kp：9.0-12.0。
Kd：1.4-2.5。
Kis：0.005-0.015。
Kps：0.4-0.8。
通过多次的实验，得到了一组最佳的参数值：Kp= 9.6，Kd= 2.2，Kis = 0.015，Kps = 0.5。