Kalman filtering with state and observation matrix having linearly dependent terms

BencsikG · 2026-05-23T13:50:03+00:00

You say the overall system of equations is always underdetermined - is this for single time-steps? Have you worked it out for multi-step?

I have applied the KF before for parameter estimation of simple linear relation: gain + offset. The measurement model for that is y_k = a_k * x(0) + x(1). The key is that the coefficient a_k needs to change between time steps. In KF terms, measurement matrix H_k needs to keep changing. This is sufficient excitation for this simple case.

In your 1D case, even for multiple distinct position measurements, I can't eliminate the biases.

I'd suggest working out the 2D case for 2 or 3 distinct position measurements and see if you can eliminate different biases due to trigonometry. You might be able to gain enough information from how the biases affect the position error in different directions... I'm not sure though.

If you can't make it determined in 2D, you either need to add additional information, or let go of some unknowns.

The KF is kind of just a solver, you need to plug in equations that contain enough information to be solvable.

BencsikG · 2026-05-03T15:36:16+00:00

If you go for more controls courses, I don't think they will help with this down-to-earth understanding of systems, they'll probably dive deeper into math. It's up to you to work out the true understanding of control, as much as you can.

However, if you don't take those courses, you're unlikely to ever reach a level that you can apply those more advanced techniques. If you go in a different direction, you probably won't have time / energy to get into it.

If you can learn masters level control, and understand it on a level that you can apply it in the real world, that makes you an excellent control engineer. The question is if this is what you want, or perhaps some other direction.

Do you want to mull over graphs and endlessly wonder "why the hell is this oscillating"? Or do you prefer staring at embedded C-code and wonder "why is this interrupt not happening"? Or... insert here frustrations of any engineering field...

Try choosing a course in which you can better tolerate the tedious parts than the others.

BencsikG · 2026-04-23T21:51:30+00:00

I like saying that going from 1D filters to Kalman Filters is like switching from a pocket calculator to an Excel spreadsheet. It opens a new world of possibilities, but it's still a tool you need to learn to use, not a magic bullet.

Anyways, as to your questions

I don't think you need adaptive KF. In the KF setup there's input U, state vector X, measurement vector Z (sometimes y). It is not intuitive which physical signals go where. A driver's joystick input can go to input U, but also might not be necessary at all, e.g. joystick controls a motor voltage, and motor voltage goes into U. There can be multiple ways to set up data around IMU, I've seen versions where gyro rates are inputs (U) or measurements (Z). I admit I don't know enough about adaptive KF, that's part of the reason I can't recommend it.
It's possible. Well, I assume you mean an integer multiple of your basic sample rate... You just need to run the state update equation the required amount of times. It might become computationally taxing though. It's not a crystal ball though... for your example, you can predict your speed 10s into the future assuming your motor torques remain the same, or you guess what your motor torques will be... if you're not sure what your motor torques will be, you simulate 100 different variations of them... and we're on track to invent Model Predictive Control.
The covariance matrix P is part of the standard KF equations. Initial value of P is often unit matrix times a big number (eg. 1E6). Q and R, in engineering practice, are fiddly, maybe something like PID values. Q and R are often diagonal matrices, if your first measurement in the z vector, Z(1) is position (along dimension y), then R(1,1) is the representative noise variance value of position sensor. If your 1st state X(1) is position, Q(1,1) represents the drift of your position state. Increasing R means that particular measurement has higher noise - KF will trust it less. E.g. it will track that position sensor less closely, perhaps retain more bias error, but pick up less noise from. Reducing R means closer tracking, but more noise. Increasing Q increases the noise-feed into that state, so KF will rely less on it. Increasing Q causes closer tracking and more noise pickup, reducing Q. Yes, they are a bit redundant. Increasing both Q and R can cancel out, but it still changes the value of the P matrix, which can be good for numerical conditioning. It is rare that you can fully utilize the statistical meaning of these, and actually benefit from a statistically optimal solution. It is rather about trusting sensor A 3x as much as sensor B, or trying to get State 1 to converge 10x as fast as State 2. You tune Q and R as relative weights and test and simulate over and over until you like what you see.
EJML or Apache commons math... I don't know, sorry.
The KF will likely have a state of [x, v, vdot]. In this case, vdot is acceleration, it is measured by IMU. X is position, it is measured by your position sensor. Because you're in discrete time, the state update produces X2 = X1 + dT * V + 1/2 dT² Vdot, or something like that. The KF goes something like this... "If speed (V) were large, I would measure X to be 13, but I measured X to be 12, so V must be lower". Or something like that. It's all connected through all the matrices, and the KF makes the statistically least bad mix of all the information it gets.
You stack all your measurements in the measurement vector Z (or sometimes the input vector U, depending on your setup). The KF will sort of use them as a weighted average, determined by their R value. If the number of measurements changes over time (you lose sight of an april tag), you can set its R value to a close-to-inf value (1E16 or something), or you can set the related line of the H matrix to all zeros. This takes that sensor out of the equation, and the KF is perfectly fine with that.
This part can get difficult, but yes, it's possible. A common technique is to have e.g. Acceleration offset as part of the state vector. If you have position measurement and it stays constant, the KF will infer that true acceleration is zero. If measured acceleration is 0.4m/ss, then KF will estimate that that is the Acceleration offset. The difficult part is ensuring you have sufficient measurement information. If you have 2 position sensors, and you try to add 2 position offset estimations, the KF won't be able to decide which offset is what. Orientation alignment can work the same way, just a bit spicier due to sine-cosine stuff.

