一步一步在平衡车上实现卡尔曼滤波

I have for along time been interrested in Kalman filers and how they work, I also used aKalman filter for my Balancingrobot, but I never explained how it actually wasimplemented. Actually I had never taken the time to sit down with a pen and apiece of paper and try to do the math by myself, so I actually did not know howit was implemented.
It turned out to be a good thing, as I actually discovered a mistake in the originalcode, but I will get back to that later.

我已经有很长一段时间想知道卡尔曼滤波到底是如何工作的，而且我把卡尔曼滤波功能用于我的平衡车机器人上。但是我从没有去解释它们到底是怎么做到的。实际上我也从没有花一些时间独自坐下来用笔和纸来做一些数学推导。因此可以说我是真不知道卡尔曼到底是怎么做到的。

静下来做这个事情将有一个好结果，因为这让我在源码中发现了一个错误，但是后面我们再转到那个主题上去吧。

I actually wroteabout the Kalman filter as my master assignment in high school back in December2011.

But I only usedthe Kalman filter to calculate the true voltage of a DC signal modulated byknown Gaussian white noise. My assignment can be found in the following zipfile: http://www.tkjelectronics.dk/uploads/Kalman_SRP.zip. It is indanish, but you can properly use google translate to translate some of it. Ifyou got any specific questions regarding the assignment, then ask in thecomments below.

实际上在2011年12月我在高级学校上学的时候就写过相关卡尔曼滤波的硕士论文，那时只是使用卡尔曼滤波去计算被高斯白噪声调制后的直流电压的真实值，你可以从下面的zip 文件 http://www.tkjelectronics.dk/uploads/Kalman_SRP.zip的中查看我的作业论文。由于它是丹麦语，你可以使用google 翻译等翻译软件去翻译为你能看懂的语言。当然，如果你有任何特殊针对这个作业的问题，你也可以在下面的comment 栏留言。

Okay, but backto the subject. As I said I had never taken the time to sit down and do themath regarding the Kalman filter based on an accelerometer and a gyroscope. Itwas not as hard as I expected, but I must confess that I still have not studiedthe deeper theory behind, on why it actually works. But for me, and most peopleout there, I am more interrested in implementing the filter, than in the deepertheory behind and why the equations works.
好了，回到今天的主题吧，正如上面所说，我一直都没有时间坐下来推导基于加速度计和陀螺仪方面的卡尔曼滤波方程，其实这并不是我想象的那么困难，但是我必须承认，我对理论方面没有研究的那么透彻，没有深究他的工作原理。但是对于我和大部分人来说，相比背后高深的理论和工作原理，我们对如何实现和使用这个滤波器更感兴趣。
Before we begin you must have some basic knowledge about matrices likemultiplication of matrices and transposing of matrices. If not then please takea look at the following websites:

在我们开始之前，建议你必须有一些矩阵的基础知识，比如矩阵乘法和转置，如果不是很熟悉，请进到下面一些网站去看一看。

·        http://en.wikipedia.org/wiki/Matrix_multiplication#Matrix_product_.28two_matrices.29

·        http://www.mathwarehouse.com/algebra/matrix/multiply-matrix.php

·        http://en.wikipedia.org/wiki/Transpose

·        http://en.wikipedia.org/wiki/Covariance_matrix

For those of youwho do not know what a Kalman filter is, it is an algorithm which uses a seriesof measurements observed over time, in this context an accelerometer and agyroscope. These measurements will contain noise that will contribute to theerror of the measurement. The Kalman filter will then try to estimate the stateof the system, based on the current and previous states, that tend to be moreprecise that than the measurements alone.

对于那些还不了解卡尔曼滤波是什么东东的人来说，卡尔曼就是一个算法，这个算法用来处理一系列基于时间的测量值，而且这些测量出来的值包含了一些噪声，因此可以说这些测量值是包含一定偏差的（所以不是很准确）。卡尔曼滤波器将会尽力去估计系统的状态，而当前的状态需要依赖前一个状态，从而会让最终获得的值比以前那些纯测量值更加精确。

In this contextthe problem is that the accelerometer is in general very noise when it is usedto measure the gravitational acceleration since the robot is moving back andforth. The problem with the gyro is that it drifts over time – just like aspinning wheel-gyro will start to fall down when it is losing speed.
In short you can say that you can only trust the gyroscope on a short termwhile you can only trust the accelerometer on a long term.

