Kalman filter

Go To Last Post
54 posts / 0 new
Author
Message
#1
  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

Hi just made a Kalman filter in Matlab :P

function uav()

dt = 0.1;
duration = 10;
a = [1 -dt; 0 1];               % transition matrix                                         
b = [dt; 0];                    % input matrix
c = [1 0];                      % measurement matrix
x = [0; 0];                     % initial state vector      X = [ angle, bias ]
P = [1 0; 0 1];
R = 0.1;                        % measurment covariance noise {R=0 Perfect follow}
Q = [0.001 0; 0 0.003];         % process covariance noise (Q_angle	= 0.001 Q_gyro	= 0.003)
x_noise = 0:dt:duration;
n = 1;

mag = [];
vig = [];

for t = 0 : dt: duration
    
    %y_noise(n) = x_noise(n)*x_noise(n)-2*x_noise(n);
    %za_noise(n)= (x_noise(n)*x_noise(n)-2*x_noise(n))+(2*rand-1)*0.2;
    %zg_noise(n)=(x_noise(n)*x_noise(n)-2*x_noise(n))+(2*rand-1)*0.2;
    
    y_noise(n) = sin(x_noise(n));
    za_noise(n)= sin(x_noise(n))+(2*rand-1)*0.2;
    zg_noise(n)= sin(x_noise(n))+(2*rand-1)*0.2;
    
    u = za_noise(n);
    y = zg_noise(n);
        x = a*x + b*u;              
    P = a*P*a' + Q;             
    Inn = y - c*x;                % accelerometer
    S = c*P*c' + R;             
    K = a*P*c'*inv(S);            
    x = x + K*Inn;                
    P = a*P*a'-a*P*c'*inv(S)*c*P*a
    mag=[mag;x(1)];
    vig=[vig;x(2)];
    n=n+1;
end



%plot(x_noise,y_noise, x_noise,za_noise, x_noise, zg_noise)

%plot(x_noise,za_noise,'b',  x_noise,zg_noise,'b',  x_noise, mag,'r');%,  x_noise, vig,'g') 

plot(x_noise,y_noise,'b', x_noise, mag,'r')

Does anybody have experience with Kalmanfiltering? Because I'm not impressed with the accuracy of the filter I made, and are woundering if I have made any mistakes?? :?

Magnus T

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

Off-topic for this forum - completed Academy projects only!

Leon

Leon Heller G1HSM

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

Hi,

i had a short look on your code. I think the calculation of the covariance of the measurement and process noise is not correct, because the definition of it is

cov(x) = E[x * xT]

therefore you need at least to square your R and your Q.

bye
tmo

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

forgot,

you should use randn, because you need normally distributed noise.

bye

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

Hi,

i played a little with your code and changed it a little. Now it looks good for me. The filter now integrates the gyro sinus input (°/s) to a proper angle value. I attached the modified m-file.

I you have further improvements, i'll be interrested.

Bye
tmo

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