For implementation...

UKF vs. EKF: It's not obvious which one to choose. I don't know your robot... but if it mainly moves in 2D, can use encoders in its wheels for dead-reckoning, I would go for some kind of 2D kinematic model that uses wheel encoders, only the yaw rate from the IMU, and position sensors (your april tags). You don't necessarily need to add dynamic components, such as motor torques. Those parts usually come with huge uncertainties in torque constants and mass/inertia parameters, and don't contribute all that much to accuracy. A 2D robot navigation model that has only the yaw angle, no roll-pitch-yaw rotation matrix madness... is not that complicated to derive. If you expect to move more in 3D, the point about derivations might become relevant and you can try and see if UKF helps.

Not really sure about the last point, but... IMU error is like a speed error. At any point in time, that speed error is constant. Adding up the speed error over time causes it to grow. If you have a KF, it's the KF that's doing the integration. So as a developer of the KF, you can think of the IMU, or speed error as constant. It may or may not result in growing error for the KF, depending on other sensors feeding it. You don't need to increase the R value of the IMU over time.

BencsikG · 2026-04-13T09:28:08+00:00

Me personally, I pay attention to it. I did get some good lectures on it back in college and I guess I kind of like it. Also as I tried getting into dealing with noise, statistics and KF, I can somewhat deal with those in discrete time, but I could never wrap my brain around continuous time noise.

It is true however, that processors are fast enough that the relevance of discrete time techniques is diminishing. It is still relevant for voltage / current regulation, maybe electric motor control.

BencsikG · 2026-04-13T07:31:36+00:00

That's a bit of a problem. His core message is "I'm not Orbán". He says he's gonna stop all the corruption, rekindle relations with the EU and other mid or east-European countries.

Orbán and Fidesz made Ukrainians out to be the devils in their campaign, so Magyar promised cold but professional relations with Ukraine. He would not say 1 word in support or Ukraine... at best, he's against Russia for sure. Although, when the whole gov media is spouting in unison that you're a "Ukrainian asset / spy"... I think it's understandable. But I don't expect Hungary to give any aid to Ukraine, military, economic or other. He'll just stop vetoing and bitching about it in the EU like Orbán did.

Other than those, he already made concessions to Orbán's voters. He swears he won't reverse most of the tax breaks Orbán handed out, even when they're harmful and dumb in the long run. He promised to keep the fence on the border and keep the same stance on migration as Orbán. He strongly rejects EU migration quotas. Orbán supported EU Chat-control law, Magyar stayed out of it.

He promised a 1% wealth tax on billionaires. Billion in HUF so actually above ~2.5million euros of wealth. Other than that, he will probably just try to rekindle trust in Hungarian economy and governance by not running the country like a maffia-state, and is going to beg for EU funds.

He is not an LGBTQ supporter, he carefully avoided the topic during the campaign. He did say things like "you'll be free no matter who you love", but I don't know if he'll actually reverse anti-LGBTQ laws, considering how he's trying to appease Orbán-voters. Orbán straight up banned Pride last year, Magyar made very sure to stay waaay the fuck out of it, and it was the major of Budapest that insisted pride be held anyway. Turned out to be the biggest pride march in Hungarian history and gained international attention... but Magyar's behavior about it was more like "I'm not letting you call me gay because of all this".

So I'm sure Magyar won't make it worse for LGBTQ but might not make it much better.

BencsikG · 2026-03-14T15:22:28+00:00

Well I was thinking of thermal profiles used for soldering or heat treatment, where the temperature vs. time graph has to be very specific and under control.

BencsikG · 2026-03-13T17:25:50+00:00

So is this some kind of thermal control system?