在本场景中，加速度计通常带有一定的噪声当在测试重力加速度的时候，因为机器人会进行前后移动，而陀螺仪会随着时间的推移会产生漂移，如当一个正在自转的陀螺会随着速度趋于零而慢慢会倒下来一样。也就是说，基于某个瞬态，你只能信任陀螺仪，而在一个相对时间比较长的范围的时候，你还是会相信加速度的。

There isactually a very easy way to deal with this by using a complimentary filter,which basicly just consist of a digital low-pass filter on the accelerometerand digital high-pass filter on the gyroscope readings. But it is not asaccurate as the Kalman filter, but other people have succesfully buildbalancing robots using a fine-tuned complimentary filter.

针对这种情况，我们有一个非常简单的方法来处理它，那就是利用一个复合滤波器。他们是由作用于加速度计的低通滤波器和作用于陀螺仪的高通滤波器结合而成。但是这种滤波器就没有卡尔曼滤波器那么精确了。其实最终有些人就成功地使用一个非常精细的复合滤波器实现了平衡机器人。

More informationabout gyroscopes, accelerometer and complimentary filters can be found in this pdf. A comparisonbetween a complimentary filter and a Kalman filter can be found in thefollowing blogpost.

更多有关陀螺仪，加速度计，及复合滤波器的信息可以在这个pdf文件中找到，而复合滤波器和卡尔曼滤波器之间的对比，你也可以在这个博客blogpost中找到它

The Kalmanfilter operates by producing a statistically optimal estimate of the systemstate based upon the measurement(s). To do this it will need to know the noiseof the input to the filter called the measurement noise, but also the noise ofthe system itself called the process noise. To do this the noise has to be Gaussian distributed and have a mean of zero, luckily for us most random noise have thischaracteristic.

卡尔曼滤波器的工作是基于统计一系列测量数据来产生系统状态的最优估计。为了达到这个效果，它需要知道一些测量噪声作为滤波器的噪声输入项。这个噪声也称作系统本身的过程噪声，而且这个噪声必须是高斯分布的，均值是0，非常幸运的是，大部分随机噪声都是具有这个性质特征。（白噪声）
For more information about the theory behind the filter take a look at thefollowing pages:

更多有关卡尔曼滤波的理论请参考下面的网页：

·        http://en.wikipedia.org/wiki/Kalman_filter

·        http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf

·        http://academic.csuohio.edu/simond/courses/eec644/kalman.pdf

The system state 

系统状态
The next of this article might seem very confusing for some, but I promise youif you grab a pen and a piece of paper and try to follow along it is not thathard if you are reasonable at math.

接下来的部分可能会让人感到一些混乱，但是如果你拿出纸和笔的话，并具有一定的数学基础，那么只要尽量一步步来，那么就不会感觉那么难了。

If you, like me,do not have a calculator or computer program that can work with matrices, thenI recommend the free online calculator Wolfram Alpha. I used it forall the calculations in this article.

另外，如果你象我一样，手头上没有计算器和计算机程序帮我们做一些矩阵运算，那么我推荐你使用一个免费的在线计算器Wolfram Alpha.在本文中全是使用它来进行演算的。

I will use thesame notation as the wikipedia article, but I willlike to note that when the matrixes are constants and does not depend on thecurrent time you do not have to write the k after them. So for instance  can be simplified to .

我将使用和wikipedia article里相同的符号，但是我还要说明下，当矩阵里元素都是常数或它并不依赖当前时间的话，我们就没有必要在它们后面加上K了，比如就可以简化为.

Also I wouldlike to write a small explanation of the other aspects of the notations.

而且我将对一些关于其它方面的符号做一个小小的解释。
First I will make a note about whats called the previous state:

首先，我们要解释下什么叫上一个状态：

Which is the previous estimated state based on the previous stateand the estimates of the states before it.

它就是在上一个状态及它之前推导计算出来的所有状态基础上推导出来的一个状态。

 

The next is the a priori state:

接下来就是先验状态：

A priori means the estimate of the state matrixat the current time k based on the previous state of the system and theestimates of the states before it.

先验的意思就是：在当前时间k时刻一个状态矩阵一个预估值，它是基于前一个系统状态和它之前的所有统计的一个状态推导出来的。

The last one iscalled a posteriori state:

最后一个就是后验状态：