My browser is unable to attach a file :(

Here is the code:

function uav() 

dt = 0.1; 
duration = 1000; 

measnoise = 0.1;
procnoise = 0.01;

a = [1 -dt; 0 1];               % transition matrix
b = [dt; 0];                    % input matrix 
c = [1 0];                      % measurement matrix 

x = [0; 0];                     % initial state vector      X = [ angle, bias ] 
x_est = x;                      % initial estimation equals initail state vector

Q = procnoise^2 * [dt^2 0; 0 0];% process covariance noise
P = Q;                          % initial estimation covariance
R = 0.1;                        % measurment covariance noise {R=0 Perfect follow} 

x_axis = 0:dt:duration; 
n = 1; 

mag = []; 

for t = 0 : dt: duration 
    input(n) = sin(x_axis(n)); 

    % Process modell
    za_noise(n)= input(n) + (procnoise * randn); 
    x = a*x + b*za_noise(n);
    
    % Measurement modell
    zg_noise(n)= (measnoise * randn); 
    y =  c*x + zg_noise(n); 

    % Innovation
    Inn = y - c*x_est;

    % Covariance of innovation
    S = c*P*c' + R;              
    
    % optimal Kalman gain
    K = a*P*c'*inv(S);
    
    % new State estimation
    x_est = x_est + K*Inn;
    
    % Covariance of prdiction error
    P = a*P*a' + Q;
    P = P - a*P*c'*inv(S)*c*P*a';
    
    mag=[mag;x_est(1)]; 
    n=n + 1; 
end 

plot(x_axis, input,'b', x_axis, mag,'r');
  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

There was a problem with wrong x_est and x usage:

function uav() 

dt = 0.1; 
duration = 500; 

measnoise = 0.1;
procnoise = 0.01;

a = [1 -dt; 0 1];               % transition matrix
b = [dt; 0];                    % input matrix 
c = [1 0];                      % measurement matrix 

x = [0; 0];                     % initial state vector      X = [ angle, bias ] 
x_est = x;                      % initial estimation equals initail state vector

Q = procnoise^2 * [dt^2 0; 0 0];% process covariance noise
P = Q;                          % initial estimation covariance
R = measnoise^2;                % measurment covariance noise {R=0 Perfect follow} 

x_axis = 0:dt:duration; 
n = 1; 

mag = []; 

for t = 0 : dt: duration 
    input(n) = sin(x_axis(n)); 

    % Process modell
    za_noise(n)= input(n) + (procnoise * randn); 
    x = a*x_est + b*za_noise(n);
    
    % Measurement modell
    zg_noise(n)= (measnoise * randn); 
    y =  c*x + zg_noise(n); 

    % Innovation
    Inn = y - c*x;

    % Covariance of innovation
    S = c*P*c' + R;              
    
    % optimal Kalman gain
    K = a*P*c'*inv(S);
    
    % new State estimation
    x_est = x_est + K*Inn;
    
    % Covariance of prdiction error
    P = a*P*a' + Q;
    P = P - a*P*c'*inv(S)*c*P*a';
    
    mag=[mag;x_est(1)]; 
    n=n + 1; 
end 

plot(x_axis, input,'b', x_axis, mag,'r');

bye

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

last update. Ilove this math stuff :D

function uav() 

dt = 0.1; 
duration = 60; 

measnoise = 0.0001;
procnoise = 0.05;

a = [1 -dt; 0 1];               % transition matrix
b = [dt; 0];                    % input matrix 
c = [1 0];                      % measurement matrix 

x = [0; 0];                     % initial state vector      X = [ angle, bias ] 
x_est = x;                      % initial estimation equals initail state vector

Q = procnoise^2 * [dt^2 0; 0 0];% process covariance noise
P = Q;                          % initial estimation covariance
R = measnoise^2;                % measurment covariance noise {R=0 Perfect follow} 

x_axis = 0:dt:duration; 
n = 1; 

mag = []; 

for t = 0 : dt: duration 
    input(n) = sin(x_axis(n)); 

    % Process
    za_noise(n)= input(n) + (procnoise * randn); 
    
    % Time estimation    
    x = a*x_est + b*za_noise(n);
    
    % Measurement estimation
    zg_noise(n)=(measnoise * randn); 
    y = c*x + zg_noise(n); 

    % Innovation
    Inn = y - c*x_est;

    % Covariance of innovation
    S = c*P*c' + R;              
    
    % optimal Kalman gain
    K = a*P*c'*inv(S);
    
    % new State estimation
    x_est = x_est + K*Inn;
    
    % Covariance of prdiction error
    P = a*P*a' + Q;
    P = P - a*P*c'*inv(S)*c*P*a';
    
    mag=[mag;x_est(1)]; 
    n=n + 1; 
end 

plot(x_axis, za_noise,'b', x_axis, mag,'r');

have fun

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

Hi tmo!
Thanks alot for your help!

My plan is to build a UAV like this: http://www.mikrokopter.com/ucwiki/MikroKopter. for stabilizattion I'm thinking of using this http://www.sparkfun.com/commerce/product_info.php?products_id=741#.

So I get both gyro and accelerometer input, and I'm thinking of using the kalmanfilter for filtering the input and preventing gyro drift.

As far as i can se the code don't simulate this anymore.. (for all i know, my code never did that..)

I'll keep you posted!!

Magnus T

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0
My plan is to build a UAV like this: http://www.mikrokopter.com/ucwiki/MikroKopter. for stabilizattion

I wish they had the wiki in English too! Who knows German :)

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

Hi,

yes you are right. The code now simulates only for gyro input (angle/s). There is no acc input. I can extend this for you, if you need this.

bye

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

