Kalman Filter and INS

Here is what i have, a measurement of a distance from an external sensor at
a lower sampling rate and acceleration measurements from accelerometer (
IMU at a higher rate). How can i use a kalman filter to fuse these to
produce a good estimate of the position travelled. What would be A, B, H,
R, Q if i were to use a kalman filter. Any help will be appreciated.

On Mon, 13 Apr 2009 14:04:18 -0500, arvkr wrote:

> Here is what i have, a measurement of a distance from an external sensor
> at a lower sampling rate and acceleration measurements from
> accelerometer ( IMU at a higher rate). How can i use a kalman filter to
> fuse these to produce a good estimate of the position travelled. What
> would be A, B, H, R, Q if i were to use a kalman filter. Any help will
> be appreciated.

First, why don't you model the system?  Model it as if you weren't using 
the Kalman, but as if you cared about the quantity that the Kalman is 
measuring.  This means you should include the outputs that you really 
care about (or just note them if they're states), and include an output 
of the predicted value of the measurement that you're going to take.

-- 
http://www.wescottdesign.com

>
>First, why don't you model the system?  Model it as if you weren't using

>the Kalman, but as if you cared about the quantity that the Kalman is 
>measuring.  This means you should include the outputs that you really 
>care about (or just note them if they're states), and include an output 
>of the predicted value of the measurement that you're going to take.
>
>-- 
>http://www.wescottdesign.com
>

My states would be 
x(k) = [pos
     vel
     acc];
state equations would be
x(k) = A*x(k-1) + B * u(k) + w(k)

Measurement equations
z(k) = H*x(k) + v(k) ( process noise)

now if we assume the model to be
pos(k) = pos(k-1) + deltaT*vel(k-1) + deltaT^2/2*acc(k-1);
vel(k) = vel(k-1)+ deltaT*acc(k-1)
acc(k) = acc(k)

then A would be
A = [1	Δt	Δt^2/2
     0	1	Δt
     0	0	1   ]

would my H be
H = [ 1 0 0
      0 0 1]
and my Z be
Z = [ distMeasured 0
      0            accMeasured]

R = [var(distMeasured) 0
     0                var(accMeasured)]

Q = ?
and what would be my initial covariance as initializing values seem
crucial.

arvkr wrote:
>> First, why don't you model the system?  Model it as if you weren't using
> 
>> the Kalman, but as if you cared about the quantity that the Kalman is 
>> measuring.  This means you should include the outputs that you really 
>> care about (or just note them if they're states), and include an output 
>> of the predicted value of the measurement that you're going to take.
>>
>> -- 
>> http://www.wescottdesign.com
>>
> 
> My states would be 
> x(k) = [pos
>      vel
>      acc];
> state equations would be
> x(k) = A*x(k-1) + B * u(k) + w(k)
> 
> Measurement equations
> z(k) = H*x(k) + v(k) ( process noise)
> 
> now if we assume the model to be
> pos(k) = pos(k-1) + deltaT*vel(k-1) + deltaT^2/2*acc(k-1);
> vel(k) = vel(k-1)+ deltaT*acc(k-1)
> acc(k) = acc(k)
> 
> then A would be
> A = [1	Δt	Δt^2/2
>      0	1	Δt
>      0	0	1   ]
> 
> would my H be
> H = [ 1 0 0
>       0 0 1]
> and my Z be
> Z = [ distMeasured 0
>       0            accMeasured]
> 
> R = [var(distMeasured) 0
>      0                var(accMeasured)]
> 
> Q = ?
> and what would be my initial covariance as initializing values seem
> crucial.
> 
Two questions:

One: Why is acceleration a state?  I thought you were measuring it.

Two: If you _are_ measuring acceleration, how do you know what direction 
it's coming from?  IOW with an accelerometer you can measure 
acceleration with respect to your vehicle frame of motion, but doesn't 
that rotate with respect to the local inertial frame?

-- 

Tim Wescott
Wescott Design Services
http://www.wescottdesign.com

Do you need to implement control loops in software?
"Applied Control Theory for Embedded Systems" was written for you.
See details at http://www.wescottdesign.com/actfes/actfes.html

>Two questions:
>
>One: Why is acceleration a state?  I thought you were measuring it.

You are right. I am measuring it and prob should have the state as acc
bias to estimate the bias. This is where i am struggling with since i don't
seem to understand kalman filter fully and i have read several articles on
the web, still haven't been able to fully get a grip on it.

arvkr wrote:
>> Two questions:
>>
>> One: Why is acceleration a state?  I thought you were measuring it.
> 
> You are right. I am measuring it and prob should have the state as acc
> bias to estimate the bias. This is where i am struggling with since i don't
> seem to understand kalman filter fully and i have read several articles on
> the web, still haven't been able to fully get a grip on it.

I have one book that talks about nothing but optimal state estimation 
(i.e. the Kalman filter and it's extended family), another that goes 
into detection, estimation and modulation theory (which has you derive 
Kalman filters as homework problems in at least two places), a couple of 
adaptive control books (that treat the system identification phase of 
adaptive control as a Kalman filtering problem), a book on strapdown 
inertial navigation (which mostly talks about devices, but which has an 
excellent section on translating combined gyro and accelerometer data 
into just the sort of model you need), and access to the entire world 
wide web.

This is backed up by a career doing various sorts of signal detection 
and estimation problems, building advanced control systems from scratch, 
and other related activity.

After all this, while I feel that I'm more than good enough to charge 
people money to design Kalman filtering algorithms I still don't feel I 
have wrapped my arms completely around the problem.  I doubt that I 
would even if I won the lottery tomorrow, got my PhD in optimal 
estimation, and spent the rest of my life teaching and doing research.

So I don't know if it's realistic for anyone to expect to figure this 
out by asking questions on USENET and reading web pages.  I'm not even 
sure if I can recommend just one book -- I think to do what you want to 
do you need at least Dan Simon's book and a good book on strapdown 
inertial nav systems.

Even then you'll find Simon's book to be an uphill battle unless you 
have a good grounding in stochastic process theory and state-space 
control -- so expect to do some supplementary reading.  Further, I have 
yet to find a book on strapdown inertial navigation theory that gives a 
blow-by-blow analysis of an inertial navigation Kalman solution -- the 
one I have is the best of the lot, but it sure doesn't give an ABC 
description.

Here's my bibliography -- the two end ones are currently on my 
'essential reading' list for inertial nav stuff.

Dan Simon, "Optimal State Estimation", Wiley 2006: 
http://www.powells.com/partner/30696/biblio/9780471708582

Harry Van Trees, "Detection, Estimation, and Modulation Theory", Wiley, 
1968: http://www.powells.com/partner/30696/biblio/9780471463825

Karl Astrom & Bjorn Wittenmark, "Adaptive Control", Addison-Wesley, 
1995: http://www.powells.com/partner/30696/biblio/9780486462783

Bitmead, Gevers, Wertz, "Adaptive Optimal Control, The Thinking Man's 
GPC", Prentice-Hall, 1990: 
http://www.powells.com/partner/30696/biblio/0139468234

Titterton, D. H. and Weston, J. L., "Strapdown Inertial Navigation 
Technology", AIAA, 2004: 
http://www.powells.com/partner/30696/biblio/9780863413582

-- 

Tim Wescott
Wescott Design Services
http://www.wescottdesign.com

Do you need to implement control loops in software?
"Applied Control Theory for Embedded Systems" was written for you.
See details at http://www.wescottdesign.com/actfes/actfes.html

>arvkr wrote:
>>> Two questions:
>>>
>>> One: Why is acceleration a state?  I thought you were measuring it.
>> 
>> You are right. I am measuring it and prob should have the state as acc
>> bias to estimate the bias. This is where i am struggling with since i
don't
>> seem to understand kalman filter fully and i have read several articles
on
>> the web, still haven't been able to fully get a grip on it.
>
>I have one book that talks about nothing but optimal state estimation 
>(i.e. the Kalman filter and it's extended family), another that goes 
>into detection, estimation and modulation theory (which has you derive 
>Kalman filters as homework problems in at least two places), a couple of

>adaptive control books (that treat the system identification phase of 
>adaptive control as a Kalman filtering problem), a book on strapdown 
>inertial navigation (which mostly talks about devices, but which has an 
>excellent section on translating combined gyro and accelerometer data 
>into just the sort of model you need), and access to the entire world 
>wide web.
>
>This is backed up by a career doing various sorts of signal detection 
>and estimation problems, building advanced control systems from scratch,

>and other related activity.
>
>After all this, while I feel that I'm more than good enough to charge 
>people money to design Kalman filtering algorithms I still don't feel I 
>have wrapped my arms completely around the problem.  I doubt that I 
>would even if I won the lottery tomorrow, got my PhD in optimal 
>estimation, and spent the rest of my life teaching and doing research.
>
>So I don't know if it's realistic for anyone to expect to figure this 
>out by asking questions on USENET and reading web pages.  I'm not even 
>sure if I can recommend just one book -- I think to do what you want to 
>do you need at least Dan Simon's book and a good book on strapdown 
>inertial nav systems.
>
>Even then you'll find Simon's book to be an uphill battle unless you 
>have a good grounding in stochastic process theory and state-space 
>control -- so expect to do some supplementary reading.  Further, I have 
>yet to find a book on strapdown inertial navigation theory that gives a 
>blow-by-blow analysis of an inertial navigation Kalman solution -- the 
>one I have is the best of the lot, but it sure doesn't give an ABC 
>description.
>
>Here's my bibliography -- the two end ones are currently on my 
>'essential reading' list for inertial nav stuff.
>
>Dan Simon, "Optimal State Estimation", Wiley 2006: 
>http://www.powells.com/partner/30696/biblio/9780471708582
>
>Harry Van Trees, "Detection, Estimation, and Modulation Theory", Wiley, 
>1968: http://www.powells.com/partner/30696/biblio/9780471463825
>
>Karl Astrom & Bjorn Wittenmark, "Adaptive Control", Addison-Wesley, 
>1995: http://www.powells.com/partner/30696/biblio/9780486462783
>
>Bitmead, Gevers, Wertz, "Adaptive Optimal Control, The Thinking Man's 
>GPC", Prentice-Hall, 1990: 
>http://www.powells.com/partner/30696/biblio/0139468234
>
>Titterton, D. H. and Weston, J. L., "Strapdown Inertial Navigation 
>Technology", AIAA, 2004: 
>http://www.powells.com/partner/30696/biblio/9780863413582
>
>-- 
>
>Tim Wescott
>Wescott Design Services
>http://www.wescottdesign.com
>
>Do you need to implement control loops in software?
>"Applied Control Theory for Embedded Systems" was written for you.
>See details at http://www.wescottdesign.com/actfes/actfes.html
>

Thanks a lot for the list of books, i will definitely take a look at
them.
As you said, web searches etc help only to a certain extent, i came across
this book on the google books

Estimation with applications to tracking and navigation 
http://books.google.ca/books?id=xz9nQ4wdXG4C&pg=PA219&lpg=PA219&dq=kalman+filter+step+by+step&source=bl&ots=VnU1u3e6uZ&sig=WTZ3IS1DDcNIPAbvZBiBgnuU1is&hl=en&ei=FZDkScu3DZ_ulQf1koHgDg&sa=X&oi=book_result&ct=result&resnum=8#PPA253,M1

have you read this one before and if so how did u find it?

arvkr wrote:
>> arvkr wrote:
>>>> Two questions:
>>>>
>>>> One: Why is acceleration a state?  I thought you were measuring it.
>>> You are right. I am measuring it and prob should have the state as acc
>>> bias to estimate the bias. This is where i am struggling with since i
> don't
>>> seem to understand kalman filter fully and i have read several articles
> on
>>> the web, still haven't been able to fully get a grip on it.
>> I have one book that talks about nothing but optimal state estimation 
>> (i.e. the Kalman filter and it's extended family), another that goes 
>> into detection, estimation and modulation theory (which has you derive 
>> Kalman filters as homework problems in at least two places), a couple of
> 
>> adaptive control books (that treat the system identification phase of 
>> adaptive control as a Kalman filtering problem), a book on strapdown 
>> inertial navigation (which mostly talks about devices, but which has an 
>> excellent section on translating combined gyro and accelerometer data 
>> into just the sort of model you need), and access to the entire world 
>> wide web.
>>
>> This is backed up by a career doing various sorts of signal detection 
>> and estimation problems, building advanced control systems from scratch,
> 
>> and other related activity.
>>
>> After all this, while I feel that I'm more than good enough to charge 
>> people money to design Kalman filtering algorithms I still don't feel I 
>> have wrapped my arms completely around the problem.  I doubt that I 
>> would even if I won the lottery tomorrow, got my PhD in optimal 
>> estimation, and spent the rest of my life teaching and doing research.
>>
>> So I don't know if it's realistic for anyone to expect to figure this 
>> out by asking questions on USENET and reading web pages.  I'm not even 
>> sure if I can recommend just one book -- I think to do what you want to 
>> do you need at least Dan Simon's book and a good book on strapdown 
>> inertial nav systems.
>>
>> Even then you'll find Simon's book to be an uphill battle unless you 
>> have a good grounding in stochastic process theory and state-space 
>> control -- so expect to do some supplementary reading.  Further, I have 
>> yet to find a book on strapdown inertial navigation theory that gives a 
>> blow-by-blow analysis of an inertial navigation Kalman solution -- the 
>> one I have is the best of the lot, but it sure doesn't give an ABC 
>> description.
>>
>> Here's my bibliography -- the two end ones are currently on my 
>> 'essential reading' list for inertial nav stuff.
>>
>> Dan Simon, "Optimal State Estimation", Wiley 2006: 
>> http://www.powells.com/partner/30696/biblio/9780471708582
>>
>> Harry Van Trees, "Detection, Estimation, and Modulation Theory", Wiley, 
>> 1968: http://www.powells.com/partner/30696/biblio/9780471463825
>>
>> Karl Astrom & Bjorn Wittenmark, "Adaptive Control", Addison-Wesley, 
>> 1995: http://www.powells.com/partner/30696/biblio/9780486462783
>>
>> Bitmead, Gevers, Wertz, "Adaptive Optimal Control, The Thinking Man's 
>> GPC", Prentice-Hall, 1990: 
>> http://www.powells.com/partner/30696/biblio/0139468234
>>
>> Titterton, D. H. and Weston, J. L., "Strapdown Inertial Navigation 
>> Technology", AIAA, 2004: 
>> http://www.powells.com/partner/30696/biblio/9780863413582
>>
>> -- 
>>
>> Tim Wescott
>> Wescott Design Services
>> http://www.wescottdesign.com
>>
>> Do you need to implement control loops in software?
>> "Applied Control Theory for Embedded Systems" was written for you.
>> See details at http://www.wescottdesign.com/actfes/actfes.html
>>
> 
> Thanks a lot for the list of books, i will definitely take a look at
> them.
> As you said, web searches etc help only to a certain extent, i came across
> this book on the google books
> 
> Estimation with applications to tracking and navigation 
> http://books.google.ca/books?id=xz9nQ4wdXG4C&pg=PA219&lpg=PA219&dq=kalman+filter+step+by+step&source=bl&ots=VnU1u3e6uZ&sig=WTZ3IS1DDcNIPAbvZBiBgnuU1is&hl=en&ei=FZDkScu3DZ_ulQf1koHgDg&sa=X&oi=book_result&ct=result&resnum=8#PPA253,M1
> 
> have you read this one before and if so how did u find it?

I haven't seen it before.  I like the table of contents, though.  If the 
writing is clear it may be a good one to get in lieu of the Simon book 
(although I _really_ like Simon's clarity, and his attitude about how 
far so-called 'optimal' methods will take you).

I didn't see anything in the table of contents that indicates that they 
directly address your problem, but it's extensive, and I only skimmed 
it.  The "Application to Navigation" part should certainly be what 
you're looking for.

-- 

Tim Wescott
Wescott Design Services
http://www.wescottdesign.com

Do you need to implement control loops in software?
"Applied Control Theory for Embedded Systems" was written for you.
See details at http://www.wescottdesign.com/actfes/actfes.html

Tim Wescott schrieb:
> arvkr wrote:
>>> arvkr wrote:
>>>>> Two questions:
>>>>>
>>>>> One: Why is acceleration a state?  I thought you were measuring it.
>>>> You are right. I am measuring it and prob should have the state as acc
>>>> bias to estimate the bias. This is where i am struggling with since i
>> don't
>>>> seem to understand kalman filter fully and i have read several articles
>> on
>>>> the web, still haven't been able to fully get a grip on it.
>>> I have one book that talks about nothing but optimal state estimation 
>>> (i.e. the Kalman filter and it's extended family), another that goes 
>>> into detection, estimation and modulation theory (which has you 
>>> derive Kalman filters as homework problems in at least two places), a 
>>> couple of
>>
>>> adaptive control books (that treat the system identification phase of 
>>> adaptive control as a Kalman filtering problem), a book on strapdown 
>>> inertial navigation (which mostly talks about devices, but which has 
>>> an excellent section on translating combined gyro and accelerometer 
>>> data into just the sort of model you need), and access to the entire 
>>> world wide web.
>>>
>>> This is backed up by a career doing various sorts of signal detection 
>>> and estimation problems, building advanced control systems from scratch,
>>
>>> and other related activity.
>>>
>>> After all this, while I feel that I'm more than good enough to charge 
>>> people money to design Kalman filtering algorithms I still don't feel 
>>> I have wrapped my arms completely around the problem.  I doubt that I 
>>> would even if I won the lottery tomorrow, got my PhD in optimal 
>>> estimation, and spent the rest of my life teaching and doing research.
>>>
>>> So I don't know if it's realistic for anyone to expect to figure this 
>>> out by asking questions on USENET and reading web pages.  I'm not 
>>> even sure if I can recommend just one book -- I think to do what you 
>>> want to do you need at least Dan Simon's book and a good book on 
>>> strapdown inertial nav systems.
>>>
>>> Even then you'll find Simon's book to be an uphill battle unless you 
>>> have a good grounding in stochastic process theory and state-space 
>>> control -- so expect to do some supplementary reading.  Further, I 
>>> have yet to find a book on strapdown inertial navigation theory that 
>>> gives a blow-by-blow analysis of an inertial navigation Kalman 
>>> solution -- the one I have is the best of the lot, but it sure 
>>> doesn't give an ABC description.
>>>
>>> Here's my bibliography -- the two end ones are currently on my 
>>> 'essential reading' list for inertial nav stuff.
>>>
>>> Dan Simon, "Optimal State Estimation", Wiley 2006: 
>>> http://www.powells.com/partner/30696/biblio/9780471708582
>>>
>>> Harry Van Trees, "Detection, Estimation, and Modulation Theory", 
>>> Wiley, 1968: http://www.powells.com/partner/30696/biblio/9780471463825
>>>
>>> Karl Astrom & Bjorn Wittenmark, "Adaptive Control", Addison-Wesley, 
>>> 1995: http://www.powells.com/partner/30696/biblio/9780486462783
>>>
>>> Bitmead, Gevers, Wertz, "Adaptive Optimal Control, The Thinking Man's 
>>> GPC", Prentice-Hall, 1990: 
>>> http://www.powells.com/partner/30696/biblio/0139468234
>>>
>>> Titterton, D. H. and Weston, J. L., "Strapdown Inertial Navigation 
>>> Technology", AIAA, 2004: 
>>> http://www.powells.com/partner/30696/biblio/9780863413582
>>>
>>> -- 
>>>
>>> Tim Wescott
>>> Wescott Design Services
>>> http://www.wescottdesign.com
>>>
>>> Do you need to implement control loops in software?
>>> "Applied Control Theory for Embedded Systems" was written for you.
>>> See details at http://www.wescottdesign.com/actfes/actfes.html
>>>
>>
>> Thanks a lot for the list of books, i will definitely take a look at
>> them.
>> As you said, web searches etc help only to a certain extent, i came 
>> across
>> this book on the google books
>>
>> Estimation with applications to tracking and navigation 
>> http://books.google.ca/books?id=xz9nQ4wdXG4C&pg=PA219&lpg=PA219&dq=kalman+filter+step+by+step&source=bl&ots=VnU1u3e6uZ&sig=WTZ3IS1DDcNIPAbvZBiBgnuU1is&hl=en&ei=FZDkScu3DZ_ulQf1koHgDg&sa=X&oi=book_result&ct=result&resnum=8#PPA253,M1 
>>
>>
>> have you read this one before and if so how did u find it?
> 
> I haven't seen it before.  I like the table of contents, though.  If the 
> writing is clear it may be a good one to get in lieu of the Simon book 
> (although I _really_ like Simon's clarity, and his attitude about how 
> far so-called 'optimal' methods will take you).
> 
> I didn't see anything in the table of contents that indicates that they 
> directly address your problem, but it's extensive, and I only skimmed 
> it.  The "Application to Navigation" part should certainly be what 
> you're looking for.
> 

The books of Bar-Shalom are a good choice if you are dealing with 
tracking problems. I have worked with this one:
http://www.amazon.de/Estimation-Applications-Tracking-Navigation-Algorithms/dp/047141655X/ref=sr_1_1?ie=UTF8&s=books-intl-de&qid=1239735289&sr=8-1

A good overview of tracking problems in practice is this one:
http://www.amazon.de/Design-Analysis-Tracking-Systems-Library/dp/1580530060/ref=sr_1_1?ie=UTF8&s=books-intl-de&qid=1239735366&sr=1-1

I have used both for my diploma thesis and they are easy to understand 
if you have a solid background in statistics and signal processing. But 
I have not designed a Kalman Filter from sketch but I only did some 
preprocessing and parametrization so my knowledge is quite limited.
But Tim is right you won't find everything you need in one book a good 
book about optimal estimation in general is good to have a look at to 
understand the whole theory.

"arvkr" <krarvind@gmail.com> writes:
> [...]

Here's an old, but accessible text on the subject:

@book{brown,
  title = "Introduction to Random Signal Analysis and Kalman Filtering",
  author = "{Robert~Grover~Brown}",
  publisher = "John Wiley and Sons",
  year = "1983"}

-- 
%  Randy Yates                  % "She's sweet on Wagner-I think she'd die for Beethoven.
%% Fuquay-Varina, NC            %  She love the way Puccini lays down a tune, and
%%% 919-577-9882                %  Verdi's always creepin' from her room." 
%%%% <yates@ieee.org>           % "Rockaria", *A New World Record*, ELO   
http://www.digitalsignallabs.com