it is the estimated of the state at time k givenobservations up to and including at time k.

它就是在给定k时刻测量值以及以前所有的测量值和理论值基础上统计出来的最优值。

The problem isthat the system state itself is hidden and can only be observed throughobservation . This is alsocalled a Hidden Markov model.

我们的状态系统类似一个隐马科夫模型，因为我们所谓的系统状态它是观测不到的，这些系统状态只有通过一定的转换，变为观测状态变量

This means thatthe state will be based upon the state at time k and all the previous states.That also means that you can not trust the estimate of the state before theKalman filter has stabilized – take a look at the graph at the front page of my assignment.

这也意味着此时的状态是基于k时刻的以及k时刻以前所有的状态的综合。这也就表明在卡尔曼滤波器稳定之前，所有以前估计的状态都是不可信的。请看下我作业的前页的图形。

The hat over the x means that is the estimate of the state. Unlike just a single  which means the true state – the one we are trying to estimate.

在x 上头戴了帽子的是表示一个某个时刻的估计状态，而不是表示某个单独时刻的真实状态，它就是需要我们通过估计计算去获得的，也可以称为最优状态。

So the notation for the state at time k is:

因此在k时刻的状态使用符号为

The state of thesystem at time k if given by:

系统状态在k时刻的状态方程表示如下：

Where  is the state matrix which is given by:

 这是一个状态矩阵，它的表示如下：包含2个元素的矩阵。

 

As you can see theoutput of the filter will be the angle  but also the bias  based upon the measurements from the accelerometer and gyroscope. The biasis the amount the gyro has drifted. This means that one can get the true rateby subtracting the bias from the gyro measurement.

正如你所见，滤波器的输出是角度值和偏置，他们都是基于加速度和陀螺仪的测量值的。偏置是整个陀螺仪的漂移量，想要获得真实的速度值需要我们在陀螺仪测量值的基础上减去这个偏置值后计算获得。（速度等于加速度乘于时间）。

The next is the  matrix, which is the state transition model which is applied to theprevouis state .

下一个是矩阵，它是状态转移矩阵，作用在上一状态上。

In this case  is defined as:

对于这个事例，我们定义如下：

 

I know that the  might seem confusing, but it will make sense later (take a look at mycomment).

我知道，也许会让人感觉一些混乱，但是，我们将在后面看到真正的意义，请看下我的注释comment。

The next is thecontrol input , in this caseit is the gyroscope measurement in degrees per second (°/s) at time k, this isalso called the rate . We willactually rewrite the state equation as:

下一个是输入控制量，在这个事例中，它是陀螺仪在k时刻的测量值，表示每秒多少度(°/s)，我们也可以称为角速率，实际上我们将把上述状态方程改写为：

The next thingis the  matrix. Which is called the control-input model, which is defined as:

下一个要关注的是矩阵，我们称它为一个控制矩阵模型，它定义为如下：

This makesperfectly sense as you will get the angle  when you multiply the rate  by the delta time  and since we can not calculate the bias directly based on the rate we willset the bottom of the matrix to 0.

这种表示是非常有意义的，因为你将很容易获得角度值，你只需用角速率乘于一个时长就行。另外我们无法在角速度下直接计算出偏置值，

      is process noise which is Gaussiandistributed with a zero mean and with covariance  to the time k:

      它是一个过程噪声，它是中值是0，在k时刻协方差为的高速分布噪声。

        is the process noise covariance matrixand in this case the covariance matrix of the state estimate of theaccelerometer and bias. In this case we will consider the estimate of the biasand the accelerometer to be independent, so it’s actually just equal to thevariance of the estimate of the accelerometer and bias.

        是一个过程噪声协方差矩阵，在这个事例中，它是加速度计和偏置的协方差状态矩阵，在这个事例中，我们将考虑到，偏置和加速度计之间是相互独立的，因此，它只是加速度计和偏置的各自的一个统计偏差，因此在其它两项中设置为0
The final matrix is defined as so:

最终的矩阵定义如下：

As you can seethe  covariance matrix depends on the current time k, so the accelerometervariance  and the variance of the bias  is multiplied by the delta time .

正如你看到的那样：协方差矩阵是依赖当前时刻K的，因此加速度计的协方差量和偏置协方差量需要用去乘的。
This makes sense as the process noise will be larger as longer time it is sincethe last update of the state. For instance the gyro could have drifted.