I know commercial Attitude and Heading Reference Systems have 3 rate gyros and 3 accelerometers. I suppose their job is to output pitch, roll, yaw, and heading 20 or 30 times a second with some specified accuracy even during manuvers of several Gs for several seconds. I suppose current flight control systems and autopilots are all digital, and using Dr Kalman's filter is accepted as a Best Practice, but I bet there were several analog autopilots that did a good job before Kalman filters were invented, and they did it all with 741s and 2N2222s. Of course, they had electromechanical rate gyros and accelerometers. I bet
a real good AHRS can be built with MEMS gyros and accelerometers and perhaps 'simulations' of the analog filters used. I 'almost' have an idea how to keep track of pitch roll yaw and heading given the rates and accels, but if you gave me the commented source for the Kalman filter, I could get as far as pressing the compile button in the IDE, and if it output garbage, I wouldnt have a clue of how it worked in order to debug it. A man's gotta know his limitations. Dirty Harry, 1974

Imagecraft compiler user

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

As far as I know, Kalman filters for navigation purposes are used mostly because they can blend the data from GPS and IMU (inertial measurement units) to give accurate position and heading, while IMU is used for short term corrections to heading, yaw, pitch and roll (generally, only the gyro parts), thus act as the 'main autopilot'. And if the plane/heli/ship/Millenium Falcon is correctly modeled, Kalman can give the exact values to the controls to mantain the desired heading.

Guillem.
"Common sense is the least common of the senses" Anonymous.

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

Hi tmo!

If you could help me I would be gratefull!!

magnus T

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

hi magnusrt,

i thought about a "correct" way for integrating gyro and accmeter to your filter. I think the "right" way is to change your process modell. As i understand your current model only reads gyro and integrates it one time to angle (the simulation shows a good filter performance for this task). The second part of your control input must be the accmeter input. This should be integrated 2 times for getting also an angle information. Last day I started with a correction of the process modell according to that. I hope i will finish tomorrow. (I hope I'am right and you want to get only angle information and not an absolut plane position in 3D according to start position (this will be 3D flight control) ?)

bye
tmo

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

Depends on if you are building an AHRS or an IMU. AHRS outputs attitude and heading. IMU puts out position. In the AHRS, the accelerometers are used to determine when vehicle is manuvering, or straight and level... then the gyros can be 'caged' to correct accumulated drift. The mechanical gyros had a mechanical 'choke control' knob that you 'pull to cage'. The electronic ones just require storing zero in pitch, roll, and yaw.

Imagecraft compiler user

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

Hi tmo!

I guess the angle is the best way to keep the UAV stable. The UAV's absolut plane position in 3D according to start position is not of interest.

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

hi magnusrt,

i checked your links of gyro and copter. These applications use all an AVR for control, but they don't use kalman filters, because of the havy math. The microcopter simply integrates the gyro signals and checks for long time drift against the acc sensors. This saves processor power.

I think a kalman filter on an avr is heavy load, because of the needed matrix math and divisions. What do you think ?

bye

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

How fast would the filter have to run to keep the plane level? 20 times a sec? 50 times a sec? 20 passes/sec is 50ms... I think a 16mhz avr can do about 40fp adds or mults a ms (40K per sec or so), so there is time to do about 1000fp ops and use half the cpu. I assume the 3D matrix ops use a 4x4 matrix... any estimate of how many fp ops to run one pass of the filter? or run one matrix mult or add?

Imagecraft compiler user

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0
 I assume the 3D matrix ops use a 4x4 matrix... any estimate of how many fp ops to run one pass of the filter? or run one matrix mult or add?

I know a 4 by 4 matrix cross a 4 by 4 matrix requires 2*(4^2) mutliplies or 32 multiplies, if that helps.

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

Hi!

I have very litle experience with AVR and electronics, so i dont know the limitations. But I found this http://tom.pycke.be/mav/92/kalman-demo-application.
If I simplify the filter, and try to get as few calculations as possible I was hoping it would be possible.

In the begining I was thinking of using Matlab for running the filter and controlling the UAV.

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

Hi,

i had a look on the code from that link. Now i understand your first posting. It was based on that code. Ok, the kalman filter is clear: Predict with gyro and update with acc. But here i have a problem, I am not a specialist for planes, how will you calculate the roll, pitch, gear from of a three axis accmeter under dynamik conditions. The provided code assumes steady situation, then the arctan() calculation with z-axis as ground reference will do the job, but under dynamic conditions, i think you can not determine the roll, pitch gear angles from the arctan(). In other words ground isn't longer ground for the accmeter (because of vector addition from the dynamic acc's to the -1g from earth). How can this be solved ? Or am i on a wrong way ?

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

