## autopilot-devel

 [Autopilot] kalman filter for lineal position with accelerometer, from modified rotomotion code From: ego - 2010-01-19 13:17:09 Attachments: Message as HTML ```Hello to all, I'm new here, and already need your help!! I'm using a kalman filter to find out the linear position of a moving object (movement limited to a single axis), where I use a ADX202 accelerometer to measure the acceleration, and take some noisy measurements of the position as well. I'm adapting the rotomotion code for a tilt sensor, where tilt is measured with a 2 axis accelerometer and angular rate with a gyro. I guess this code is well known but I post the link here: http://scratchpad.wikia.com/wiki/RotomotionCode In my project the state vector would be: X=[position, velocity, acceleration_bias] And so the state functions: Xdot=[position_dot, velocity_dot, acceleration_bias_dot] position_dot=velocity velocity_dot= acceleration-acceleration_bias_dot acceleration_bias_dot=0 And the covariance matrix (initial state): P=[1 0 0] [0 0 0] [0 0 1] The jacobian of Xdot with respect to the states: A=[d(pos_dot)/d(position) d(pos_dot)/d(vel) d(pos_dot)/d(a_bias)] [d(vel_dot)/d(position) d(vel_dot)/d(vel) d(vel_dot)/d(a_bias)] [d(a_bias_dot/d(pos) d(a_bias_dot/d(vel) d(a_bias_dot/d(a_bias)] A=[0 1 -dt] [0 0 -1] [0 0 0] (is this correct??) Pdot=A*P+P'*A+Q where Q=[Q_pos 0 0] [0 Q_vel 0] [0 0 Q_accel] C matrix is: C=[1 0 0] As I said I measure the position with a noisy sensor and the acceleration so: accel = measured_accel - a_bias vel = vel + accel*dt pos= pos+vel*dt+1/2*accel*dt^2 Inn=pos_meas - p And then I update the elements of the matrix Pdot (Pdot=A*P+P'*A+Q) Then compute the covariance matrix P=P+Pdot*dt E=CPC'+R K=PC'inv(E) Then update the covariance matrix: P=P-KCP And finally update the state estimate X=X+K*Inn The results I get from this filter are quite good, but there is something I am not doing right. When the object is stopped, I get the correct position, velocity is zero, BUT the a_bias is not correct as it gives me a residual acceleration that is not zero. pos=correct (stopped) vel=0 accel=(a_meas-a_bias)<>0 I don't understand exactly how this kalman filter is implemented (and I cannot find any info on this way of implementing the filter, I mean updating the covariace matrix P=P+Pdot*dt). Can you help me? Do you see anything that I do wrong? Thanks for your help ```
 Re: [Autopilot] kalman filter for lineal position with accelerometer, from modified rotomotion code From: Hugo Marien - 2010-01-19 17:26:34 ```Hi, There are only a few readers left here... Here is a bug report that was posted on this forum on 7/22/2008 by Achim Walther. Maybe it can be of some help to you... ------------------------------------------------------------------- I know there's not much going on in this list and that is still understatement, but I want to report a bug in the autopilot code to the few remaining readers. (For amusement or whatever ;-) I own a Rotomotion 6DOF IMU. To use it with a Multiplex EasyStar I had to reduce the size of the main board by applying SMD components. I'm using the IMU only for data gathering and doing the maths with a Gumstix computer. Therefore I had to port the Kalman filtering code. I chose the code from ahrs.c that comes in directory autopilot-2.5/onboard/avr. Which may be a mistake because it now looks like an abandoned version branch to me. However, line 156 in function kalman_update tries to invert matrix E by saying m33_inv( E, T2 ); but does the opposite. The correct function call would be m33_inv( T2, E ); since E is the source and T2 is the target. Having fixed that, my 3D visualisation of the Kalman filter works fine and I hope it will do the same in the airborne software. But - as I said - I'm afraid of having used an old branch... Regards, Achim. ------------------------------------------- Regards and success, Hugo I know there's not much going on in this list and that is still understatement, but I want to report a bug in the autopilot code to the few remaining readers. (For amusement or whatever ;-) I own a Rotomotion 6DOF IMU. To use it with a Multiplex EasyStar I had to reduce the size of the main board by applying SMD components. I'm using the IMU only for data gathering and doing the maths with a Gumstix computer. Therefore I had to port the Kalman filtering code. I chose the code from ahrs.c that comes in directory autopilot-2.5/onboard/avr. Which may be a mistake because it now looks like an abandoned version branch to me. However, line 156 in function kalman_update tries to invert matrix E by saying m33_inv( E, T2 ); but does the opposite. The correct function call would be m33_inv( T2, E ); since E is the source and T2 is the target. Having fixed that, my 3D visualisation of the Kalman filter works fine and I hope it will do the same in the airborne software. But - as I said - I'm afraid of having used an old branch... Regards, Achim. ego wrote: > Hello to all, > I'm new here, and already need your help!! > > I'm using a kalman filter to find out the linear position of a moving > object (movement limited to a single axis), where I use a ADX202 > accelerometer to measure the acceleration, and take some noisy > measurements of the position as well. > > I'm adapting the rotomotion code for a tilt sensor, where tilt is > measured with a 2 axis accelerometer and angular rate with a gyro. I > guess this code is well known but I post the link here: > http://scratchpad.wikia.com/wiki/RotomotionCode > > In my project the state vector would be: > > X=[position, velocity, acceleration_bias] > > And so the state functions: > > Xdot=[position_dot, velocity_dot, acceleration_bias_dot] > > position_dot=velocity > velocity_dot= acceleration-acceleration_bias_dot > acceleration_bias_dot=0 > > And the covariance matrix (initial state): > > P=[1 0 0] > [0 0 0] > [0 0 1] > > The jacobian of Xdot with respect to the states: > > A=[d(pos_dot)/d(position) d(pos_dot)/d(vel) d(pos_dot)/d(a_bias)] > [d(vel_dot)/d(position) d(vel_dot)/d(vel) d(vel_dot)/d(a_bias)] > [d(a_bias_dot/d(pos) d(a_bias_dot/d(vel) d(a_bias_dot/d(a_bias)] > > A=[0 1 -dt] > [0 0 -1] > [0 0 0] (is this correct??) > > Pdot=A*P+P'*A+Q > > where Q=[Q_pos 0 0] > [0 Q_vel 0] > [0 0 Q_accel] > > C matrix is: C=[1 0 0] > > > As I said I measure the position with a noisy sensor and the > acceleration so: > > accel = measured_accel - a_bias > vel = vel + accel*dt > pos= pos+vel*dt+1/2*accel*dt^2 > > Inn=pos_meas - p > > And then I update the elements of the matrix Pdot (Pdot=A*P+P'*A+Q) > > Then compute the covariance matrix P=P+Pdot*dt > > E=CPC'+R > > K=PC'inv(E) > > Then update the covariance matrix: P=P-KCP > > And finally update the state estimate X=X+K*Inn > > > > The results I get from this filter are quite good, but there is > something I am not doing right. When the object is stopped, I get the > correct position, velocity is zero, BUT the a_bias is not correct as > it gives me a residual acceleration that is not zero. > pos=correct (stopped) > vel=0 > accel=(a_meas-a_bias)<>0 > > I don't understand exactly how this kalman filter is implemented (and > I cannot find any info on this way of implementing the filter, I mean > updating the covariace matrix P=P+Pdot*dt). > > Can you help me? Do you see anything that I do wrong? > > Thanks for your help > ------------------------------------------------------------------------ > > ------------------------------------------------------------------------------ > Throughout its 18-year history, RSA Conference consistently attracts the > world's best and brightest in the field, creating opportunities for Conference > attendees to learn about information security's most important issues through > interactions with peers, luminaries and emerging and established companies. > http://p.sf.net/sfu/rsaconf-dev2dev > ------------------------------------------------------------------------ > > _______________________________________________ > http://autopilot.sourceforge.net/ Development mailing list > https://lists.sourceforge.net/lists/listinfo/autopilot-devel > ```
 Re: [Autopilot] kalman filter for lineal position with accelerometer, from modified rotomotion code From: Srinath M - 2010-01-28 18:46:49 Attachments: Message as HTML ```Hi Ego Sorry to reply late, i am no expert on actual implementation, and am trying to do the same thing as you. I am not sure if your A matrix is correct Shouldn't the continous system, just be represented as xdot = Acontx +Bcontu + w I assume like you that the state vector is [true_pos true_velocity accelrometer_bias]. Let us regard accelrometer reading as an input u to the system. For this state vector Acont would be [0 1 0; 0 0 -1; 0 0 0],Bcont would be [0;1;0] and w would be white noise So for the discrete system, shouldn't A just be Adisc=[1 dt 0;0 1 -dt; 0 0 1]. and this doesn't match with your A Pls correct me if i am wrong. I hv learnt KF's from this site. http://www.cs.unc.edu/~welch/kalman/kalmanIntro.html Regards Srinath On Tue, Jan 19, 2010 at 5:45 , ego wrote: > Hello to all, > I'm new here, and already need your help!! > > I'm using a kalman filter to find out the linear position of a moving > object (movement limited to a single axis), where I use a ADX202 > accelerometer to measure the acceleration, and take some noisy measurements > of the position as well. > > I'm adapting the rotomotion code for a tilt sensor, where tilt is measured > with a 2 axis accelerometer and angular rate with a gyro. I guess this code > is well known but I post the link here: > http://scratchpad.wikia.com/wiki/RotomotionCode > > In my project the state vector would be: > > X=[position, velocity, acceleration_bias] > > And so the state functions: > > Xdot=[position_dot, velocity_dot, acceleration_bias_dot] > > position_dot=velocity > velocity_dot= acceleration-acceleration_bias_dot > acceleration_bias_dot=0 > > And the covariance matrix (initial state): > > P=[1 0 0] > [0 0 0] > [0 0 1] > > The jacobian of Xdot with respect to the states: > > A=[d(pos_dot)/d(position) d(pos_dot)/d(vel) d(pos_dot)/d(a_bias)] > [d(vel_dot)/d(position) d(vel_dot)/d(vel) d(vel_dot)/d(a_bias)] > [d(a_bias_dot/d(pos) d(a_bias_dot/d(vel) d(a_bias_dot/d(a_bias)] > > A=[0 1 -dt] > [0 0 -1] > [0 0 0] (is this correct??) > > Pdot=A*P+P'*A+Q > > where Q=[Q_pos 0 0] > [0 Q_vel 0] > [0 0 Q_accel] > > C matrix is: C=[1 0 0] > > > As I said I measure the position with a noisy sensor and the acceleration > so: > > accel = measured_accel - a_bias > vel = vel + accel*dt > pos= pos+vel*dt+1/2*accel*dt^2 > > Inn=pos_meas - p > > And then I update the elements of the matrix Pdot (Pdot=A*P+P'*A+Q) > > Then compute the covariance matrix P=P+Pdot*dt > > E=CPC'+R > > K=PC'inv(E) > > Then update the covariance matrix: P=P-KCP > > And finally update the state estimate X=X+K*Inn > > > > The results I get from this filter are quite good, but there is something I > am not doing right. When the object is stopped, I get the correct position, > velocity is zero, BUT the a_bias is not correct as it gives me a residual > acceleration that is not zero. > pos=correct (stopped) > vel=0 > accel=(a_meas-a_bias)<>0 > > I don't understand exactly how this kalman filter is implemented (and I > cannot find any info on this way of implementing the filter, I mean updating > the covariace matrix P=P+Pdot*dt). > > Can you help me? Do you see anything that I do wrong? > > Thanks for your help > > > ------------------------------------------------------------------------------ > Throughout its 18-year history, RSA Conference consistently attracts the > world's best and brightest in the field, creating opportunities for > Conference > attendees to learn about information security's most important issues > through > interactions with peers, luminaries and emerging and established companies. > http://p.sf.net/sfu/rsaconf-dev2dev > _______________________________________________ > http://autopilot.sourceforge.net/ Development mailing list > https://lists.sourceforge.net/lists/listinfo/autopilot-devel > > ```
 Re: [Autopilot] kalman filter for lineal position with accelerometer, from modified rotomotion code From: ego - 2010-02-09 10:15:18 Attachments: Message as HTML ```Hi Srinath, I've been busy, sorry for my late reply. I don't see how you get your Adisc matrix=[1 dt 0;0 1 -dt;0 0 1] In the original rotomotion code, xdot is the derivative of the state vector X over time. My state vector matrix X=[true_pos, true_velocity, accel_bias] xdot=A*X + B*u xdot=[pos_dot,vel_dot,a_bias_dot]=[vel,accel,0] vel(update)=vel+a*dt=vel+(a_measure-a_bias)*dt accel(update)=a_measure-a_bias So, my A matrix should be [0 1 -dt;0 0 -1;0 0 0] and B=[dt 1 0]; But then, I don't see how this filter is implemented, nor the theory that supports it. This kalman filter is different from what I have seen in the papers I read. The result of the corrected position is fairly good, but the corrected accelerometer error is unacceptable. It just doesn't track and correct the accelerometer error, which is my final goal. Let me know how it goes to you. This is the only code I have found to do what I'm trying to do. Best regards, Ego 2010/1/28 Srinath M > Hi Ego > > Sorry to reply late, i am no expert on actual implementation, and am trying > to do the same thing as you. > I am not sure if your A matrix is correct > Shouldn't the continous system, just be represented as > xdot = Acontx +Bcontu + w > I assume like you that the state vector is [true_pos true_velocity > accelrometer_bias]. > Let us regard accelrometer reading as an input u to the system. > For this state vector Acont would be [0 1 0; 0 0 -1; 0 0 0],Bcont would be > [0;1;0] and w would be white noise > So for the discrete system, shouldn't A just be > Adisc=[1 dt 0;0 1 -dt; 0 0 1]. > and this doesn't match with your A > > Pls correct me if i am wrong. > I hv learnt KF's from this site. > http://www.cs.unc.edu/~welch/kalman/kalmanIntro.html > > Regards > Srinath > > > On Tue, Jan 19, 2010 at 5:45 , ego wrote: > >> Hello to all, >> I'm new here, and already need your help!! >> >> I'm using a kalman filter to find out the linear position of a moving >> object (movement limited to a single axis), where I use a ADX202 >> accelerometer to measure the acceleration, and take some noisy measurements >> of the position as well. >> >> I'm adapting the rotomotion code for a tilt sensor, where tilt is measured >> with a 2 axis accelerometer and angular rate with a gyro. I guess this code >> is well known but I post the link here: >> http://scratchpad.wikia.com/wiki/RotomotionCode >> >> In my project the state vector would be: >> >> X=[position, velocity, acceleration_bias] >> >> And so the state functions: >> >> Xdot=[position_dot, velocity_dot, acceleration_bias_dot] >> >> position_dot=velocity >> velocity_dot= acceleration-acceleration_bias_dot >> acceleration_bias_dot=0 >> >> And the covariance matrix (initial state): >> >> P=[1 0 0] >> [0 0 0] >> [0 0 1] >> >> The jacobian of Xdot with respect to the states: >> >> A=[d(pos_dot)/d(position) d(pos_dot)/d(vel) d(pos_dot)/d(a_bias)] >> [d(vel_dot)/d(position) d(vel_dot)/d(vel) d(vel_dot)/d(a_bias)] >> [d(a_bias_dot/d(pos) d(a_bias_dot/d(vel) d(a_bias_dot/d(a_bias)] >> >> A=[0 1 -dt] >> [0 0 -1] >> [0 0 0] (is this correct??) >> >> Pdot=A*P+P'*A+Q >> >> where Q=[Q_pos 0 0] >> [0 Q_vel 0] >> [0 0 Q_accel] >> >> C matrix is: C=[1 0 0] >> >> >> As I said I measure the position with a noisy sensor and the acceleration >> so: >> >> accel = measured_accel - a_bias >> vel = vel + accel*dt >> pos= pos+vel*dt+1/2*accel*dt^2 >> >> Inn=pos_meas - p >> >> And then I update the elements of the matrix Pdot (Pdot=A*P+P'*A+Q) >> >> Then compute the covariance matrix P=P+Pdot*dt >> >> E=CPC'+R >> >> K=PC'inv(E) >> >> Then update the covariance matrix: P=P-KCP >> >> And finally update the state estimate X=X+K*Inn >> >> >> >> The results I get from this filter are quite good, but there is something >> I am not doing right. When the object is stopped, I get the correct >> position, velocity is zero, BUT the a_bias is not correct as it gives me a >> residual acceleration that is not zero. >> pos=correct (stopped) >> vel=0 >> accel=(a_meas-a_bias)<>0 >> >> I don't understand exactly how this kalman filter is implemented (and I >> cannot find any info on this way of implementing the filter, I mean updating >> the covariace matrix P=P+Pdot*dt). >> >> Can you help me? Do you see anything that I do wrong? >> >> Thanks for your help >> >> >> ------------------------------------------------------------------------------ >> Throughout its 18-year history, RSA Conference consistently attracts the >> world's best and brightest in the field, creating opportunities for >> Conference >> attendees to learn about information security's most important issues >> through >> interactions with peers, luminaries and emerging and established >> companies. >> http://p.sf.net/sfu/rsaconf-dev2dev >> _______________________________________________ >> http://autopilot.sourceforge.net/ Development mailing list >> https://lists.sourceforge.net/lists/listinfo/autopilot-devel >> >> > > > ------------------------------------------------------------------------------ > The Planet: dedicated and managed hosting, cloud storage, colocation > Stay online with enterprise data centers and the best network in the > business > Choose flexible plans and management services without long-term contracts > Personal 24x7 support from experience hosting pros just a phone call away. > http://p.sf.net/sfu/theplanet-com > _______________________________________________ > http://autopilot.sourceforge.net/ Development mailing list > https://lists.sourceforge.net/lists/listinfo/autopilot-devel > > ```