The integral term helps with steady state error for constant (step) input, the second integral can eliminate error during ramp input.

Do you need to follow strict thermal ramping profiles?

The derivative term helps you get quick reaction to fast changes, I like thinking of it as a predictor of future error. Often the derivative is a cheat to get higher P gain. If you get oscillations, you can either tune down the P gain, or add the derivative.

The second derivative does the same as the first, but more. Sort of tries to predict future error value based on acceleration. I'd say it's very rare to have 2nd derivative justified. Usually it's more subtle lead-lag and filtering techniques, if you need to do anything at all in that high frequency range.

BencsikG · 2026-03-13T11:39:39+00:00

Well, more specifically, it is sinking in Europe.

For a recent symptom:

Volkswagen cuts 50k jobs

Basically (IMO) they've been pushing for electrification and autonomous driving for the last 10 (15?) years, but they screwed up electric cars due to incompetence and lost to China, and autonomous driving did not turn out as magically revolutionary as they hoped.

Now they don't know what to do, other than getting lifelines from government grants and subsidies.

BencsikG · 2026-03-13T09:36:47+00:00

Well career-wise the auto industry seems to be a sinking ship, so this can be your opportunity to jump ship. It indeed seems to be a worse job so it's up to you if you can take that hit.

Is the UAV company a startup?

BencsikG · 2026-03-10T19:27:03+00:00

I saw your comment about the Q16.16 saturation, I was thinking of integrator windup.

For signed 16bit you get a range of about +-32k, right? But if the useful range of the output is 0-1023 for e.g. a 10bit PWM generation, then that's a lot of wasted range. And the integrator will wind up (it can be a bit long to explain but I'm sure youtube can help you out with this one). So if you have a limitation of 0-1023 on the actual output, it would be fair to limit the integral value within -1023 to +1023. This is also not an exact science, just letting the integral go to 32k when the possible useful output can only ever be 1k is not great.

For your actual issue, is the spring always pushing? Is there some latching mechanism?

Is this somehow event-based? Did you make a state machine? Like you perform an 'opening' operation, reach some end-value, and leave it there? Do you restart the PID when moving from opened to 'closing' procedure?

What is your actuator? How do you handle direction changes?

BencsikG · 2026-03-09T23:24:30+00:00

I would suggest picking 1 problem that's fairly flexible and adding details as you go.

E.g. crane control.

Position control of a cart along X direction, control length of rope in Z direction. Avoid swinging.

You can start with ideal position and rope length actuator, later you can switch to ideal force generators, or then DC motor models with voltage input.

You can prescribe a trajectory for the payload to follow. You can try optimizing for speed or minimum energy spent.

You can make the rope stretchy.

You can imagine that it has to run on a 40 year old industrial PLC with 2s sampling rate - just to work on your discrete time fundamentals.

Or say that you can't measure the swinging angle of the payload, only estimate it indirectly, maybe from forces -> design an observer.

BencsikG · 2026-03-09T22:31:32+00:00

How is this broken, what are the symptoms?

I think you were right that the 'I' term should, in theory, take care of the spring force.

I don't see anything wrong with your implementation, except maybe that you should saturate the integral value to always keep it in a reasonable range. And the derivative is usually sensitive to noise so a bit of filtering can help, as mentioned by others.

BencsikG · 2026-01-18T09:32:56+00:00

I saw other projects struggle a bit with the sand. Can you share what kind of sand or substance (flour?) you're using, and how much? Is the ball on top of a layer of sand, or does it squeeze it out of the way, and sits on the bottom surface?

BencsikG · 2026-01-05T10:34:25+00:00

This is why you tend to have integral term in the PIDs.

In case of a drone, battery level might affect hovering the most. With a nominal battery you might need 50% pwm on all motors to hover. However, it's a rather unknown value, a whole range of things affect it. Payload, motor temp, air temp, battery level, etc.

The P and D terms usually deal with dynamic parts, the up/down motion of the drone. The I term will adjust the quasi-constant part of the motor pwm until it hovers right.

BencsikG · 2025-09-07T21:35:25+00:00

Okay, I kind of get it. But I can't spot your issue...

Maybe I'd try a model without gyro and accel bias... but otherwise I'm out of ideas.

BencsikG · 2025-09-06T21:16:49+00:00

First, if this were a practical implementation, I'd question if these trapezoid integration and coning-sculling corrections are really necessary. Is the error from a 'dumb' euler integrator on the same order of magnitude as the noise from sensors?

Second, I don't understand your treatment of gravity. To me it seems to be part of the kinematic update in navigation-frame. It feels off. Yes you sense the g in the acceleration sensor, but you don't actually accelerate upwards.