Hi tmo

On the pyke webpage he has an article about this problem, and I also think he only uses a steady state.
I guest that it was a good enough estimation for the dynamic system.. and I don't have any other ways to do it...

But thats maybe not good enough..

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

hi,

i read a lot of articles about that problem and most of them say it is not enough for long term stabilisation. The pilot needs to correct manually. The next problem is to detect the steady state for calculation the bias of the gyros.

Do you have some ideas for that ?

bye

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

Hi

My idea is to make a UAV wich I will controll via remote. I don't know if it is necessary with kalman filtering, but I thought it was a good way to do it.

I'm not sure how to calculate the bias of the gyro, but was thinking that if I could make the filter in Matlab, and use matlab to control the UAV. I can collect data and use matlab to calculate the bias... maby use the "fit" function in matlab.

I'm sure how to do it but maybe this could be a posibility.

Trial and error is also possible. Make a program in matlab that will try all possible combination, and visually deside when the UAV is the most stable...

I'm studying medicine and have very littel experience with these kind of things, so I'm learning by doing....

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

The magnitude of the Gs is sqrt(xsq+ysq+zsq). If the plane is level, x and y are zero, because they are pointing out front and out one wing. Z is -1 or 1 depending on your point of view and if you are left handed or right handed. If you are turning, like a coordinated turn, the total Gs would be like 1.5 or something, but the pitch and roll tilt calcs using atan2 would be 'fooled' into thinking the coordinated turn was a level turn. So the idea is, use the pitch and roll tilt computed from the accelerometers to cross check the pitch and roll computed by integrating the pitch and roll rates from the gyros, which drift. If you get a second or so of level flight within a few tenths of a G from 1, you can assume the pitch and roll tilt angles from the accellerometers are valid, and stuff that angle into the gyro angle. "Electronic cage" of the gyros.

Imagecraft compiler user

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

Hi tmo

How is the work on the filter going?

magnus

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

How do you leverage the knowledge gained with matlab to help with the avr uav program? Surely one cant write a matlab emulator to run on the avr. Maybe the 'top down bottom up' approach works here... matlab at the top and reading 3 rate gyros and accumulating the deltaGs and printing out pitch roll and yaw and tilt the thing around and see how bad it drifts. Then try that electronic cage trick I described a couple of messages ago... you need to know when its level in pitch and roll and not pulling Gs so you can zero out the gyro pitch and roll.

Imagecraft compiler user

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

hi magnusrt,

i have now a working matlab model, simulating gyro with bias and 3 axis acc sensor. I have to do some small modifications and hope to post it the next days.

bye
tmo

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

hi magnusrt,

here is the promised matlab plane model for one axis, with working kalman filter on noisy and biassed senosors. Have fun ...

dT = 0.1;
duration = 60;
cycles = 5;

gyroBiasDot = 0.1;
angleNoise = 0.025;
gyroNoise = 0.7;

x = [0; 0];

a = [1 -dT; 0 1];
b = [dT; 0];
Q = [0 0; 0 gyroNoise];
R = angleNoise;
P = Q;
H = [1 0];
I = [1 0; 0 1];

anglePlaneLog = [];
gyroPlaneLog = []; 
angleRealLog = [];
gyroRealLog = [];
angleKalman = [];
biasKalman = [];

gyroBias = 0;
n = 1;
time = 0:dT:duration;

for t = 0:dT:duration
    time(n) = time(n) + 1;

    anglePlane = sin(cycles * (2 * pi * t) / duration);                     % real world plane model (with ideal sensors)
    gyroPlane = cos(cycles * (2 * pi * t) / duration);

    gyroBias = gyroBias  + (dT * gyroBiasDot);                              % sensor signals (noisy and bias), derived from real world plane model
    gyroReal = gyroPlane + gyroBias + (((2 * rand) - 1) * gyroNoise);
    angleReal = anglePlane + (((2 * rand) - 1) * angleNoise);
    
    x = a * x + b * gyroReal;                                               % kalman filter prediction part
    P = a * P * a' + Q;
                                                                            
    inn = angleReal - H * x;                                                % kalman filter update part
    S = H * P * H' + R;
    K = P * H' * inv(S);
    x = x + K * inn;
    P = (I - K * H) * P;
    
    anglePlaneLog = [anglePlaneLog; anglePlane];                            % data logging for plot
    gyroPlaneLog = [gyroPlaneLog; gyroPlane];

    gyroRealLog = [gyroRealLog; gyroReal];
    angleRealLog = [angleRealLog; angleReal];
    
    angleKalman = [angleKalman; x(1)];
    biasKalman = [biasKalman; x(2)];
    n = n + 1;    
