function [muo_avg,ho_avg,mst_avg,vst_avg]= ... upd_mssg_belief( mstold, vstold, mstn, ... vstn, muo_old, ho_old, lr,fmvst) if nargin < 7, lr = 1; end if nargin < 8, fmvst = 1; end ivst = inv(vstn); ivstold = inv(vstold); ho_new = ivst - ivstold; ivstmst = ivstold*mstold; muo_new = ivst*mstn - ivstmst; % average the message based on step size lr ho_avg = lr*(ho_new - ho_old) + ho_old; muo_avg = lr*(muo_new - muo_old) + muo_old; if fmvst == 0 vst_avg = []; mst_avg = []; else % Update the belief if lr ~= 1 vst_avg = inv( ivstold + ho_avg ); mst_avg = vst_avg *( ivstmst + muo_avg ); else vst_avg = vstn; mst_avg = mstn; end end % if isposdef(vst_avg) == 0 if min(eig(vst_avg)) <= 0 wstr = ['vstnew:not positive definite after obs. message update']; warning(wstr); end