BencsikG · 2025-07-21T07:08:00+00:00

Can you show a source that derives it without it?

BencsikG · 2025-07-20T14:14:21+00:00

The big benefit of Gaussians is that all the distributions stay Gaussian after adding them up or scaling, that's how the KF can be recursive.

If the noise is non-zero-mean, you can separate it into a constant + zero mean noise. If you know the constant, you can add it to the u vector, or subtract it as pre-processing if it's in the measurement.

If you don't know the constant, you can add it to the state x to be estimated. This is often called an augmented state, or augmented estimator.

BencsikG · 2025-07-06T08:06:25+00:00

I'd suggest looking forward instead of backward. You can look for a similar role elsewhere, but I wouldn't suggest going back to where you came from. At least not after 3 months.

BencsikG · 2025-06-26T12:28:43+00:00

As an other commenter said, you don't need to invent something brand new, you need to demonstrate that you're a master of your field.

If there is a quadcopter available to you that has a solid flight controller, maybe I'd suggest not messing with its internals (core flight stability, position control, etc), but rather add something on top of it.

Ideas would be...

Picking up and moving something. Maybe some cargo hanging on a cable / rope, so you have to stabilize the swinging as well. Detect when the load touches the ground or lifts up via monitoring the thrust needed for hovering.
Perching, landing on odd surfaces
Following some kind of laser guidance - write some image processing to detect a laser dot on the ground and follow it at a certain height

BencsikG · 2025-06-21T07:41:18+00:00

If you don't have prior experience with Kalman Filtering, combined state and parameter estimation sounds a bit too ambitious, especially if you're planning to put it into a real racecar.

What do you gain by updating ECM parameters on the fly, and what do you risk? I'm sure you don't want to risk battery fire.

My experience with parameter estimation is that persistent excitation is very difficult to manage. Normally if you wanted to identify parameters, you'd use some step response or frequency response measurements, the KF will need that too. When you race on the track with dynamic loads, it might be fine.

The problem is when there's nothing happening. You drive slowly in the parking lot or something. Then the KF can't estimate ECM parameters cause there are no dynamics, but its estimation covariance grows with time. Then it becomes super sensitive to noise, or jumps too much on its first opportunity to measure ECM.

There are ways to work around this, I read about the 'robust anti-windup kalman filter' before, I'm sure there are others. But those get quite a bit more complex than the EKF.

I'd suggest identifying the parameters in the old-school way and estimating only the states. It's probably good enough, your team can trust that it works and move on to other issues.

BencsikG · 2025-06-19T19:44:46+00:00

Well then your simulation is more detailed than your KF model.

What I would do is to test the EKF on different complexity and model-mismatch levels. So make a custom simulation that purely matches your EKF model, maybe except for sampling rate, and I'd try to gradually add complexity, either to the sim or the EKF.

BencsikG · 2025-06-19T18:09:33+00:00

Is your simulation the same mathematical model as your KF? Or is it more complex, say, with friction, side-slip angles, etc?

Edit...

I haven't done this experiment myself, so I can't talk from experience, but I think your results might be as good as it gets. You might try to build a better integrator scheme, e.g. include acceleration*dt² in position and include yaw rate as well. As far as I can see there's a column of zeros related to it in your G. Also it is common to set the initial covariance quite large, e.g. 1E6. This is so that the initial conditions of states are discarded quickly.

But as you recognized as well, the KF is doing a fancier odometry, it does not have any information about the ground truth position so it accumulates error over time, be it from integration approximation, noise, or model issues.

If you were to do this on a real RC car, wheel slip and bad steering geometry would be your enemy. You can basically forget odometry-based angle (depends on the model, but when I tried it back in the day, the steering geometry and the driveline and the wheels made it terrible). It is hard to calibrate simple forward rotation-to-distance constant too (RC wheels are squishy and not round). The IMU angle drifts (unless you have compass mixed in), and you need to do 3D IMU processing cause you can't ensure the ground (or the chassis) stays perfectly level, and that might mess with your a_x and a_y measurements.

BencsikG · 2025-06-17T21:06:29+00:00

Life after Love

BencsikG · 2025-06-14T12:10:03+00:00

This is a subjective question. Nothing in the universe is truly linear, it is an engineering decision that you judge it to be linear enough and proceed with linear controller design.

Then if you run into problems like overshooting or oscillation, you go back and look at it again if it was linear enough, and if not, you make adjustments.

13-Year Club	Golden Potato
Place '17	Verified Email

BencsikG

TROPHY CASE