end    

subplot(2,1,1), plot(time, gyroRealLog, 'b', time, angleRealLog, 'r', time, angleKalman, 'y', time, biasKalman, 'c');
subplot(2,1,2), plot(time, anglePlaneLog, 'g', time, gyroPlaneLog, 'c', time, angleKalman, 'b', time, biasKalman, 'r');

bye tmo

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

small improvement, for better visualization ...


dT = 0.1;                                                                   % rate for prediction part
duration = 60;                                                              % simulation time
inFreq = 0.1;                                                               % frequency of sinus input
updatePrescaler = 10;                                                       % prescaler of rate for update part

gyroBiasDot = 0.1;
angleNoise = 0.05;
gyroNoise = 0.5;

x = [0; 0];

a = [1 -dT; 0 1];
b = [dT; 0];
Q = [0 0; 0 gyroNoise];
R = angleNoise;
P = Q;
H = [1 0];
I = [1 0; 0 1];

anglePlaneLog = [];
gyroPlaneLog = []; 
angleRealLog = [];
gyroRealLog = [];
angleKalman = [];
biasKalman = [];

gyroBias = 0;
n = 1;
time = 0:dT:duration;

prescaler = updatePrescaler;

for t = 0:dT:duration
    anglePlane = sin(inFreq * 60 * (2 * pi * t) / duration);                % real world plane model (with ideal sensors)
    gyroPlane = cos(inFreq * 60 * (2 * pi * t) / duration);

    gyroBias = gyroBias  + (dT * gyroBiasDot);                              % sensor signals (noisy and bias), derived from real world plane model
    gyroReal = gyroPlane + gyroBias + (((2 * rand) - 1) * gyroNoise);
    angleReal = anglePlane + (((2 * rand) - 1) * angleNoise);
    
    x = a * x + b * gyroReal;                                               % kalman filter prediction part
    P = a * P * a' + Q;
                                                                            
    prescaler = prescaler - 1;                                              % kalman filter update part

    if (prescaler == 0)
        prescaler = updatePrescaler;
    
        inn = angleReal - H * x;
        S = H * P * H' + R;
        K = P * H' * inv(S);
        x = x + K * inn;
        P = (I - K * H) * P;
    end        
    
    anglePlaneLog = [anglePlaneLog; anglePlane];                            % data logging for plot
    gyroPlaneLog = [gyroPlaneLog; gyroPlane];

    gyroRealLog = [gyroRealLog; gyroReal];
    angleRealLog = [angleRealLog; angleReal];
    
    angleKalman = [angleKalman; x(1)];
    biasKalman = [biasKalman; x(2)];
    n = n + 1;    
end    

subplot(2,1,1), plot(time, gyroRealLog, 'b', time, angleRealLog, 'r', time, angleKalman, 'y', time, biasKalman, 'c');
subplot(2,1,2), plot(time, anglePlaneLog, 'g', time, gyroPlaneLog, 'c', time, angleKalman, 'b', time, biasKalman, 'r');

bye

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

How about using Kalman filter in tracking application. I planning to do sensor network system which listens RSSI (Received Signal Strength Indication) information.

In the application there is at least three transmitter and one receiver. The transmitters are fixed and they have measured locations in XY coord. Receiver moving inside the network and polls RSSI information each transmitter.

Then we can calculate the location and the velocity using receiver RSSI values.

I use CC2420 radio on my prject and that RSSI value is not linear. That probably makes me project difficult and imprecise. using more than three transmitters will give better results.

Probably I must use moving average filter to smoothing noise when receiver is in place. there is also two axis accelerometer on my board.

Is there any change to use Kalman filter on this application? I've heard GPS-receivers use Kalman filter :lol:

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

Hi,

yes, that seems to be a perfect situation for an KF. You need to build your linear state space model, based on your accelerometers (that's the prediction part -> simple kinematics model -> one time integration of acc is speed, two times integration of acc is distance). Then you can stabilize your prediction with your position measurements based on your RSSI information (that's the update part). Then you can play arround with Q and R to trim your kalman filter.

