I have a project I'm working on that requires rotational data (yaw, pitch, roll) to be taken from a few different sensors to be combined in code.
The first sensor I have, I can get good angels from, but has a very bad drifting problem.
The second sensor can has very good angles with minimum drifting, but only has a -90 to 90 degree range
of motion.
My question is how can I combine these two sensors data so that I have minimum drifting and a 360° range?
I can provide sample data if needed.
Thanks in advance!
Related
So here's the thing: consider a detector, made of several vertical strings, each hosting 60 sensors distributed equidistantly, as shown in the following picture (dark dots are sensors):
Particles will then flow through and produce pulses on each sensor that you can use. Informations include time of passing by, coordinates (stringnumber+position on string or cartesian coordinates), total charge, etc.
The final goal is to reconstruct angles of incoming particles and their energies. Though we first just consider a simple classification problem of finding out whether the particle came in form the north or south hemisphere (i.e. from the top or bottom direction of the detector.
As input values we use every timestamp of each sensor, their exact position hasn't to be passed as it will be encoded by the position of the timestamp in the input list.
The problem we have is with sensor that didn't detect any particle. Is it clever to insert their timestamp as float("Inf")? Leaving their time as zero is another option, but it will then be ambiguous with the sensor that triggered as first (having timestamps normalised to [0,1]).
I'd be glad to hear any hint from people who have already worked with timestamps in neural networks. Also, if you have any good idea on developing NN's for our future tasks please share them! The planned approach are convolutional (deep) neural networks, but we still have to think about how to encode sensor-positions in that irregular hexagonal shape.
A few points to consider:
- How about -1?
- How about NaN?
What language are you programming in?
My question may seem trivial, but the more I read about it - the more confused I get... I have started a little project where I want to roughly track the movements of a rotating object. (A basketball to be precise)
I have a 3-axis accelerometer (low-pass-filtered) and a 3-axis gyroscope measuring °/s.
I know about the issues of a gyro, but as the measurements will only be several seconds and the angles tend to be huge - I don't care about drift and gimbal right now.
My Gyro gives me the rotation speed of all 3 axis. As I want to integrate the acceleration twice to get the position at each timestep, I wanted to convert the sensors coordinate-system into an earthbound system.
For the first try, I want to keep things simple, so I decided to go with the big standard rotation matrix.
But as my results are horrible I wonder if this is the right way to do so. If I understood correctly - the matrix is simply 3 matrices multiplied in a certain order. As rotation of a basketball doesn't have any "natural" order, this may not be a good idea. My sensor measures 3 angular velocitys at once. If I throw them into my system "step by step" it will not be correct since my second matrix calculates the rotation around the "new y-axis" , but my sensor actually measured an angular velocity around the "old y-axis". Is that correct so far?
So how can I correctly calculate the 3D rotation?
Do I need to go for quaternoins? but how do I get one from 3 different rotations? And don't I have the same issue here again?
I start with a unity-matrix ((1, 0, 0)(0, 1, 0)(0, 0, 1)) multiplied with the acceleration vector to give me the first movement.
Then I want use the Rotation matrix to find out, where the next acceleration is really heading so I can simply add the accelerations together.
But right now I am just too confused to find a proper way.
Any suggestions?
btw. sorry for my poor english, I am tired and (obviously) not a native speaker ;)
Thanks,
Alex
Short answer
Yes, go for quaternions and use a first order linearization of the rotation to calculate how orientation changes. This reduces to the following pseudocode:
float pose_initial[4]; // quaternion describing original orientation
float g_x, g_y, g_z; // gyro rates
float dt; // time step. The smaller the better.
// quaternion with "pose increment", calculated from the first-order
// linearization of continuous rotation formula
delta_quat = {1, 0.5*dt*g_x, 0.5*dt*g_y, 0.5*dt*g_z};
// final orientation at start time + dt
pose_final = quaternion_hamilton_product(pose_initial, delta_quat);
This solution is used in PixHawk's EKF navigation filter (it is open source, check out formulation here). It is simple, cheap, stable and accurate enough.
Unit matrix (describing a "null" rotation) is equivalent to quaternion [1 0 0 0]. You can get the quaternion describing other poses using a suitable conversion formula (for example, if you have Euler angles you can go for this one).
Notes:
Quaternions following [w, i, j, k] notation.
These equations assume angular speeds in SI units, this is, radians per second.
Long answer
A gyroscope describes the rotational speed of an object as a decomposition in three rotational speeds around the orthogonal local axes XYZ. However, you could equivalently describe the rotational speed as a single rate around a certain axis --either in reference system that is local to the rotated body or in a global one.
The three rotational speeds affect the body simultaneously, continously changing the rotation axis.
Here we have the problem of switching from the continuous-time real world to a simpler discrete-time formulation that can be easily solved using a computer. When discretizing, we are always going to introduce errors. Some approaches will lead to bigger errors, while others will be notably more accurate.
Your approach of concatenating three simultaneous rotations around orthogonal axes work reasonably well with small integration steps (let's say smaller than 1/1000 s, although it depends on the application), so that you are simulating the continuous change of rotation axis. However, this is computationally expensive, and error grows as you make time steps bigger.
As an alternative to first-order linearization, you can calculate pose increments as a small delta of angular speed gradient (also using quaternion representation):
quat_gyro = {0, g_x, g_y, g_z};
q_grad = 0.5 * quaternion_product(pose_initial, quat_gyro);
// Important to normalize result to get unit quaternion!
pose_final = quaternion_normalize(pose_initial + q_grad*dt);
This technique is used in Madgwick rotation filter (here an implementation), and works pretty fine for me.
I am working on a software (Ruby - Sketchup) to calculate the radiation (sun, sky and surrounding buildings) within urban development at pedestrian level. The final goal is to be able to create a contour map that shows the level of total radiation. With total radiation I mean shortwave (light) and longwave(heat). (To give you an idea: http://www.iaacblog.com/maa2011-2012-digitaltools/files/2012/01/Insolation-Analysis-All-Year.jpg)
I know there are several existing software that do this, but I need to write my own as this calculation is only part of a more complex workflow.
The (obvious) pseudo code is the following:
Select and mesh surface for analysis
From each point of the mesh
Cast n (see below) rays in the upper hemisphere (precalculated)
For each ray check whether it is in shade
If in shade => Extract properties from intersected surface
If not in shade => Flag it
loop
loop
loop
The approach above is brute force, but it is the only I can think of. The calculation time increases with the fourth power of the accuracy (Dx,Dy,Dazimth, Dtilt). I know that software like radiance use a Montecarlo approach to reduce the number of rays.
As you can imagine, the accuracy of the calculation for a specific point of the mesh is strongly dependent by the accuracy of the skydome subdivision. Similarly the accuracy on the surface depends on the coarseness of the mesh.
I was thinking to a different approach using adaptive refinement based on the results of the calculations. The refinement could work for the surface analyzed and the skydome. If the results between two adjacent points differ more than a threshold value, than a refinement will be performed. This is usually done in fluid simulation, but I could not find anything about light simulation.
Also i wonder whether there are are algorithms, from computer graphics for example, that would allow to minimize the number of calculations. For example: check the maximum height of the surroundings so to exclude certain part of the skydome for certain points.
I don't need extreme accuracy as I am not doing rendering. My priority is speed at this moment.
Any suggestion on the approach?
Thanks
n rays
At the moment I subdivide the sky by constant azimuth and tilt steps; this causes irregular solid angles. There are other subdivisions (e.g. Tregenza) that maintain a constant solid angle.
EDIT: Response to the great questions from Spektre
Time frame. I run one simulation for each hour of the year. The weather data is extracted from an epw weather file. It contains, for each hour, solar altitude and azimuth, direct radiation, diffuse radiation, cloudiness (for atmospheric longwave diffuse). My algorithm calculates the shadow mask separately then it uses this shadow mask to calculate the radiation on the surface (and on a typical pedestrian) for each hour of the year. It is in this second step that I add the actual radiation. In the the first step I just gather information on the geometry and properties of the various surfaces.
Sun paths. No, i don't. See point 1
Include reflection from buildings? Not at the moment, but I plan to include it as an overall diffuse shortwave reflection based on sky view factor. I consider only shortwave reflection from the ground now.
Include heat dissipation from buildings? Absolutely yes. That is the reason why I wrote this code myself. Here in Dubai this is key as building surfaces gets very, very hot.
Surfaces albedo? Yes, I do. In Skethcup I have associated a dictionary to every surface and in this dictionary I include all the surface properties: temperature, emissivity, etc.. At the moment the temperatures are fixed (ambient temperature if not assigned), but I plan, in the future, to combine this with the results from a building dynamic thermal simulation that already calculates all the surfaces temperatures.
Map resolution. The resolution is chosen by the user and the mesh generated by the algorithm. In terms of scale, I use this for masterplans. The scale goes from 100mx100m up to 2000mx2000m. I usually tend to use a minimum resolution of 2m. The limit is the memory and the simulation time. I also have the option to refine specific areas with a much finer mesh: for example areas where there are restaurants or other amenities.
Framerate. I do not need to make an animation. Results are exported in a VTK file and visualized in Paraview and animated there just to show off during presentations :-)
Heat and light. Yes. Shortwave and longwave are handled separately. See point 4. The geolocalization is only used to select the correct weather file. I do not calculate all the radiation components. The weather files I need have measured data. They are not great, but good enough for now.
https://www.lucidchart.com/documents/view/5ca88b92-9a21-40a8-aa3a-0ff7a5968142/0
visible light
for relatively flat global base ground light map I would use projection shadow texture techniques instead of ray tracing angular integration. It is way faster with almost the same result. This will not work on non flat grounds (many bigger bumps which cast bigger shadows and also change active light absorbtion area to anisotropic). Urban areas are usually flat enough (inclination does not matter) so the technique is as follows:
camera and viewport
the ground map is a target screen so set the viewpoint to underground looking towards Sun direction upwards. Resolution is at least your map resolution and there is no perspective projection.
rendering light map 1st pass
first clear map with the full radiation (direct+diffuse) (light blue) then render buildings/objects but with diffuse radiation only (shadow). This will make the base map without reflections and or soft shadows in the Magenta rendering target
rendering light map 2nd pass
now you need to add building faces (walls) reflections for that I would take every outdoor face of the building facing Sun or heated enough and compute reflection points onto light map and render reflection directly to map
in tis parts you can add ray tracing for vertexes only to make it more precise and also for including multiple reflections (bu in that case do not forget to add scattering)
project target screen to destination radiation map
just project the Magenta rendering target image to ground plane (green). It is only simple linear affine transform ...
post processing
you can add soft shadows by blurring/smoothing the light map. To make it more precise you can add info to each pixel if it is shadow or wall. Actual walls are just pixels that are at 0m height above ground so you can use Z-buffer values directly for this. Level of blurring depends on the scattering properties of the air and of coarse pixels at 0m ground height are not blurred at all
IR
this can be done in similar way but temperature behaves a bit differently so I would make several layers of the scene in few altitudes above ground forming a volume render and then post process the energy transfers between pixels and layers. Also do not forget to add the cooling effect of green plants and water vaporisation.
I do not have enough experience in this field to make more suggestions I am more used to temperature maps for very high temperature variances in specific conditions and material not the outdoor conditions.
PS. I forgot albedo for IR and visible light is very different for many materials especially aluminium and some wall paintings
I am trying to implement a simple kalman filter that will be used for filtering/predicting the movement of a vehicle in long/loat coordinates.
There are no measurements from vehicle sensors, just a new update on an observed long/lat position, so basically the state that I will be trying to predict and correct is the longitude and latitude of the vehicle at any given time.
As far as I understand the model is non linear as there may be random accelerations change of direction etc, but I think this can be largely ignored as long as I keep track of the bearing as well in my state. My problem is that I do not know how to model this system in terms of the state and prediction matrices and on top of this it seems that it is necessary to convert/project the long/lat coordinates into some cartesian xy system so that the two become independent, but I am not exactly sure how to go about this.
It seems that converting back to wgs84 from xy is not that trivial and potentially a bit intense computationally. Can anyone shed some light into this?
It looks like your state variable would be a vector [lat, long], and your measurement variable would be [lat, long, bearing]. You will need to figure out appropriate f and h functions, based on these vectors, for the process and measurement models, respectively. Since this is a non-linear problem, you will probably need to use a nonlinear filter, such as the EKF, UKF or CKF (cubature Kalman filter).
When using a Kalman-type filter to deal with modular values such as angles, latitudes, or longitudes, there is a big problem whenever your state or your measurements are near the discontinuous modular boundary. For example, if your bearing is an angle from 0 to 360 degrees, the filter will have problems if you are measuring at 1 degree or 359 degrees. Also, you could have problems if your longitude is around plus or minus 180 degrees longitude, or your latitude is at one of the poles (which might be remote possibilities that you could ignore).
One example of how to deal with angles in the state or measurement variables is presented in David Frederic Crouse, "Cubature/ Unscented/ Sigma Point Kalman Filtering with Angular Measurement Models," 18th Int'l Conf. on Information Fusion (Wash., D.C. July 6-9, 2015), https://ieeexplore.ieee.org/document/7266741. Based on Crouse's approach, to use bearing as a measurement variable you would need to add a wrapping function to a couple of locations in the standard unscented or cubature filter equations. If you want to deal with the discontinuities in latitude and longitude (which you might not), you would need to transform the coordinates to and from Euclidean space while calculating the covariance matrices.
Current situation:
I have implemented a particle filter for an indoor localisation system. It uses fingerprints of the magnetic field. The implementation of the particle filter is pretty straight forward:
I create all particles uniformly distributed over the entire area
Each particle gets a velocity (gaussian distributed with the mean of a 'normal' walk speed) and a direction (uniformly distributed in all directions)
Change velocity and direction (both gaussian distributed)
Move all particles in the given direction by velocity multiplied by the time difference of the last and the current measurement
Find the closest fingerprint of each particle
Calculate the new weight of each particle by comparing the closest fingerprint and the given measurement
Normalize
Resample
Repeat #3 to #9 for every measurement
The problem:
Now I would like to do basically the same but add another sensor to the system (namely WiFi measurements). If the measurements would appear at the same time there wouldn't be a problem. Then I would just calculate the probability for the first sensor and multiply this by the probability of the second sensor to get my weight for the particle at #6.
But the magnetic field sensor has a very high sample rate (about 100 Hz) and the WiFi measurement appears roughly every second.
I don't know what would be the best way to handle the problem.
Possible solutions:
I could throw away (or average) all the magnetic field measurements till a WiFi measurement appears and use the last magnetic field measurement (or the average) and the WiFi signal together. So basically I reduce the sample rate of the magentic field sensor to the rate of the WiFi sensor
For every magnetic field measurement I use the last seen WiFi measurement
I use the sensors separated. That means if I get a measurement of one sensor I do all the steps #3 to #9 without using any measurement data of the other sensor
Any other solution I haven't thought about ;)
I'm not sure which would be the best solution. All the solutions dont seem to be good.
With #1 I would say I'm loosing information. Although I'm not sure if it makes sense to use a sample rate of about 100 Hz for a particle filter.
At #2 I have to assume that the WiFi signal doesn't chance quickly which I can't prove.
If I use the sensors separately the magnetic field measurements become more important than the WiFi measurements since all the steps will have happened 100 times with the magnetic data till one WiFi measurement appears.
Do you know a good paper which is dealing with this problem?
Is there already a standard solution how to handle multiple sensors with different sample sizes in a particle filter?
Does a sample size of 100 Hz make sense? Or what would be a proper time difference for one step of the particle filter?
Thank you very much for any kind of hint or solution :)
In #2 instead of using sample-and-hold you could delay the filter by 1s and interpolate between WiFi-measurements in order to up-sample so you have both signals at 100Hz.
If you know more about the WiFi behavior you could use something more advanced than linear interpolation to model the Wifi behavior between updates. These folks use a more advanced asynchronous hold to up-sample the slower sensor signal but something like a Kalman filter might also work.
With regards to update speed I think 100Hz sounds high for your application (assuming you are doing positioning of a human walking indoors) since you are likely to take a lot of noise into account, lowering the sampling frequency is a cheap way to filter out high-frequency noise.