Kalman filter basics with one gyro

[jblc]

While trying to learn kalman filters, I see contradictory info for a simple gyro State Space representation. Please be patient with this post, I've put a fair bit of time trying to make it easy to follow and read!
My goal is to understand how a moving IMU's orientation can be found. I'd appreciate any help!

Say there's just one gyro, with angular rate output reading ω. And just one state output, actual angVel of the system. Yes, this is a trivial example, as of course angVel of system = ω (if ignoring noise).

Question: Which of the below two cases is generally correct? Later i'll work up to a higher DOF, but even in the 1D case A and B matrices change depending on which version is used below, so the kalman filter should behave differently. Any help would be great.
Note : this post assumes discrete control, ie xk+1 = Axk + Buk, vs the xdot = Ax + Bu type.


Case 1) some people input the reading ω into the control vector u: angVel = Ax + Bu = Ax + Bω. This is then angVel = 1*ω. So A = 0 and B = 1.
Eg. equation 9 of http://www.mouser.com/applications/sensor_solutions_mems/

But ω is a sensor reading, not a control input of the system, no? It is telling us what we're seeing, not what we're inputting. Normally B maps things in u, like forces/torques, into describing how the system moves as a result.
The forces/torque mapping of, say, an airplane is described by B; a gyro is just a reading of what actually happens. So where does the reading get inputted? Or do all of those forces/torques and the gyro readings get put into the control vector u together?

I would have thought the sensor measurements (gyro reading) go into the x vector, to be then mapped into the measurement vector z later on via z = Cx: z = angVelreading = 1*ω, so C = 1 in this trivial example.


Case 2) Others input the reading ω into part of the state vector x: angVel = Ax + Bu = Aω = ω. So A = 1 and B = 0. Eg pg.4 of http://see-out.com/sandramau/doc/Mau05MathKalman.pdf. In the case of the kalman filter, though, it seems that we shouldn't be erasing/over-riding part of the state vector by putting our reading ω in place of angVel, because all of the previous calculated (not measured) state is needed for a proper prediction.


Lastly for a single gyro, some people take the angle θ as the measurement output in z. Eg. Eqtn.3.2 of http://www.olliw.eu/2013/imu-data-fusing/
This doesn't make sense to me, as our purpose in the first place may be to measure angle, and angle measurements aren't readily available -- only rates ω are available. Wouldn't you want angular velocity as one of the the measurement states instead of θ?

 

[chingkui]

If you only have one single gyro, there is no way you can estimate the attitude unless your system is highly constrained with two components locked. You need 3 gyros in general. And what other sensors are available? Even if you have 3 gyros, KF is useless by itself if you have no other measurement. If you want to formulate a KF, you need 1) a state prediction model and 2) a sensor measurement model. Looks like in your case you only have a prediction model (gyro reading to angular position), without other sensors, there is no way of doing correction using KF.

 

[jblc]

right, as mentioned this was a simplified and constrained 1D case with just 1 gyro, just for learning purposes. I agree and realize that an actual IMU requires 3.

The sensor measurement model was the z=Cx part above, or are you referring to something else? I thought i had both the measurement model and state prediction model listed.

My main question is figuring out what sensor goes into the measurement prediction model, and what goes into the state prediction model...and why. For example if I have a vertical-plane-constrained gyro, and 2-axis accelerometer as well (to estimate an angle), how to tell where the accel signals go, and where the gyro signals go? Or are they both put into z in the K(z-Cx) part of the kalman filter? Or is one put into x, and another into z?

Thanks!

 

[chingkui]

In designing a KF, first you have to write down exactly what you are trying to estimate here (the state x), and then write down the dynamic equations x(k+1) = Fx(k) + n(t), by writing down the dynamics, you probably would see if the model needs the sensor input. From your description, I am not sure exactly which angle you are trying to measure, is it one angle? which axis? or is it all three angles since you also have two accelerometers? The single gyro IMU data fusion in the olliw's website you quoted above uses at least one more accelerometer to measure the same angle the gyro is measuring. The basic idea is that the gyro readings are used in the dynamic model to predict the rotation angle (one of the states) and use the accelerometer to measure angle a, and compare it with the predicted rotation angle (hence a is used on the measurement model). You can probably learn from that to write down the state and dynamic model of your current problem. In using Kalman filter, you need to have redundancy in measurement, as in the olliw's example, you can estimate the rotation by the gyro or by the accelerometer, and picking one of them as the dynamic model, the other would be your measurement so you can apply corrections to your prediction.

 

[jblc]

Exactly, I'm referring to the sensor fusion example where the gyro angle rate refers to the same angle that the two accelerometer axes can measure due to gravity. It's a redundant measurement.
"The basic idea is that the gyro readings are used in the dynamic model to predict the rotation angle "
-->Why is that needed though? The Fx(k) in the dynamic model is the natural non-input motion of the system. Can't we still use a kalman filter even if no sensor is input to the prediction, because we have a state update correction anyway later in x(k) = x_prediction(k) + K(z-C*x_prediction)?