The state vector may be (distance speed acc)'.

bye tmo

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

Hi tmo

Thanks alot for the help with the filter!

I simplified the code and put it on a mega88 @ 20MHz, and it did about 20 calculation per secound!!

By converting doubles to int I can probably increase the speed futher.

I'll keep on optimizing!!

magnus

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

I am not an expert on the Kalman filter, but I think there is one more point for reducing the calculations.
When you are in steady state, meaning that the filter has run long enough that the initial state does not matter anymore, the covariance matrices should be constant. The covariance matirces are calculated independent of the measured values, thus one could do all the calulation of the covariance matrices upfront during compile-time. Probably one can live with using only the stady state values. And anyway they are adjustable parameters one has to estimate by some other methods or just guess.
My estimate is that this would reduce the number of calculation to about 1/4 or even less.

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

I'm making an inverted pendulum balancing robot and I would like to know if the last piece of code tmo posted will give a true tilt angle after being transfered to my microcontroller. I am assuming anglePlane is the accelerometer reading and gyroPlane is the raw gyro reading.

Many thanks!

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

Hi wasssup1990,

anglePlane and gyroPlane are a sinmple ideal real world model of one plane axis. gyroReal and angelReal represent the readings from real sensors (these values derived from gyroPlane and anglePlane, are noisy and in case of the gyro with a bias).

Hope this helps

tmo

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

Hello tmo,

I notice that "angleKalman" is closely following the angle plane. Why? How does the accelerometer sense angle when the UAV or balancing robot is falling or moving around?

Thanks :)

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

hi,

this is the nature of the kalman filter. This filter can combine different sensors to increase the accurancy of the desired information.

In the plane case the accmeter on two axis can be used to calculate the angle of the plane in one axis by usage of the arctan() and some simple vector math (notice the earth acc is always measured !!). Then this value can be used to increase the gyro acurancy and calculate the bias of the gyro.

bye

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

Cool! That's what I need help with.

Can you teach me or point me in the right direction on how I can use the two axis in my accelerometer to provide tilt angle?

Thanks! :D

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

You don't need a kalman filter to get the tilt angle. This is simple trigonometry, just the arctan(ax/ay) and maybe some sign handling if one works beyond +-90 Deg.
Of cause this does not always work in a moving, or more accurately accelerating, vehicle or plane.

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

But it is easy to detect the moving case from the not moving case. If you are not moving, then the sqrt(xGs*xGs + yGs*yGs) is 1. If you are moving, it is not 1.

Imagecraft compiler user

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

@Bob:
Its little less: If the sum is not 1 then you are moving, or tilt toward the 3rd axis.
One example to get 1g in a moving system is to follow a spiral down. This can have 1g acceleration outward and free fall (0g) downwards.

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

But if you had a 3 axis accelerometer, when in a downward spiral, the sqrt of the sum of the squares of the X Y and Z Gs would not be 1. I thought we were describing a much simpler system with only 2 accelerometers trying to detect tilt in one axis.

Imagecraft compiler user

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

Thanks for replying I'll see what I can do with the formulae posted above.

You see, what I am making is an inverted pendulum balancing robot and I obviously need a responisive alogarithm to calculate tilt angle very quickly. I have the kalman filter code already in my microcontroller but getting the absolute tilt angle from my accelerometers is proving to be difficult without some form of compensation when the robot is moving around. I'll see if the forumlae posted above can rectify the problem, or can someone suggest something to solve my problem?

All the best! :)

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

Ok I have just found some C source code that is supposed to grab readings from the gyro and accelerometer to give a tilt angle. The code is pretty straight forward; you have two functions there and thier role is heavly commented.

This code is different from the old kalman filter code I had in my uC and I'm thinking of replacing it with this code. Should work right? I just hope I program the uC in the right way to manipulate the raw sensor data before it is sent to this kalman filter, only if that is required.

I just thought this code might be usefull for us to talk about to get my robot going.

Waiting eagerly for a reply. Thanks! :D

Attachment(s): 

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

Maybe using an Analog Devices rate gyro would give you a tilt angle that wasnt as sensitive to fwd and backward acceleration?

Imagecraft compiler user

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0
  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

Hi,
From the Sparkfun site mpthompson links to working code avr_imu_3
http://forum.sparkfun.com/viewto...
which runs the tilt code or similar that you mention above on an ATMega168 at 20MHz. Easily modified for other ATMega devices. The code works well even when using MMA7260Q in place of the ADXL330 on the IMU5dof that you have.
Regards
Dez Ellis

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