这就意味着，过程噪声将会变得越来越大，随着时间越来越久。比如陀螺仪的漂移会越来越大。

You will have toknow these constants for the Kalman filter to work.

所有你必须要知道这些常数是如何让卡尔曼怎么工作的。
Note if you set a larger value, the more noise in the estimation of the state.So for instance if the estimated angle starts to drift you have to increase thevalue of . Otherwise ifthe estimate tends to be slow you are trusting the estimate of the angle toomuch and should try to decrease the value of  to make it more responsive.

注意，如果你把这个值设置成比较大的话，后面统计的状态将会带有更多的噪声分量。因此，如果后面统计出来的角度有漂移了，你必须要加大的值，否则，如果估计的变化趋于缓慢，表现为你信任角度值更多一些，那么你就必须减少，让它变得更加灵敏。

The measurement 

测量矩阵
Now we will take a look at the observation or measurement  of the true state . Theobservation  is given by:

现在让我们看下我们的观测矩阵或测量矩阵，它是基于真值状态的，表示如下：

As you can seethe measurement  is given by the current state  multiplied by the  matrix plus the measurement noise .

如你所看到的，测量矩阵是由当前状态左乘一个矩阵，再加上一个测量噪声获得。

        is called the observation model and isused to map the true state space into the observed space. The true state cannot be observed. Since the measurement is just the measurement from theaccelerometer,  is given by:

        称为观测模型，它用来把真值状态空间映像为观测空间，因为真值状态是不能观测的，由于我们只是从加速度计获得读数而获得测量值，因此可以定义如下：

The noise of themeasurement have to be Gaussian distributed as well with a zero mean and  as the covariance:

测量噪声必须是高斯分布的中值为0的，由表示的协方差。

But as  is not a matrix the measurement noise is just equal to the variance of themeasurement, since the covariance of the same variable is equal to thevariance. See thispage for more information.
Now we can define  as so:

但是此时的不是一个矩阵，测量噪声只是等于测量方差。，因为同一个变量的协方差就等于这个方差。要获得更多信息请参考page 。

More informationabout covariance can be found on Wikipedia and in my assignment.

更多关于协方差的内容请查看Wikipedia 和我的作业my assignment

We will assumethat the measurement noise is the same and does not depend on the time k:

我们将假设，测量噪声是一个常量，它不随时间的变化而变化。

Note that if youset the measurement noise variance  too high the filter will respond really slowly as it is trusting newmeasurements less, but if it is too small the value might overshoot and benoisy since we trust the accelerometer measurements too much.

注意，如果我们把这个测量噪声方差设置的比较大，那么滤波器将会响应的比较慢，因为我们将信任测量的部分少。然而，如果这个值设置的太小，又可能导致溢出及嘈杂，因为我们信任加速度计测量过多，实际上这里含有一些噪声的。

So to round upyou have to find the process noise variances  and  and the measurement variance of the measurement noise . There aremultiple ways to find them, but it is out of the aspect of this article.

因此，要全面了解过程噪声方差和，以及测量噪声的测量方差。有多种途径可以找到它们，但这不是本文需要讨论的内容。

The Kalman filter equations
Okay now to the equations we will use to estimate the true state of the systemat time k . Some cleverguys came up with equations found below to estimate the state of the system.

好了，我们将用卡尔曼滤波方程来估计系统在k时的真实状态。聪明如你的家伙应该想到了下面的方程就是用来估计系统的状态的。

The equations can be written more compact, but I prefer to have them stretchedout, so it is easier to implement and understand the different steps.

本来这些方程可以写得更紧凑些，但我更喜欢把它们展开来写，这样我们就可以更容易实现他们并理解不同的步骤。

Predict
In the first two equations we will try to predict the current state and theerror covariance matrix at time k. First the filter will try to estimate thecurrent state based on all the previous states and the gyro measurement:

 前两个方程，我们将尽量用来预测在k时刻的当前状态及协方差矩阵。

下面是第一个方程，是滤波器将尽量根据上一个状态值及陀螺仪的测试值来统计当前k时刻的一个状态值。

That is also why it is called a control input,since we use it as an extra input to estimate the state at the current time kcalled them a priori state  as described in the beginning of the article.

这也是为什么它被称为输入控制，我们把它当作一个额外的输入用来估计在k时刻的状态，我们称上面这个为先验状态。注意他们的下标，表示为用k-1时刻的真值，来估计k时刻的值。这个值我称为理论过度值。