Say we start with k=0. Then since state x(0) didn't have any sensor readings in there (and it's just the current state), x(0)=0 vector of course, and x(0)_prediction = Fx(0) = 0. But then there will be a K gain because z will in fact show a sensor reading, so x(0)_estimate will still be correctly updated when calculating x(1) = x_prediction(0) + K(z-C*x_prediction). here z (=measured rates and angles) is very different from prediction (=0 state) so that term has a large contribution.

In the next iteration after that, x(1)_prediction = Fx(1), and this time x(1) is non-zero because of that update, (say it has an angle and angular velocity) and x(1)_prediction will have physical meaning starting this iteration.
Is this correct?

" In using Kalman filter, you need to have redundancy in measurement"
A kalman filter can still smooth even just a changing scalar number, so i don't think redundancy is its core property? The core property seems to be prediction based on a probabalistic model.
But agreed data fusion is very important, and that was indeed my question.
Say z = [angle angularRate]'. By putting the gyro and accel both into the measurement update, the redundancy is taken into account, and via K(z-CX), the final estimated state x(k+1) = x(k)_predicted + K(z-CX) should be correct, no?

I guess I don't see why they need to be split up, since after one iteration the kalman gain K will start to relate the measurement vector z with the states.

 

[chingkui]

The system you refer to has 1 rotation DOF, the current state (angle) is the sum of the previous state and the change of rotation angle the most recent interval, hence x(k+1) = x(k) + w*dt is the system dynamics. If you instead write the dynamics equation as gyro input free, you are modeling a physical system completely different from the real system. With no gyro input in the equation, you are actually saying the angle is a constant. Let's say the system rotates from x(0) = 0 to 10 degrees at time = 1, the system dynamics will predict x(1) is approximately 10 degrees (error due to gyro noise), and the angular measurement derived from the accelerometer will also tell you x(1) is close to 10 deg, and so the prediction and measurement are consistent. On the other hand, using the measurement-free system dynamics will predict x(1) = x(0) = 0 deg, while the measurement will tell you x(1) should be close to 10 deg. Now, when the dynamics and measurement tell you completely different story, who should the KF listen to? The KF will definitely try to come up with a number somewhere between the two, but good luck if you expect the KF could figure out the right answer. The bottom line is, you have to model the system dynamics as close to the reality as possible. KF will just do what you tell it to do. If you give it nonsense, it will not be happy and will give you garbage in return.

 

[jblc]

Ah, I see, that is more clear now. You explained that better than other sources I read, thanks!
To verify that i understand, say that an object to be tracked in position/attitude is experiencing an un-modeled disturbance--say an unknown force and torque.
--> For the KF to work best, a sensor needs to detect the expected disturbance type as directly as possible. Eg acceleration, gyro, etc.. Those are placed onto the object for Prediction. Corrections/Updates may be added from other sensors as well in the update stage.
--> Or we *could* instead just use an external sensor that doesn't measure the disturbance directly, but rather the outcome: eg an external camera on the Update step. The Predict step will then just (incorrectly) predict evolution of the previous state. This will work too but much less effectively, since it will not predict state evolution but rather only the Update, and can't compare those errors. As such it won't be all that effective, but will be instead more like a low-pass filter.

 

[chingkui]

--> Or we *could* instead just use an external sensor that doesn't measure the disturbance directly, but rather the outcome: eg an external camera on the Update step. The Predict step will then just (incorrectly) predict evolution of the previous state. This will work too but much less effectively, since it will not predict state evolution but rather only the Update, and can't compare those errors. As such it won't be all that effective, but will be instead more like a low-pass filter.

Not sure if I understand what you said correctly. But if you mean using a wrong prediction model in KF, and then rely on the measurement updates to correct the state, I am not very optimistic that it will work. If the only reliable information is the measurement update at each instant, why use a KF at all? It is better just do a least square based on the measurements at each step. KF is different from optimization at each time step in that it takes into account the whole history of all measurements through the state propagation model. Under certain assumptions of the error statistic or error correction method, KF is an optimal filter conditioning on all the previous measurements. And it only works if a correct state propagation model is used.

 

[jblc]

"If the only reliable information is the measurement update at each instant, why use a KF at all?"
Wouldn't that still count as a kalman filter? We can still estimate the new state without a measurement, from the previous state's propagation, and make an estimate from that alone. The associated state covariances evolves as well.
Then in the measurement update, the combination still yields an intelligent guess when combined, no?

Would this not effectively still be a kalman filter, then, since it combines two probabilistic estimates: one of a prediction and uncertainty, and another with a measurement and uncertainty.

Not as optimal as the structure you mentioned, but I don't see why fundamentally this idea wouldn't be considered a kalman filter.

 

[chingkui]

Wouldn't that still count as a kalman filter? We can still estimate the new state without a measurement, from the previous state's propagation, and make an estimate from that alone. The associated state covariances evolves as well.
Then in the measurement update, the combination still yields an intelligent guess when combined, no?

No, it isn't. To see why it is not, you have to go through the derivation of KF, and the associated error propagation. One key properties of KF is that it is an unbiased state estimator. Your proposed state propagation model is biased by exactly the missing measurement term which has non-zero mean in general. So, the expected error of your method won't be zero.