I try to estimate place using velocity. how do I calculate measurement and process covariances? The system model (A matrix) is a time invariant so covariance(s) must be time variant(s).

function [poshat] = kalman(time,r, v, p)

% time = time vector 
% r = distance vector input
% v = upper limit for process (velocity) noise [m/10msec]
% p = upper limit for position measurement noise [m]
i = 2;                %time index
td = (time(i) - time(i-1)); %generate time steps
                                                  
M = [1 0; 0 1];           % System model A matrix  
x = [0 0]';               % Initial state 
xhat = x;                 % Initial state estimate

C = [1 0];                % Observation model

%???
W = v * [td^2 td; td 1]; % Process noise covariance ???
N = p; % Measurement noise covariance 
       % N=0 Perfect follow

P = W;   % Initial estimate covariance

pos = [];     % Position
posmeas = []; % Measured position
poshat = [];  % Estimated position 


for t = 0 : td : max(time),
    
    x = [r(i) (xhat(1) - r(i))/td]'; %place and velocity
    
    w = v * [td; 1];     % Process noise
    x = M * x + w;       % Simulate the system
    
    n = p;               % Measurement noise
    z = C * x + n;       % Simulate the measurement
    
    y = z - C * xhat;   % Innovation of measurement residual  
    S = C * P * C' + N; % Innovation covariance
    K = M * P * C' * inv(S);  % Optimal Kalman gain
    P = M * P * M' + W - M * P * C' * inv(S)* C * P * M';  % Estimate covariance
    
    xhat = M*xhat + K * y;                % State estimate using the Kalman filter
    i=i+1;
    % Save actual, measured and estimated positions
    pos = [pos x(1)];
    posmeas = [posmeas z];
    poshat = [poshat xhat(1)];
end

% Plot the results
%t = 0 : td : max(time);
%plot(t,pos,'r',t,posmeas,'g',t,poshat,'b');
%legend('true position','predicted position', 'updated position');
%xlabel('t [10ms]');
%ylabel('Position [m]');
%grid;

Distance vector calculated RSSI value by using correction function. RSSI has very big noise component.

Last Edited: Wed. Apr 23, 2008 - 02:39 PM
  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

Pls. report what I do wrong!? :)

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

Now its working almost correctly:


function [poshat] = kalman(time,r, v, p)

% time = time vector 
% r = radius vector input
% v = upper limit for process (velocity) noise [m/10msec]
% p = upper limit for position measurement noise [m]
time = [0; time];                       %add a new zero row 
time(2) = 1;                            %inhibit divide by zero
t = 2;                                  %time index
td = abs(time(t) - time(t-1));                                                  
M = [1 0; 0 1];                             % System model  
x = [0 0]';                                 % Initial state 
xhat = x;                                   % Initial state estimate

C = [1 0];                                  % Observation model

W = v * [td^2 td; td 1];                  % Process noise covariance ???
N = p;                                    % Measurement noise covariance 
                                          % N=0 Perfect follow

P = W;                                      % Initial estimate covariance

pos = [];                                   % Position
posmeas = [];                               % Measured position
poshat = [];                                % Estimated position 


[col row] = size (time);

for t = 2 : 1 : col,
    td = (time(t) - time(t-1));
    
   
    x = [r(t-1) (xhat(1) - r(t-1))/td]';   %place and velocity
    
    w = v * [td; 1];                  % Process noise ???
    x = M * x + w;                     % Simulate the system
    
    n = p;                             % Measurement noise
    z = C * x + n;                     % Simulate the measurement
    
    y = z - C * xhat;                              % Innovation of measurement residual  
    S = C * P * C' + N;                            % Innovation covariance
    K = M * P * C' * inv(S);                       % Optimal Kalman gain
    P = M * P * M' + W - M * P * C' * inv(S)* C * P * M';  % Estimate covariance
    
    xhat = M*xhat + K * y;                % State estimate using the Kalman filter
   
    
    % Save actual, measured and estimated positions
    pos = [pos x(1)];
    posmeas = [posmeas z];
    poshat = [poshat xhat(1)];

end
Last Edited: Wed. Apr 23, 2008 - 02:36 PM
  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

Hi,

the mystic offset is for simulation of gyro drift in real world application.

bye

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

I corrected covariance parameters then offset disappeared.