The next thingis that we will try to estimate the a priori error covariance matrix  based on the previous error covariance matrix , which isdefined as:

 下一件事就是，我们要尽量估计出先验误差协方差矩阵，它也是在上一个误差协方差矩阵基础上理论推算得来的，它们定位为如下：

This matrix isused to estimate how much we trust the current values of the estimated state.The smaller the more we trust the current estimated state. The principle of theequation above is actually pretty easy to understand, as it is pretty obviousthat the error covariance will increase since we last updated the estimate ofthe state, therefore we multiplied the error covariance matrix by the statetransition model  and the transpose of that  and add the current process noise  at time k.

这个矩阵是用来估计我们到底应该信任多少这个估计除了的状态，当这个值越小的话，我们就会越认为当前的估计状态越可靠。其实这个方程的原理实际上是比较容易理解的，很明显，由于我们最后更新了状态估计，误差协方差将增加，因此我们通过状态转换模型和转置矩阵乘以误差协方差矩阵，并在当前时刻k加入了当前过程噪声。

 

The errorcovariance matrix  in our case is a 2×2 matrix:

在我们这个事例中，误差协方差矩阵是一个2×2矩阵，表示如下：

Update
The first thing we will do is to compute the difference between the measurement  and the a priori state , this is alsocalled the innovation:

第一件事我们需要做的是，去计算测量矩阵值和先验状态左乘观测模型后的差，这里我们称这个差为新息。其表示如下:

The observationmodel  is used to map the a priori state  into the observed space which is the measurement from the accelerometer,therefore the innovation is not a matrix

观测模型矩阵是用来把先验状态映像到观测空间，而观测空间里只是获取加速计的值，因此这个新息不是一个矩阵。

The next thingwe will do is calculate whats called the innovation covariance:

下一件事就是我们要计算所谓的协方差新息：表示如下：

What it does is that it tries to predict howmuch we should trust the measurement based on the a priori error covariancematrix  and the measurement covariance matrix . The observation model  is used to map the a priori error covariance matrix  into observed space.

The bigger thevalue of the measurement noise the larger the value of , this meansthat we do not trust the incoming measurement that much.

上面方程就是尽量预测下，我们到底该相信多少这个测量值，它们需要先验误差协方差矩阵和测量协方差矩阵等几个共同决定，而观测模型矩阵是用来把先验误差协方差矩阵映像到观测空间来的。这个测量噪声值越大，那么就将会越大，也就意味着这次测量带有很多误差，因此就越不可信。
In this case  is not a matrix and is just written as:

在我们这个事例中不再是个矩阵了，因此将会进行如下表示：

The next step isto calculate the Kalman gain. The Kalman gain is used to indicate how much wetrust the innovation and is defined as:

下一步就是计算卡尔曼增益。卡尔曼增益就是用来指示我们该信任这个信息多少的一个比例量。它定义如下：

You can see thatif we do not trust the innovation that much the innovation covariance  will be high and if we trust the estimate of the state then the errorcovariance matrix  will be small the Kalman gain will therefore be small and opposite if wetrust the innovation but does not trust the estimation of the current state.

如你所见，如果我们信任新息少一些，那么新息协方差将会很大，如果我们信任状态估计多点，那么误差协方差矩阵将会很小，同时卡尔曼增益将会变小，相反，如果我们信任新息多些，就不会更多信任估计出来的系统状态。

If you take adeeper look you can see that the transpose of the observation model  is used to map the state of the error covariance matrix  into observed space. We then compare the error covariance matrix bymultiplying with the inverse of the innovation covariance .

如果你再深究下，你可以看到先用观测矩阵的转置把误差协方差矩阵映像到观测空间，然后再右乘新息协方差的逆，最后获得的结果就是我们需要的卡尔曼增益。

This make senseas we will use the observation model  to extract data from the state error covariance and compare that with thecurrent estimate of the innovation covariance.

这个的意义就是我们用观测模型矩阵去抽取一些状态误差协方差数据，并把抽取的数据去核我们当前估计的信息协方差进行比较。

Note that if youdo not know the state at startup you can set the error covariance matrix likeso:

注意如果我们不知道最初的起始状态，我们可以设置这个误差协方差矩阵为如下形式：

Where  represent a large number.

其中是表示一个比较大的数字。

For my balancingrobot I know the starting angle and I find the bias of the gyro at startup bycalibrating, so I assume that the state will be known at startup, so Iinitialize the error covariance matrix like so:

对于我的平衡机器人，我知道它的起始角度，而且陀螺仪也是经过校正过的，因此我可以认为这个起始状态是已知的，所以我初始化这个协方差矩阵为如下，全零的一个2x2矩阵。

Take a look at mycalibration routine for moreinformation.

要获得更多信息，请参看下我的校正程序mycalibration routine

In this case theKalman gain is a 2×1 matrix:

在这个事例中，卡尔曼增益是一个2x1的矩阵，如下所示：

Now we canupdate the a posteriori estimate of the current state:

现在我们就可以更新下当前状态的后验估计值了，我这里称为最优状态：

 

 

 

This is done by adding the a priori state  with the Kalman gain multiplied by the innovation .
Remember that the innovation  is the difference between the measurement  and the estimated priori state , so the innovation can both be positive and negative.

它就是先验状态加上一个卡尔曼倍数的新息。记住这个卡尔曼新息是由观测矩阵和先验矩阵乘上观测矩阵模型后的差，因此这个新息可能会是正数也可能是负数。

A littlesimplified the equation can be understood as we simply correct the estimate ofthe a priori state , that wascalculated using the previous state and the gyro measurement, with themeasurement – in this case the accelerometer.

稍微简化一点，这个方程就更好理解了，我们只需修正一个先验状态的估计，即用先前的状态和陀螺仪测量值来计算，在本例中，加速度计就能获得测量值的。

The last thingwe will do is update the a posteriori error covariance matrix:

最后一件事就是，我们要更新后验误差协方差矩阵：

Where  is called the identity matrix and is defined as:

其中在这里就是单位矩阵，它定义如下：

What the filteris doing is that it is basically self-correcting the error covariance matrixbased on how much we corrected the estimate. This make sense as we correctedthe state based the a priori error covariance matrix , but also theinnovation covariance .

滤波器所做的是：基本上它就是一个自校正系统，通过校正估计值的多少来自适应修正误差协方差矩阵，这是有意义的，我们修正了基于先验误差协方差矩阵的系统状态，也修正了协方差新息。

Implementing the filter

实现这个滤波器：
In this section I will use the equation from above to implement the filter intoa simple c++ code that can be used for balancing robots, quadcopters and other applications where you need to compute the angle, bias or rate.

在这个部分，我将是用上面介绍的方程等式来实现一个用C++写的滤波器，它可以用作平衡机器人和四轴飞行器上，或者其它你需要计算角度，偏置或速率的工程上。

In case you wantthe code next to you, it can be found at

如果你需要源代码，你可以在以下网址上找到，

github:https://github.com/TKJElectronics/KalmanFilter.

I will simplywrite the equations at the top of each step and then simplify them after that Iwill write how it is can be done it and finally I will link to calculationsdone in Wolfram Alpha in the bottom of each step, as I used them to do the calculation.

我将先简单把每步的方程写在上端，然后在下面我会写推导的一个过程，最后我还会在每个步骤的底部加上链接，在链接的网址里有计算的过程，

 Step 1:

As you can seethe a priori estimate of the angle is  is equal to the estimate of the previous state  plus the unbiased rate times the delta time .

正如你看到的，一个先验估计的角度等于上一个最优估计状态加上无偏置的角速度乘于一个时长
Since we can not directly measure the bias the estimate of the a priori bias isjust equal to the previous one.

既然我们不能直接测量到当前的偏置值，这里我就假设这个先验的偏置值就等于前一个最优的偏置值。

This can bewritten in C like so:

因此用C 语言写出来就是下面这个样子：

rate = newRate - bias;
angle += dt * rate;

Note that Icalculate the unbiased rate, so it can be be used by the user as well.

注意，以上是我计算无偏率的方式，所以用户也可以这样去使用它。

Wolfram Alphalinks:

·        Eq.1.1

Step 2:

The equationsabove can be written in C like so:

以上方程可以写成C 语言格式如下：

P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle);
P[0][1] -= dt * P[1][1];
P[1][0] -= dt * P[1][1];
P[1][1] += Q_gyroBias * dt;

Note that thisis the part of the code that there was an error in in the original code that Iused.

注意，这是部分代码，在我使用的原始代码中有一个错误。

Wolfram Alphalinks:

·        Eq.2.1

·        Eq.2.2

·        Eq.2.3

·        Eq.2.4

Step 3:

The innovationcan be calculated in C like so:

那么新息用C 语言书写的计算过程如下：

y = newAngle - angle;

Wolfram Alphalinks:

·        Eq.3.1

Step 4:

Again the C codeis pretty simple:

再一次，它用C语言写就非常简单了

S = P[0][0] + R_measure;

Wolfram Alphalinks:

·        Eq.4.1

Step 5:

Note that inother cases  can be a matrix and you can not just simply divide  by . Instead youhave to calculate the inverse of the matrix. See the following page for more information on how to do so.

注意，在其它的事例中，可能是一个矩阵，因此你也不能简化为一个除数，你应该先计算的逆，你可以参考下面的页面page 去获得更多关于如何求矩阵的逆的信息。

The Cimplementation looks like this:

用C 语言实现的代码就像下面这个样子了：

K[0] = P[0][0] / S;
K[1] = P[1][0] / S;

Wolfram Alphalinks:

·        Eq.5.1

Step 6:

Yet again theequation end up pretty short, and can be written as so in C:

再一次，这个方程就相当简短，你可以用C 语言写成如下形式：

angle += K[0] * y;
bias += K[1] * y;

Step 7:

     

                             

 

 

Remember that wedecrease the error covariance matrix again, since the error of the estimate ofthe state has been decreased.

记住，我们减少了误差协方差矩阵，因为状态估计的误差减小了。
The C code looks like this:

float P00_temp = P[0][0];
float P01_temp = P[0][1];

P[0][0] -= K[0] * P00_temp;
P[0][1] -= K[0] * P01_temp;
P[1][0] -= K[1] * P00_temp;
P[1][1] -= K[1] * P01_temp;

Wolfram Alphalinks:

·        Eq.7.1

·        Eq.7.2

·        Eq.7.3

Note that I havefound that the following variances works perfectly for most IMUs:

请注意，我发现以下变量值能完美匹配大部分IMUs（惯性测量单元）

float Q_angle = 0.001;
float Q_gyroBias = 0.003;
float R_measure = 0.03;

Remember thatit’s very important to set the target angle at startup if you need to use theoutput at startup. For more information, see the calibrationroutine for my balancing robot.

请记住，如果需要在启动时使用输出量，那么在启动时设置目标角度就变得非常重要了。更多信息，请看 calibrationroutine for my balancing robot.

In case youmissed it here is the library I wrote a library that can be used by anymicrocontroller that supports floating math. The source code can be found at

如果你错过了，我也写了一个专门的库，它可以被任何支持浮点计算的微控制器使用。源代码可以进到以下网址

github:https://github.com/TKJElectronics/KalmanFilter.

If you prefer avideo explanation about the Kalman filter, I recommend the following videoseries: http://www.youtube.com/watch?v=FkCT_LV9Syk.

如果你想看有关卡尔曼滤波相关的视频教程，我推荐如下视频系列：http://www.youtube.com/watch?v=FkCT_LV9Syk.

Note that youcan not use the library if you need to represent something in a full 3Dorientations, as euler angles suffer from what is called Gimbal lock you will need to use Quaternions to do that, but that is a whole anotherstory. For now take a look at the followingpage.

请注意，我们这个库可不能用于表示带全3D方向的工程里去，因为欧拉角有所谓的万向节锁限制，因此你需要使用四元数的去做，这个就不在本文的讨论范围内。现在你可以看下下面的页面：page.

This is all forknow, I hope that you will find it helpful, if you do or have any questions feelfree to post a comment below – it supports LaTeX syntax as well, if you need to write equations.
If you spot any errors please let me know as well.

这一切都是为了知道卡尔曼的工作原理，我希望这些对你有用，如果你在使用过程中有任何问题，欢迎到评论区发表，如果你的评论包含有方程格式，评论栏也支持 LaTeX syntax去书写。同时，如果您发现有任何错误，请也让我知晓！

 

总之，卡尔曼滤波器就是基于物理等理论数据公式，加上实际某种仪器测量出来的（测量里带有一些白噪声以及仪器本身的精度），针对一些噪声的大小进行动态，自适应地取得加权平均值，来综合理论值和实际测量值。